From 749b2c7ddb9a199c7841b0734f92b09aea2aa98a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 12 Oct 2026 18:59:23 +0200 Subject: [PATCH 1/4] Add ouster_pcap_to_laz: Ouster pcap + metadata JSON -> mandeye session rosbags/OusterPcapReader parses the Ouster metadata JSON (FW 2.4+ layout) and decodes lidar and IMU UDP packets from a pcap. ouster_pcap_to_laz writes them as a mandeye session through MandeyeSessionWriter: points in the sensor frame with column timestamps and reflectivity as intensity, IMU rotated into the same frame. --stationary-imu writes a synthetic IMU stream for captures without IMU packets. MandeyeSessionWriter gains with_rgb, writing LAS point format 3, so RGB profiles keep their colour (--rgb auto|raw|off). Co-Authored-By: Claude Opus 5.5 --- apps/console_tools/CMakeLists.txt | 23 + apps/console_tools/ouster_pcap_to_laz.cpp | 637 +++++++++++++++++ deploy_mandeye.bat | 2 +- rosbags/MandeyeSessionWriter.cpp | 28 +- rosbags/MandeyeSessionWriter.h | 15 +- rosbags/OusterPcapReader.cpp | 800 ++++++++++++++++++++++ rosbags/OusterPcapReader.h | 140 ++++ rosbags/tests/CMakeLists.txt | 6 +- rosbags/tests/test_mcap_reader.cpp | 53 +- rosbags/tests/test_ouster_pcap_reader.cpp | 416 +++++++++++ 10 files changed, 2111 insertions(+), 9 deletions(-) create mode 100644 apps/console_tools/ouster_pcap_to_laz.cpp create mode 100644 rosbags/OusterPcapReader.cpp create mode 100644 rosbags/OusterPcapReader.h create mode 100644 rosbags/tests/test_ouster_pcap_reader.cpp diff --git a/apps/console_tools/CMakeLists.txt b/apps/console_tools/CMakeLists.txt index c6fe6c21..af51b84d 100644 --- a/apps/console_tools/CMakeLists.txt +++ b/apps/console_tools/CMakeLists.txt @@ -115,6 +115,28 @@ if (MSVC) target_compile_options(mcap_to_laz PRIVATE /bigobj) endif() +# ouster_pcap_to_laz: Ouster pcap + metadata JSON -> mandeye session directory. +# Same writer as mcap_to_laz; the pcap/packet decoding is rosbags/OusterPcapReader, +# so laszip + nlohmann json + spdlog is the whole dependency set. +add_executable( + ouster_pcap_to_laz ouster_pcap_to_laz.cpp + ${REPOSITORY_DIRECTORY}/rosbags/OusterPcapReader.h ${REPOSITORY_DIRECTORY}/rosbags/OusterPcapReader.cpp + ${REPOSITORY_DIRECTORY}/rosbags/MandeyeSessionWriter.h ${REPOSITORY_DIRECTORY}/rosbags/MandeyeSessionWriter.cpp +) + +target_include_directories( + ouster_pcap_to_laz + PRIVATE ${REPOSITORY_DIRECTORY}/rosbags + ${THIRDPARTY_DIRECTORY}/json/include + ${LASZIP_INCLUDE_DIR}/LASzip/include) + +target_link_libraries( + ouster_pcap_to_laz + PRIVATE + spdlog::spdlog + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS}) + if (MSVC) target_compile_options(laz_to_mcap PRIVATE /bigobj) endif() @@ -179,4 +201,5 @@ hdmapping_install_app( laz_to_txt laz_to_mcap mcap_to_laz + ouster_pcap_to_laz session_to_mcap) diff --git a/apps/console_tools/ouster_pcap_to_laz.cpp b/apps/console_tools/ouster_pcap_to_laz.cpp new file mode 100644 index 00000000..d1c5604c --- /dev/null +++ b/apps/console_tools/ouster_pcap_to_laz.cpp @@ -0,0 +1,637 @@ +// Ouster pcap (+ its metadata JSON) -> mandeye session directory importer. +// +// Like mcap_to_laz, deliberately free of core/TBB/glm: reading is +// rosbags/OusterPcapReader and writing is rosbags/MandeyeSessionWriter. +#include "MandeyeSessionWriter.h" +#include "OusterPcapReader.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace fs = std::filesystem; + +namespace +{ + + // Same absolute time grid as mcap_to_laz: a timestamp t belongs to bin + // floor(t / chunk_seconds), so lidar and IMU bin identically. + int64_t timeBin(double timestamp_s, double chunk_seconds) + { + if (chunk_seconds <= 0.0) + return 0; // splitting disabled: everything lands in one chunk + return static_cast(std::floor(timestamp_s / chunk_seconds)); + } + + // Rate of the --stationary-imu stream: the Livox Mid-360 IMU rate mandeye + // sessions are tuned for. + constexpr double kStationaryImuHz = 200.0; + + enum class RgbMode + { + Auto, + Raw, + Off + }; + + // Lidar time at the start of the recording whose colour --rgb auto is fitted on. + constexpr double kRgbFitSeconds = 1.0; + + // Maps the sensor's raw colour words to LAS colour. The FW 4.0 manual calls + // them float16, but the pre-release recordings read as linear 16-bit counts: + // as float16 the median pixel (~400) would be a subnormal and the 0xFFFF + // marker a NaN. 0xFFFF in all three channels marks a pixel without colour (it + // clusters on a couple of beams and never ramps up from below like + // saturation); those points get their reflectivity as grey, the way + // camera_lidar_trajectory_viewer exports uncoloured points. + class RgbMapper + { + public: + using Rgb = rosbags::MandeyeSessionWriter::Rgb; + + bool fitted() const + { + return fitted_; + } + + void setRaw() + { + raw_ = fitted_ = true; + } + + // Grey-world white balance, white point at the 99th percentile of the + // balanced values, gamma 2.2. Fitted once, so colour stays comparable + // across chunks. + void fit(const std::vector*>& samples) + { + std::array sum{}; + for (const auto* colours : samples) + { + for (const auto& c : *colours) + { + if (!hasColour(c)) + continue; + for (int k = 0; k < 3; ++k) + sum[k] += c[k]; + ++samples_; + } + } + const double grey = (sum[0] + sum[1] + sum[2]) / 3.0; + for (int k = 0; k < 3; ++k) + gains_[k] = sum[k] > 0.0 ? grey / sum[k] : 1.0; + + std::vector balanced; + balanced.reserve(samples_ * 3); + for (const auto* colours : samples) + { + for (const auto& c : *colours) + { + if (!hasColour(c)) + continue; + for (int k = 0; k < 3; ++k) + balanced.push_back(static_cast(c[k] * gains_[k])); + } + } + white_ = 65535.0; + if (!balanced.empty()) + { + const auto nth = balanced.begin() + static_cast(0.99 * static_cast(balanced.size() - 1)); + std::nth_element(balanced.begin(), nth, balanced.end()); + white_ = std::max(1.0, static_cast(*nth)); + } + + for (int k = 0; k < 3; ++k) + { + lut_[k].resize(65536); + for (size_t v = 0; v < lut_[k].size(); ++v) + { + const double x = std::clamp(static_cast(v) * gains_[k] / white_, 0.0, 1.0); + lut_[k][v] = static_cast(std::lround(65535.0 * std::pow(x, 1.0 / 2.2))); + } + } + fitted_ = true; + } + + void apply(const std::vector& points, const std::vector& in, std::vector& out) const + { + if (raw_) + { + out = in; + return; + } + out.resize(in.size()); + for (size_t i = 0; i < in.size(); ++i) + { + if (hasColour(in[i])) + out[i] = { lut_[0][in[i][0]], lut_[1][in[i][1]], lut_[2][in[i][2]] }; + else + { + const auto grey = static_cast(std::clamp(points[i].intensity, 0.0f, 255.0f) * 257.0f); + out[i] = { grey, grey, grey }; + } + } + } + + const std::array& gains() const + { + return gains_; + } + double white() const + { + return white_; + } + size_t samples() const + { + return samples_; + } + + private: + static bool hasColour(const Rgb& c) + { + return !(c[0] == 0xFFFF && c[1] == 0xFFFF && c[2] == 0xFFFF); + } + + bool fitted_ = false; + bool raw_ = false; + std::array gains_{ 1.0, 1.0, 1.0 }; + double white_ = 65535.0; + size_t samples_ = 0; + std::array, 3> lut_; + }; + + void print_usage(const char* argv0) + { + spdlog::error("Usage: {} [options]", argv0); + spdlog::error(" input.pcap an Ouster recording (ouster-cli / Ouster SDK / tcpdump, classic pcap format)"); + spdlog::error(" output_session_dir written as mandeye lidarNNNN.laz + imuNNNN.csv chunk pairs,"); + spdlog::error(" ready to open in lidar_odometry_step_1"); + spdlog::error("Options:"); + spdlog::error(" --metadata sensor metadata (default: .json next to the pcap)"); + spdlog::error(" --chunk-seconds chunk length in seconds (default: 20, mandeye's own"); + spdlog::error(" cadence; 0 = one single chunk)"); + spdlog::error(" --start skip the first N seconds of lidar data (default: 0)"); + spdlog::error(" --duration convert only N seconds of lidar data, counted from --start (default: all)"); + spdlog::error(" --column-step keep every Nth measurement column, e.g. 2 turns 1024 columns"); + spdlog::error(" into 512 (default: 1 = all)"); + spdlog::error(" --lidar-port lidar UDP port (default: config_params.udp_port_lidar)"); + spdlog::error(" --imu-port IMU UDP port (default: config_params.udp_port_imu)"); + spdlog::error(" --stationary-imu the capture has no IMU packets: write a synthetic {:.0f} Hz stream", kStationaryImuHz); + spdlog::error(" of zero rotation and +1 g on Z instead. lidar_odometry_step_1 then"); + spdlog::error(" runs as lidar-only odometry with a level, motionless prior --"); + spdlog::error(" an experiment, expect drift on fast turns."); + spdlog::error(" --rgb colour on RGB profiles, written as LAS point format 3 (default: auto)"); + spdlog::error(" auto: white-balanced, exposed and gamma-corrected, fitted on the"); + spdlog::error(" first {:.0f} s of the recording and then kept fixed", kRgbFitSeconds); + spdlog::error(" raw: the sensor's 16-bit colour words unchanged"); + spdlog::error(" off: no colour (point format 1)"); + spdlog::error(""); + spdlog::error("Points are written in the Ouster SENSOR frame (lidar_to_sensor_transform applied), with"); + spdlog::error("the column timestamp in seconds and reflectivity as intensity. The IMU is rotated into the"); + spdlog::error("same frame (gyro rad/s, acc g). No calibration.json or .sn file is written."); + spdlog::error("lidar_odometry_step_1 reads the points but not their colour."); + } + + bool parse_double(const std::string& flag, const std::string& value, double& out) + { + try + { + out = std::stod(value); + } catch (const std::exception&) + { + spdlog::error("Invalid {} '{}' (expected a number)", flag, value); + return false; + } + if (!std::isfinite(out) || out < 0.0) + { + spdlog::error("Invalid {} '{}' (expected >= 0)", flag, value); + return false; + } + return true; + } + + bool parse_int(const std::string& flag, const std::string& value, int min, int max, int& out) + { + try + { + size_t used = 0; + out = std::stoi(value, &used); + if (used != value.size()) + throw std::invalid_argument(value); + } catch (const std::exception&) + { + spdlog::error("Invalid {} '{}' (expected an integer)", flag, value); + return false; + } + if (out < min || out > max) + { + spdlog::error("Invalid {} '{}' (expected {}..{})", flag, value, min, max); + return false; + } + return true; + } + + // Routes every buffered IMU sample into the chunk that was open at its + // timestamp, exactly as mcap_to_laz does: the last chunk whose bin is <= the + // sample's bin, clamped to the first chunk for samples that predate all lidar + // data. Capture order is normally time order, but a sensor clock that jumps + // back (e.g. a PTP step) would leave chunk_bins unsorted, so search a sorted + // (bin, chunk index) view. + bool write_imu_chunks( + rosbags::MandeyeSessionWriter& writer, + const std::vector& samples, + const std::vector& chunk_bins, + double chunk_seconds) + { + std::vector> per_chunk(chunk_bins.size()); + + std::vector> bin_to_chunk; + bin_to_chunk.reserve(chunk_bins.size()); + for (size_t i = 0; i < chunk_bins.size(); ++i) + bin_to_chunk.emplace_back(chunk_bins[i], i); + std::sort(bin_to_chunk.begin(), bin_to_chunk.end()); + + for (const auto& sample : samples) + { + const int64_t bin = timeBin(sample.timestamp, chunk_seconds); + const auto it = std::upper_bound( + bin_to_chunk.begin(), + bin_to_chunk.end(), + bin, + [](int64_t value, const std::pair& entry) + { + return value < entry.first; + }); + const size_t index = (it == bin_to_chunk.begin()) ? bin_to_chunk.front().second : std::prev(it)->second; + per_chunk[index].push_back(sample); + } + + for (size_t i = 0; i < per_chunk.size(); ++i) + { + if (!writer.writeImuChunk(static_cast(i), per_chunk[i])) + return false; + } + return true; + } + +} // namespace + +int main(const int argc, const char** argv) +{ + if (argc < 2) + { + print_usage(argv[0]); + return EXIT_FAILURE; + } + + const fs::path pcap_path = argv[1]; + std::string session_dir; + fs::path metadata_path; + rosbags::OusterPcapReaderOptions options; + double chunk_seconds = 20.0; + double start = 0.0; + double duration = 0.0; + bool stationary_imu = false; + RgbMode rgb_mode = RgbMode::Auto; + bool rgb_requested = false; + + for (int i = 2; i < argc; ++i) + { + const std::string arg = argv[i]; + const bool hasValue = i + 1 < argc; + int port = 0; + + if (arg == "--metadata" && hasValue) + metadata_path = argv[++i]; + else if (arg == "--chunk-seconds" && hasValue) + { + if (!parse_double(arg, argv[++i], chunk_seconds)) + return EXIT_FAILURE; + } + else if (arg == "--start" && hasValue) + { + if (!parse_double(arg, argv[++i], start)) + return EXIT_FAILURE; + } + else if (arg == "--duration" && hasValue) + { + if (!parse_double(arg, argv[++i], duration)) + return EXIT_FAILURE; + } + else if (arg == "--column-step" && hasValue) + { + if (!parse_int(arg, argv[++i], 1, 4096, options.column_step)) + return EXIT_FAILURE; + } + else if (arg == "--lidar-port" && hasValue) + { + if (!parse_int(arg, argv[++i], 1, 65535, port)) + return EXIT_FAILURE; + options.lidar_port = static_cast(port); + } + else if (arg == "--imu-port" && hasValue) + { + if (!parse_int(arg, argv[++i], 1, 65535, port)) + return EXIT_FAILURE; + options.imu_port = static_cast(port); + } + else if (arg == "--stationary-imu") + stationary_imu = true; + else if (arg == "--rgb" && hasValue) + { + const std::string mode = argv[++i]; + if (mode == "auto") + rgb_mode = RgbMode::Auto; + else if (mode == "raw") + rgb_mode = RgbMode::Raw; + else if (mode == "off") + rgb_mode = RgbMode::Off; + else + { + spdlog::error("Unknown --rgb '{}' (expected auto|raw|off)", mode); + return EXIT_FAILURE; + } + rgb_requested = true; + } + else if (!arg.starts_with("--") && session_dir.empty()) + session_dir = arg; + else + { + spdlog::error("Unrecognized argument '{}'", arg); + print_usage(argv[0]); + return EXIT_FAILURE; + } + } + + if (!fs::exists(pcap_path)) + { + spdlog::error("Input file {} does not exist", pcap_path.string()); + return EXIT_FAILURE; + } + if (session_dir.empty()) + { + spdlog::error("Missing "); + print_usage(argv[0]); + return EXIT_FAILURE; + } + if (metadata_path.empty()) + metadata_path = fs::path(pcap_path).replace_extension(".json"); + + std::ifstream metadata_file(metadata_path); + if (!metadata_file.is_open()) + { + spdlog::error("Cannot open metadata {} - pass it with --metadata", metadata_path.string()); + return EXIT_FAILURE; + } + std::stringstream metadata_text; + metadata_text << metadata_file.rdbuf(); + + rosbags::OusterMetadata metadata; + std::string error; + if (!rosbags::parseOusterMetadata(metadata_text.str(), metadata, error)) + { + spdlog::error("{}: {}", metadata_path.string(), error); + return EXIT_FAILURE; + } + spdlog::info( + "Sensor {} sn {} fw {}: {}x{} @ {} columns/packet, profile {} ({} B/pixel, {} B/packet), lidar port {}, IMU port {} ({})", + metadata.prod_line, + metadata.prod_sn, + metadata.firmware, + metadata.columns_per_frame, + metadata.pixels_per_column, + metadata.columns_per_packet, + metadata.udp_profile_lidar, + metadata.pixel_bytes, + metadata.lidarPacketSize(), + options.lidar_port ? options.lidar_port : metadata.udp_port_lidar, + options.imu_port ? options.imu_port : metadata.udp_port_imu, + metadata.udp_profile_imu); + if (metadata.udp_profile_imu != "LEGACY") + spdlog::warn("IMU profile {} is not supported (LEGACY only) - IMU packets will be ignored", metadata.udp_profile_imu); + + rosbags::OusterPcapReader reader(pcap_path, metadata, options); + if (!reader.isOpen()) + { + spdlog::error("Cannot read {}: {}", pcap_path.string(), reader.error()); + return EXIT_FAILURE; + } + + const bool with_rgb = metadata.rgb_width > 0 && rgb_mode != RgbMode::Off; + if (rgb_requested && rgb_mode != RgbMode::Off && metadata.rgb_width == 0) + spdlog::warn("Profile {} carries no colour - --rgb ignored", metadata.udp_profile_lidar); + + rosbags::MandeyeSessionWriter writer(session_dir, with_rgb); + if (!writer.isOpen()) + { + spdlog::error("Cannot write session directory: {}", writer.error()); + return EXIT_FAILURE; + } + + using Rgb = rosbags::MandeyeSessionWriter::Rgb; + RgbMapper rgb_mapper; + if (rgb_mode == RgbMode::Raw) + rgb_mapper.setRaw(); + std::vector mapped_rgb; + + std::vector imu; + std::vector chunk_bins; + int64_t current_bin = 0; + double recording_start = std::numeric_limits::quiet_NaN(); + double first_stamp = std::numeric_limits::quiet_NaN(); + double last_stamp = 0.0; + bool ok = true; + + // A packet is the atomic unit, binned on its first column's timestamp. + auto write_packet = [&](const std::vector& points, const std::vector& rgb) + { + last_stamp = points.back().timestamp; + const int64_t bin = timeBin(points.front().timestamp, chunk_seconds); + if (chunk_bins.empty() || bin != current_bin) + { + if (!writer.beginChunk(static_cast(chunk_bins.size()))) + return false; + chunk_bins.push_back(bin); + current_bin = bin; + } + if (!with_rgb) + return writer.addPoints(points); + rgb_mapper.apply(points, rgb, mapped_rgb); + return writer.addPoints(points, mapped_rgb); + }; + + // Packets held back until --rgb auto has seen kRgbFitSeconds of colour. + struct HeldPacket + { + std::vector points; + std::vector rgb; + }; + std::vector held; + auto fit_rgb_and_flush = [&]() + { + std::vector*> samples; + for (const auto& packet : held) + samples.push_back(&packet.rgb); + rgb_mapper.fit(samples); + spdlog::info( + "RGB: white balance x{:.3f}/{:.3f}/{:.3f}, white point {:.0f}, gamma 2.2 - fitted on the first {:.2f} s ({} coloured " + "points); --rgb raw writes the sensor's values instead", + rgb_mapper.gains()[0], + rgb_mapper.gains()[1], + rgb_mapper.gains()[2], + rgb_mapper.white(), + held.back().points.back().timestamp - held.front().points.front().timestamp, + rgb_mapper.samples()); + for (const auto& packet : held) + { + if (!write_packet(packet.points, packet.rgb)) + return false; + } + held.clear(); + return true; + }; + + rosbags::OusterPcapReader::Callbacks callbacks; + callbacks.onImu = [&](const rosbags::McapImuSample& sample) + { + imu.push_back(sample); + }; + callbacks.onPoints = [&](std::vector&& points, std::vector&& rgb) + { + if (!ok) + return; + const double stamp_s = points.front().timestamp; + if (std::isnan(recording_start)) + recording_start = stamp_s; + if (stamp_s - recording_start < start) + return; + if (std::isnan(first_stamp)) + first_stamp = stamp_s; + if (duration > 0.0 && stamp_s - first_stamp >= duration) + { + reader.requestStop(); + return; + } + + if (with_rgb && !rgb_mapper.fitted()) + { + held.push_back({ std::move(points), std::move(rgb) }); + if (stamp_s - held.front().points.front().timestamp < kRgbFitSeconds) + return; + ok = fit_rgb_and_flush(); + } + else + ok = write_packet(points, rgb); + if (!ok) + reader.requestStop(); + }; + + const bool read_ok = reader.read(callbacks); + if (ok && !held.empty()) // recording shorter than the colour fit window + ok = fit_rgb_and_flush(); + if (!writer.endChunk()) + ok = false; + const auto& stats = reader.stats(); + spdlog::info( + "Read {} pcap record(s): {} lidar packet(s), {} IMU packet(s), {} other, {} reassembled from IP fragments", + stats.records, + stats.lidar_packets, + stats.imu_packets, + stats.other_packets, + stats.reassembled_datagrams); + if (stats.bad_size_packets > 0) + spdlog::warn( + "{} packet(s) on the lidar/IMU port had the wrong size for profile {} - is this the metadata of this recording?", + stats.bad_size_packets, + metadata.udp_profile_lidar); + if (stats.foreign_packets > 0) + spdlog::warn( + "{} lidar packet(s) skipped: their init_id / serial number does not match the metadata (another sensor, or the " + "sensor was reinitialized after the metadata was saved)", + stats.foreign_packets); + if (stats.dropped_fragments > 0) + spdlog::warn("{} IP fragment(s) dropped: their datagram never completed (packet loss during capture)", stats.dropped_fragments); + + if (!read_ok) + { + spdlog::error("Failed reading {}: {}", pcap_path.string(), reader.error()); + return EXIT_FAILURE; + } + if (!ok) + { + spdlog::error("Failed writing lidar chunks: {}", writer.error()); + return EXIT_FAILURE; + } + if (chunk_bins.empty()) + { + spdlog::error( + "No lidar points decoded from {} - nothing to write{}", pcap_path.string(), start > 0.0 ? " (is --start past the end?)" : ""); + return EXIT_FAILURE; + } + + if (start > 0.0 || duration > 0.0) + { + // Samples outside the converted lidar window would only pad the first / last csv. + std::erase_if( + imu, + [&](const rosbags::McapImuSample& s) + { + return s.timestamp < first_stamp || s.timestamp > last_stamp; + }); + } + + if (imu.empty() && stationary_imu) + { + const auto count = static_cast(std::floor((last_stamp - first_stamp) * kStationaryImuHz)) + 1; + imu.reserve(count); + for (size_t i = 0; i < count; ++i) + { + rosbags::McapImuSample s; + s.timestamp = first_stamp + static_cast(i) / kStationaryImuHz; + s.acc_z = 1.0f; + imu.push_back(s); + } + spdlog::warn("No IMU packets in the capture: wrote {} synthetic stationary IMU sample(s) (--stationary-imu)", imu.size()); + } + else if (stationary_imu) + spdlog::warn("--stationary-imu ignored: the capture has {} real IMU sample(s)", imu.size()); + + if (!write_imu_chunks(writer, imu, chunk_bins, chunk_seconds)) + { + spdlog::error("Failed writing IMU chunks: {}", writer.error()); + return EXIT_FAILURE; + } + + spdlog::info( + "Wrote {} chunk(s) to {}: {} point(s){} over {:.2f} s, {} IMU sample(s)", + chunk_bins.size(), + session_dir, + writer.pointsWritten(), + with_rgb ? " with colour" : "", + last_stamp - first_stamp, + writer.imuSamplesWritten()); + + if (imu.empty()) + spdlog::warn( + "No IMU data: the imuNNNN.csv files are header-only and lidar_odometry_step_1 cannot run on this session. Check the " + "capture includes UDP port {}, or retry with --stationary-imu", + options.imu_port ? options.imu_port : metadata.udp_port_imu); + + // lidar_odometry_step_1's load_data() needs more than two input files and then + // discards the first chunk's points outright. + if (chunk_bins.size() < 3) + spdlog::warn( + "Only {} chunk(s): lidar_odometry_step_1 requires more than two files and discards the first chunk's " + "points - lower --chunk-seconds to split this recording further", + chunk_bins.size()); + + return EXIT_SUCCESS; +} diff --git a/deploy_mandeye.bat b/deploy_mandeye.bat index 74fef193..5899b6c9 100644 --- a/deploy_mandeye.bat +++ b/deploy_mandeye.bat @@ -60,7 +60,7 @@ rem This list has to be kept in sync by hand with the hdmapping_install_app() rem calls in apps/*/CMakeLists.txt. hd_mapper is deliberately absent: it is rem gated behind BUILD_WITH_HD_MAPPER_APPLICATION, which defaults to OFF, so rem listing it would warn on every normal build. -set "EXPECTED=camera_lidar_calibration.exe camera_lidar_intrinsics_calib.exe camera_lidar_trajectory_viewer.exe concatenate_multi_livox.exe drag_folder_with_mandeye_data_and_drop_here-precision_forestry.exe laz_to_mcap.exe laz_to_pcd.exe laz_to_ply.exe laz_to_txt.exe lidar_odometry_step_1.exe livox_mid_360_intrinsic_calibration.exe mandeye_compare_trajectories.exe mandeye_mission_recorder_calibration.exe mandeye_raw_data_viewer.exe mandeye_single_session_viewer.exe matrix_mul.exe mcap_to_laz.exe multiply_timestamps_session_point_cloud_laz.exe multiply_timestamps_session_trajectory_csv.exe multi_session_registration_step_3.exe multi_session_registration_step_3_legacy.exe multi_view_tls_registration_step_2.exe multi_view_tls_registration_step_2_legacy.exe pcd_to_laz.exe precision_forestry_tools.exe session_to_mcap.exe split_multi_livox.exe freeglut.dll laszip3.dll opencv_world4130.dll proj_9_3.dll tbb12.dll z.dll" +set "EXPECTED=camera_lidar_calibration.exe camera_lidar_intrinsics_calib.exe camera_lidar_trajectory_viewer.exe concatenate_multi_livox.exe drag_folder_with_mandeye_data_and_drop_here-precision_forestry.exe laz_to_mcap.exe laz_to_pcd.exe laz_to_ply.exe laz_to_txt.exe lidar_odometry_step_1.exe livox_mid_360_intrinsic_calibration.exe mandeye_compare_trajectories.exe mandeye_mission_recorder_calibration.exe mandeye_raw_data_viewer.exe mandeye_single_session_viewer.exe matrix_mul.exe mcap_to_laz.exe multiply_timestamps_session_point_cloud_laz.exe multiply_timestamps_session_trajectory_csv.exe multi_session_registration_step_3.exe multi_session_registration_step_3_legacy.exe multi_view_tls_registration_step_2.exe multi_view_tls_registration_step_2_legacy.exe ouster_pcap_to_laz.exe pcd_to_laz.exe precision_forestry_tools.exe session_to_mcap.exe split_multi_livox.exe freeglut.dll laszip3.dll opencv_world4130.dll proj_9_3.dll tbb12.dll z.dll" set "MISSING=" for %%F in (%EXPECTED%) do ( diff --git a/rosbags/MandeyeSessionWriter.cpp b/rosbags/MandeyeSessionWriter.cpp index 8aff3f13..a3f696d6 100644 --- a/rosbags/MandeyeSessionWriter.cpp +++ b/rosbags/MandeyeSessionWriter.cpp @@ -34,6 +34,7 @@ struct MandeyeSessionWriter::Impl std::filesystem::path dir; std::string error; bool open = false; + bool withRgb = false; laszip_POINTER writer = nullptr; laszip_point* point = nullptr; @@ -50,10 +51,11 @@ struct MandeyeSessionWriter::Impl } }; -MandeyeSessionWriter::MandeyeSessionWriter(const std::filesystem::path& dir) +MandeyeSessionWriter::MandeyeSessionWriter(const std::filesystem::path& dir, bool with_rgb) : impl_(std::make_unique()) { impl_->dir = dir; + impl_->withRgb = with_rgb; std::error_code ec; std::filesystem::create_directories(dir, ec); @@ -110,8 +112,9 @@ bool MandeyeSessionWriter::beginChunk(int index) header->global_encoding = (1 << 0); header->version_major = 1; header->version_minor = 2; - header->point_data_format = 1; // XYZ + intensity + user_data + gps_time - header->point_data_record_length = 28; + // 1: XYZ + intensity + user_data + gps_time; 3: the same + 16-bit RGB. + header->point_data_format = impl_->withRgb ? 3 : 1; + header->point_data_record_length = impl_->withRgb ? 34 : 28; header->number_of_point_records = 0; header->number_of_points_by_return[0] = 0; header->number_of_points_by_return[1] = 0; @@ -134,14 +137,31 @@ bool MandeyeSessionWriter::beginChunk(int index) } bool MandeyeSessionWriter::addPoints(const std::vector& points) +{ + return addPoints(points, {}); +} + +bool MandeyeSessionWriter::addPoints(const std::vector& points, const std::vector& rgb) { if(!isOpen()) return false; if(!impl_->writer) return impl_->fail("addPoints() called with no chunk open"); + if(!rgb.empty() && (!impl_->withRgb || rgb.size() != points.size())) + return impl_->fail( + impl_->withRgb ? "addPoints(): " + std::to_string(rgb.size()) + " colours for " + std::to_string(points.size()) + " points" + : "addPoints(): colours given to a writer opened without with_rgb"); - for(const auto& p : points) + for(size_t i = 0; i < points.size(); ++i) { + const auto& p = points[i]; + if(impl_->withRgb) + { + const Rgb colour = rgb.empty() ? Rgb{} : rgb[i]; + impl_->point->rgb[0] = colour[0]; + impl_->point->rgb[1] = colour[1]; + impl_->point->rgb[2] = colour[2]; + } impl_->point->intensity = static_cast(std::clamp(p.intensity, 0.0f, 65535.0f)); impl_->point->return_number = 1; impl_->point->number_of_returns = 1; diff --git a/rosbags/MandeyeSessionWriter.h b/rosbags/MandeyeSessionWriter.h index 84d488f2..14157106 100644 --- a/rosbags/MandeyeSessionWriter.h +++ b/rosbags/MandeyeSessionWriter.h @@ -1,5 +1,6 @@ #pragma once #include "McapWriter.h" // McapPoint, McapImuSample +#include #include #include #include @@ -22,6 +23,9 @@ namespace rosbags // gps_time = McapPoint::timestamp (absolute seconds, unscaled) // user_data = 0 (lidar id; single sensor per session) // intensity = McapPoint::intensity (clamped to uint16) +// red/green/blue (with_rgb only) LAS 16-bit colour, point format 3 +// instead of 1; load_point_cloud() reads +// neither the colour nor the format // // The IMU csv uses the modern named-column format load_imu() prefers, in the // column order concatenate_multi_livox already writes, with both timestamps in @@ -29,8 +33,12 @@ namespace rosbags class MandeyeSessionWriter { public: - // Creates `dir` if it does not exist. - explicit MandeyeSessionWriter(const std::filesystem::path& dir); + // One point's LAS colour (red, green, blue, full 16-bit scale). + using Rgb = std::array; + + // Creates `dir` if it does not exist. `with_rgb` writes every chunk as LAS + // point format 3 (format 1 + RGB) instead of format 1. + explicit MandeyeSessionWriter(const std::filesystem::path& dir, bool with_rgb = false); ~MandeyeSessionWriter(); // Non-copyable @@ -42,7 +50,10 @@ class MandeyeSessionWriter // Opens lidar.laz. Any chunk still open is closed first. bool beginChunk(int index); + // Without colours, a with_rgb writer stores black. bool addPoints(const std::vector& points); + // `rgb` is parallel to `points`; only valid on a with_rgb writer. + bool addPoints(const std::vector& points, const std::vector& rgb); bool endChunk(); // Writes imu.csv in one shot. Samples are written in the order diff --git a/rosbags/OusterPcapReader.cpp b/rosbags/OusterPcapReader.cpp new file mode 100644 index 00000000..0453252b --- /dev/null +++ b/rosbags/OusterPcapReader.cpp @@ -0,0 +1,800 @@ +#include "OusterPcapReader.h" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rosbags +{ + +namespace +{ + +using nlohmann::json; + +// Non-legacy lidar packet framing ("header_type": "STANDARD"). +constexpr size_t kPacketHeaderBytes = 32; +constexpr size_t kPacketFooterBytes = 32; +constexpr size_t kColumnHeaderBytes = 12; // u64 timestamp ns, u16 measurement_id, u16 status +constexpr size_t kLegacyImuPacketBytes = 48; + +// Profiles whose channel data block is known without a pixel_data_format. All of +// them carry the first return in word 0 -- range [18:0] in mm, reflectivity +// [31:24] -- then signal and NIR in word 1, and differ only in what follows: +// the RGB ones put red, green, blue in bits [79:64], [95:80], [111:96]. +struct KnownProfile +{ + const char* name; + int pixel_bytes; + bool rgb; +}; +constexpr KnownProfile kKnownProfiles[] = { + {"RNG19_RFL8_SIG16_NIR16", 12, false}, + {"RNG19_RFL8_SIG16_NIR16_RGB16", 16, true}, + // FW 4.0.0 pre-release name of the RGB profile: two RGB sets, 20 bytes. + {"RNG19_RFL8_SIG16_NIR16_R16_G16_B16", 20, true}, +}; + +// pcap link-layer types handled by linkPayload(). +constexpr uint32_t kLinkNull = 0; +constexpr uint32_t kLinkEthernet = 1; +constexpr uint32_t kLinkRaw = 101; +constexpr uint32_t kLinkLoop = 108; +constexpr uint32_t kLinkLinuxSll = 113; +constexpr uint32_t kLinkIpv4 = 228; +constexpr uint32_t kLinkLinuxSll2 = 276; + +// Anything bigger is a corrupt record header, not a packet: the largest Ouster +// lidar packet is ~33 kB and a reassembled IPv4 datagram is at most 64 kB. +constexpr uint32_t kMaxRecordBytes = 256 * 1024; +// Datagrams being reassembled at once; Ouster streams interleave at most a +// lidar and an IMU datagram, so anything beyond this is fragments whose +// siblings were never captured. +constexpr size_t kMaxPendingDatagrams = 64; + +uint16_t le16(const uint8_t* p) +{ + return static_cast(p[0] | (p[1] << 8)); +} + +uint64_t le64(const uint8_t* p) +{ + uint64_t v = 0; + for(int i = 7; i >= 0; --i) + v = (v << 8) | p[i]; + return v; +} + +float leFloat(const uint8_t* p) +{ + const uint32_t bits = static_cast(p[0]) | (static_cast(p[1]) << 8) | (static_cast(p[2]) << 16) | + (static_cast(p[3]) << 24); + float v; + std::memcpy(&v, &bits, sizeof(v)); + return v; +} + +uint16_t be16(const uint8_t* p) +{ + return static_cast((p[0] << 8) | p[1]); +} + +uint32_t be32(const uint8_t* p) +{ + return (static_cast(p[0]) << 24) | (static_cast(p[1]) << 16) | (static_cast(p[2]) << 8) | p[3]; +} + +uint32_t swap32(uint32_t v) +{ + return ((v & 0xFF) << 24) | ((v & 0xFF00) << 8) | ((v >> 8) & 0xFF00) | (v >> 24); +} + +// Little-endian bit field of a channel data block, as pixel_data_format +// describes it (bit 0 = LSB of byte 0). +uint32_t bitField(const uint8_t* pixel, int start_bit, int width) +{ + const uint8_t* p = pixel + start_bit / 8; + const int shift = start_bit % 8; + const int bytes = (shift + width + 7) / 8; + uint64_t v = 0; + for(int i = bytes - 1; i >= 0; --i) + v = (v << 8) | p[i]; + return static_cast((v >> shift) & ((uint64_t{1} << width) - 1)); +} + +bool readMatrix(const json& parent, const char* key, std::array& out, std::string& error) +{ + if(!parent.contains(key) || !parent[key].is_array() || parent[key].size() != 16) + { + error = std::string("metadata: '") + key + "' must be an array of 16 numbers"; + return false; + } + for(size_t i = 0; i < 16; ++i) + out[i] = parent[key][i].get(); + return true; +} + +bool readAngles(const json& parent, const char* key, size_t expected, std::vector& out, std::string& error) +{ + if(!parent.contains(key) || !parent[key].is_array() || parent[key].size() != expected) + { + error = std::string("metadata: beam_intrinsics.") + key + " must hold one angle per beam (" + std::to_string(expected) + ")"; + return false; + } + out = parent[key].get>(); + return true; +} + +} // namespace + +size_t OusterMetadata::lidarPacketSize() const +{ + return kPacketHeaderBytes + static_cast(columns_per_packet) * (kColumnHeaderBytes + static_cast(pixels_per_column) * pixel_bytes) + + kPacketFooterBytes; +} + +bool parseOusterMetadata(const std::string& json_text, OusterMetadata& out, std::string& error) +{ + OusterMetadata m; + try + { + const json j = json::parse(json_text); + if(!j.contains("lidar_data_format") || !j.contains("beam_intrinsics") || !j.contains("config_params")) + { + error = "metadata: missing lidar_data_format / beam_intrinsics / config_params -- only the non-legacy " + "(FW 2.4+) metadata layout is supported"; + return false; + } + + if(j.contains("sensor_info")) + { + const json& info = j["sensor_info"]; + m.prod_sn = info.value("prod_sn", ""); + m.prod_line = info.value("prod_line", ""); + m.firmware = info.value("image_rev", ""); + if(info.contains("initialization_id") && info["initialization_id"].is_number_unsigned()) + { + m.initialization_id = info["initialization_id"].get(); + m.has_initialization_id = true; + } + } + + const json& config = j["config_params"]; + m.udp_port_lidar = config.value("udp_port_lidar", uint16_t{7502}); + m.udp_port_imu = config.value("udp_port_imu", uint16_t{7503}); + m.udp_profile_imu = config.value("udp_profile_imu", std::string("LEGACY")); + + const json& format = j["lidar_data_format"]; + m.pixels_per_column = format.at("pixels_per_column").get(); + m.columns_per_packet = format.at("columns_per_packet").get(); + m.columns_per_frame = format.at("columns_per_frame").get(); + m.udp_profile_lidar = format.value("udp_profile_lidar", config.value("udp_profile_lidar", std::string("LEGACY"))); + m.header_type = format.value("header_type", std::string("STANDARD")); + if(m.pixels_per_column <= 0 || m.columns_per_packet <= 0 || m.columns_per_frame <= 0) + { + error = "metadata: lidar_data_format has a non-positive pixels_per_column / columns_per_packet / columns_per_frame"; + return false; + } + if(m.header_type != "STANDARD") + { + error = "metadata: header_type '" + m.header_type + "' is not supported (STANDARD only)"; + return false; + } + if(m.udp_profile_lidar == "LEGACY") + { + error = "metadata: the LEGACY lidar profile is not supported -- reconfigure the sensor to RNG19_RFL8_SIG16_NIR16"; + return false; + } + + // The sensor's own description of the channel data block wins over the + // name table: it is what lets a pre-release profile be read at all. + if(j.contains("pixel_data_format")) + { + const json& pdf = j["pixel_data_format"]; + m.pixel_bytes = pdf.at("pixel_bytes").get(); + bool has_range = false; + std::array rgb_width{}; + for(const auto& field : pdf.at("fields")) + { + const std::string name = field.at("field_name").get(); + const int start = field.at("start_bit").get(); + const int width = field.at("width").get(); + // First match only: in a dual-return profile return 1 is listed first. + // Range fields are in mm (e.g. range_capped_mm_0). + if(!has_range && name.starts_with("range")) + { + m.range_start_bit = start; + m.range_width = width; + has_range = true; + } + else if(m.reflectivity_width == 0 && name.starts_with("reflectivity")) + { + m.reflectivity_start_bit = start; + m.reflectivity_width = width; + } + else + { + // Exact names: the pre-release RGB profile also lists a second + // set, rgb_r1/rgb_g1/rgb_b1, which carries no image. + static constexpr const char* kRgbNames[3] = {"rgb_r", "rgb_g", "rgb_b"}; + for(int c = 0; c < 3; ++c) + { + if(name == kRgbNames[c]) + { + m.rgb_start_bit[c] = start; + rgb_width[c] = width; + } + } + } + } + if(!has_range) + { + error = "metadata: pixel_data_format has no range field"; + return false; + } + // Colour only when all three channels are there and fit a LAS 16-bit channel. + if(rgb_width[0] > 0 && rgb_width[0] <= 16 && rgb_width[1] == rgb_width[0] && rgb_width[2] == rgb_width[0]) + m.rgb_width = rgb_width[0]; + } + else + { + const auto it = std::find_if( + std::begin(kKnownProfiles), + std::end(kKnownProfiles), + [&](const KnownProfile& p) + { + return m.udp_profile_lidar == p.name; + }); + if(it == std::end(kKnownProfiles)) + { + std::string known; + for(const auto& p : kKnownProfiles) + known += std::string(known.empty() ? "" : ", ") + p.name; + error = "metadata: lidar profile '" + m.udp_profile_lidar + "' is not supported and the metadata has no pixel_data_format " + "to describe it (known: " + known + ")"; + return false; + } + m.pixel_bytes = it->pixel_bytes; + m.range_start_bit = 0; + m.range_width = 19; + m.reflectivity_start_bit = 24; + m.reflectivity_width = 8; + if(it->rgb) + { + m.rgb_start_bit = {64, 80, 96}; + m.rgb_width = 16; + } + } + for(const auto& [start, width] : + {std::pair{m.range_start_bit, m.range_width}, + std::pair{m.reflectivity_start_bit, m.reflectivity_width}, + std::pair{m.rgb_start_bit[0], m.rgb_width}, + std::pair{m.rgb_start_bit[1], m.rgb_width}, + std::pair{m.rgb_start_bit[2], m.rgb_width}}) + { + if(width > 32 || start < 0 || start + width > m.pixel_bytes * 8) + { + error = "metadata: a pixel field lies outside the " + std::to_string(m.pixel_bytes) + "-byte channel data block or is wider than 32 bits"; + return false; + } + } + + const json& beams = j["beam_intrinsics"]; + const size_t beam_count = static_cast(m.pixels_per_column); + if(!readAngles(beams, "beam_altitude_angles", beam_count, m.beam_altitude_angles_deg, error) || + !readAngles(beams, "beam_azimuth_angles", beam_count, m.beam_azimuth_angles_deg, error)) + return false; + m.lidar_origin_to_beam_origin_mm = beams.value("lidar_origin_to_beam_origin_mm", 0.0); + if(beams.contains("beam_to_lidar_transform")) + { + if(!readMatrix(beams, "beam_to_lidar_transform", m.beam_to_lidar_transform, error)) + return false; + } + else + { + // Pre-FW 2.5 metadata: only the scalar offset, along the beam's x axis. + m.beam_to_lidar_transform = {1, 0, 0, m.lidar_origin_to_beam_origin_mm, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; + } + + if(!j.contains("lidar_intrinsics") || !readMatrix(j["lidar_intrinsics"], "lidar_to_sensor_transform", m.lidar_to_sensor_transform, error)) + { + error = "metadata: lidar_intrinsics.lidar_to_sensor_transform is missing or malformed"; + return false; + } + if(!j.contains("imu_intrinsics") || !readMatrix(j["imu_intrinsics"], "imu_to_sensor_transform", m.imu_to_sensor_transform, error)) + { + error = "metadata: imu_intrinsics.imu_to_sensor_transform is missing or malformed"; + return false; + } + } catch(const std::exception& e) + { + error = std::string("metadata: ") + e.what(); + return false; + } + + out = std::move(m); + return true; +} + +struct OusterPcapReader::Impl +{ + OusterMetadata meta; + OusterPcapReaderOptions options; + OusterPcapStats stats; + std::string error; + bool open = false; + bool stop = false; + bool have_lidar_timestamp = false; + + std::ifstream in; + std::vector buffer; + bool swapped = false; + bool nanoseconds = false; + uint32_t linktype = 0; + + uint64_t prod_sn = 0; + bool check_sn = false; + + // Per pixel (row-major, beam u, measurement id v): point = direction * range_mm + // + offset, already in the sensor frame and in metres -- the Ouster SDK's + // make_xyz_lut() with range_unit = 1 mm. + std::vector lut_direction; + std::vector lut_offset; + std::array imu_rotation{}; + + struct PendingDatagram + { + std::map> parts; // byte offset -> payload + uint32_t total = 0; // known once the last fragment arrives + uint64_t age = 0; + }; + std::map, PendingDatagram> pending; + uint64_t pending_clock = 0; + + std::vector record; + std::vector datagram; + std::vector points; + std::vector> rgb; + + bool fail(std::string message) + { + if(error.empty()) + error = std::move(message); + return false; + } + + void buildLut() + { + const int w = meta.columns_per_frame; + const int h = meta.pixels_per_column; + const auto& b2l = meta.beam_to_lidar_transform; + const auto& l2s = meta.lidar_to_sensor_transform; + const double beam_offset_x = b2l[3]; + const double beam_offset_z = b2l[11]; + const double n = std::hypot(beam_offset_x, beam_offset_z); + constexpr double kDeg = std::numbers::pi / 180.0; + constexpr double kMmToM = 0.001; + + lut_direction.resize(static_cast(w) * h * 3); + lut_offset.resize(static_cast(w) * h * 3); + for(int u = 0; u < h; ++u) + { + const double azimuth = -meta.beam_azimuth_angles_deg[u] * kDeg; + const double altitude = meta.beam_altitude_angles_deg[u] * kDeg; + for(int v = 0; v < w; ++v) + { + const double encoder = 2.0 * std::numbers::pi - v * (2.0 * std::numbers::pi / w); + const double dir[3] = { + std::cos(encoder + azimuth) * std::cos(altitude), std::sin(encoder + azimuth) * std::cos(altitude), std::sin(altitude)}; + const double off[3] = { + std::cos(encoder) * beam_offset_x - dir[0] * n, std::sin(encoder) * beam_offset_x - dir[1] * n, -dir[2] * n + beam_offset_z}; + + const size_t i = (static_cast(u) * w + v) * 3; + for(int r = 0; r < 3; ++r) + { + const double* row = &l2s[r * 4]; + lut_direction[i + r] = (row[0] * dir[0] + row[1] * dir[1] + row[2] * dir[2]) * kMmToM; + lut_offset[i + r] = (row[0] * off[0] + row[1] * off[1] + row[2] * off[2] + row[3]) * kMmToM; + } + } + } + } + + uint32_t u32(const uint8_t* p) const + { + uint32_t v; + std::memcpy(&v, p, sizeof(v)); + return swapped ? swap32(v) : v; + } + + // Locates the IPv4 header inside a captured frame. Returns false for anything + // that is not IPv4 (ARP, IPv6, ...). + bool linkPayload(const uint8_t* frame, size_t size, size_t& ip_offset) const + { + switch(linktype) + { + case kLinkEthernet: + { + size_t off = 12; + if(size < off + 2) + return false; + uint16_t ethertype = be16(frame + off); + while(ethertype == 0x8100 || ethertype == 0x88A8 || ethertype == 0x9100) // VLAN / QinQ tags + { + off += 4; + if(size < off + 2) + return false; + ethertype = be16(frame + off); + } + ip_offset = off + 2; + return ethertype == 0x0800; + } + case kLinkLinuxSll: + ip_offset = 16; + return size >= 16 && be16(frame + 14) == 0x0800; + case kLinkLinuxSll2: + ip_offset = 20; + return size >= 20 && be16(frame) == 0x0800; + case kLinkNull: + case kLinkLoop: + ip_offset = 4; + return size >= 4 && (u32(frame) == 2 || swap32(u32(frame)) == 2); // AF_INET, host order of the capturing machine + default: // kLinkRaw, kLinkIpv4 + ip_offset = 0; + return size >= 1 && (frame[0] >> 4) == 4; + } + } + + void handleUdp(const uint8_t* udp, size_t size) + { + if(size < 8) + { + ++stats.other_packets; + return; + } + const uint16_t dport = be16(udp + 2); + const size_t length = std::min(std::max(be16(udp + 4), 8), size) - 8; + const uint8_t* payload = udp + 8; + + if(dport == options.lidar_port) + handleLidar(payload, length); + else if(dport == options.imu_port && meta.udp_profile_imu == "LEGACY") + handleImu(payload, length); + else + ++stats.other_packets; + } + + void handleLidar(const uint8_t* p, size_t size) + { + if(size != meta.lidarPacketSize()) + { + ++stats.bad_size_packets; + return; + } + const uint64_t init_id = p[4] | (p[5] << 8) | (static_cast(p[6]) << 16); + uint64_t sn = 0; + for(int i = 11; i >= 7; --i) + sn = (sn << 8) | p[i]; + if((meta.has_initialization_id && init_id != meta.initialization_id) || (check_sn && sn != prod_sn)) + { + ++stats.foreign_packets; + return; + } + + const int w = meta.columns_per_frame; + const int h = meta.pixels_per_column; + const size_t column_bytes = kColumnHeaderBytes + static_cast(h) * meta.pixel_bytes; + points.clear(); + points.reserve(static_cast(meta.columns_per_packet) * h); + rgb.clear(); + if(meta.rgb_width) + rgb.reserve(points.capacity()); + bool any_valid = false; + for(int c = 0; c < meta.columns_per_packet; ++c) + { + const uint8_t* col = p + kPacketHeaderBytes + c * column_bytes; + const uint64_t timestamp_ns = le64(col); + const uint16_t measurement_id = le16(col + 8); + const uint16_t status = le16(col + 10); + if(!(status & 1) || measurement_id >= w) + { + ++stats.invalid_columns; + continue; + } + const double t = static_cast(timestamp_ns) * 1e-9; + if(!have_lidar_timestamp) + { + stats.first_lidar_timestamp = t; + have_lidar_timestamp = true; + } + stats.last_lidar_timestamp = std::max(stats.last_lidar_timestamp, t); + any_valid = true; + if(measurement_id % options.column_step != 0) + continue; + + const uint8_t* pixel = col + kColumnHeaderBytes; + for(int u = 0; u < h; ++u, pixel += meta.pixel_bytes) + { + const uint32_t range_mm = bitField(pixel, meta.range_start_bit, meta.range_width); + if(range_mm == 0) + continue; // no return + const size_t i = (static_cast(u) * w + measurement_id) * 3; + McapPoint pt; + pt.x = static_cast(lut_direction[i] * range_mm + lut_offset[i]); + pt.y = static_cast(lut_direction[i + 1] * range_mm + lut_offset[i + 1]); + pt.z = static_cast(lut_direction[i + 2] * range_mm + lut_offset[i + 2]); + pt.intensity = + meta.reflectivity_width ? static_cast(bitField(pixel, meta.reflectivity_start_bit, meta.reflectivity_width)) : 0.0f; + pt.ring = static_cast(u); + pt.timestamp = t; + points.push_back(pt); + if(meta.rgb_width) + { + rgb.push_back( + {static_cast(bitField(pixel, meta.rgb_start_bit[0], meta.rgb_width)), + static_cast(bitField(pixel, meta.rgb_start_bit[1], meta.rgb_width)), + static_cast(bitField(pixel, meta.rgb_start_bit[2], meta.rgb_width))}); + } + } + } + if(any_valid) + ++stats.lidar_packets; + if(!points.empty() && onPoints) + onPoints(std::move(points), std::move(rgb)); + points = {}; + rgb = {}; + } + + void handleImu(const uint8_t* p, size_t size) + { + // LEGACY IMU packet: u64 system / accel / gyro read times (ns), then + // accel x y z (g) and gyro x y z (deg/s) as float32. + if(size != kLegacyImuPacketBytes) + { + ++stats.bad_size_packets; + return; + } + const double acc[3] = {leFloat(p + 24), leFloat(p + 28), leFloat(p + 32)}; + const double gyro[3] = {leFloat(p + 36), leFloat(p + 40), leFloat(p + 44)}; + constexpr double kDegToRad = std::numbers::pi / 180.0; + const auto& r = imu_rotation; + + McapImuSample s; + // The gyro read time follows the configured timestamp_mode, like the lidar + // column timestamps; the system timestamp is a diagnostic counter. + s.timestamp = static_cast(le64(p + 16)) * 1e-9; + s.acc_x = static_cast(r[0] * acc[0] + r[1] * acc[1] + r[2] * acc[2]); + s.acc_y = static_cast(r[3] * acc[0] + r[4] * acc[1] + r[5] * acc[2]); + s.acc_z = static_cast(r[6] * acc[0] + r[7] * acc[1] + r[8] * acc[2]); + s.gyro_x = static_cast((r[0] * gyro[0] + r[1] * gyro[1] + r[2] * gyro[2]) * kDegToRad); + s.gyro_y = static_cast((r[3] * gyro[0] + r[4] * gyro[1] + r[5] * gyro[2]) * kDegToRad); + s.gyro_z = static_cast((r[6] * gyro[0] + r[7] * gyro[1] + r[8] * gyro[2]) * kDegToRad); + ++stats.imu_packets; + if(onImu) + onImu(s); + } + + // Feeds one IPv4 fragment in; once every byte of its datagram has arrived, + // hands the reassembled UDP datagram on and forgets it. + void handleFragment(const uint8_t* ip, size_t ihl, size_t ip_length) + { + const auto key = std::make_tuple(be32(ip + 12), be32(ip + 16), be16(ip + 4)); + const uint16_t flags_offset = be16(ip + 6); + const bool more_fragments = (flags_offset & 0x2000) != 0; + const uint32_t offset = static_cast(flags_offset & 0x1FFF) * 8; + const size_t length = ip_length - ihl; + + auto [it, inserted] = pending.try_emplace(key); + PendingDatagram& d = it->second; + if(inserted) + d.age = pending_clock++; + d.parts[offset].assign(ip + ihl, ip + ihl + length); + if(!more_fragments) + d.total = offset + static_cast(length); + + if(d.total != 0) + { + uint32_t covered = 0; + for(const auto& [part_offset, bytes] : d.parts) + { + if(part_offset > covered) + break; // hole + covered = std::max(covered, part_offset + static_cast(bytes.size())); + } + if(covered >= d.total) + { + datagram.assign(d.total, 0); + for(const auto& [part_offset, bytes] : d.parts) + { + if(part_offset >= d.total) + break; + std::memcpy(datagram.data() + part_offset, bytes.data(), std::min(bytes.size(), d.total - part_offset)); + } + pending.erase(it); + ++stats.reassembled_datagrams; + handleUdp(datagram.data(), datagram.size()); + return; + } + } + + if(pending.size() > kMaxPendingDatagrams) + { + auto oldest = std::min_element( + pending.begin(), + pending.end(), + [](const auto& a, const auto& b) + { + return a.second.age < b.second.age; + }); + stats.dropped_fragments += oldest->second.parts.size(); + pending.erase(oldest); + } + } + + void handleFrame(const uint8_t* frame, size_t size) + { + size_t off = 0; + if(!linkPayload(frame, size, off) || size < off + 20) + { + ++stats.other_packets; + return; + } + const uint8_t* ip = frame + off; + const size_t ihl = static_cast(ip[0] & 0x0F) * 4; + const size_t ip_length = std::min(be16(ip + 2), size - off); + if((ip[0] >> 4) != 4 || ihl < 20 || ip_length < ihl || ip[9] != 17 /* UDP */) + { + ++stats.other_packets; + return; + } + if((be16(ip + 6) & 0x3FFF) != 0) // more-fragments flag or a non-zero offset + handleFragment(ip, ihl, ip_length); + else + handleUdp(ip + ihl, ip_length - ihl); + } + + decltype(Callbacks::onPoints) onPoints; + std::function onImu; +}; + +OusterPcapReader::OusterPcapReader(const std::filesystem::path& pcap_path, const OusterMetadata& metadata, const OusterPcapReaderOptions& options) + : impl_(std::make_unique()) +{ + Impl& d = *impl_; + d.meta = metadata; + d.options = options; + if(d.options.lidar_port == 0) + d.options.lidar_port = metadata.udp_port_lidar; + if(d.options.imu_port == 0) + d.options.imu_port = metadata.udp_port_imu; + d.options.column_step = std::max(1, d.options.column_step); + + if(!metadata.prod_sn.empty()) + { + try + { + d.prod_sn = std::stoull(metadata.prod_sn); + d.check_sn = true; + } catch(const std::exception&) + { + // Non-numeric serial: nothing to compare the 40-bit header field with. + } + } + + const auto& m = metadata.imu_to_sensor_transform; + d.imu_rotation = {m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]}; + d.buildLut(); + + d.buffer.resize(1 << 20); + d.in.rdbuf()->pubsetbuf(d.buffer.data(), static_cast(d.buffer.size())); + d.in.open(pcap_path, std::ios::binary); + if(!d.in.is_open()) + { + d.fail("cannot open " + pcap_path.string()); + return; + } + + uint8_t header[24]; + if(!d.in.read(reinterpret_cast(header), sizeof(header))) + { + d.fail(pcap_path.string() + " is too short to be a pcap file"); + return; + } + uint32_t magic; + std::memcpy(&magic, header, sizeof(magic)); + switch(magic) + { + case 0xA1B2C3D4: + break; + case 0xA1B23C4D: + d.nanoseconds = true; + break; + case 0xD4C3B2A1: + d.swapped = true; + break; + case 0x4D3CB2A1: + d.swapped = d.nanoseconds = true; + break; + case 0x0A0D0D0A: + d.fail(pcap_path.string() + " is pcapng, which is not supported -- convert it with: editcap -F pcap in.pcapng out.pcap"); + return; + default: + d.fail(pcap_path.string() + " is not a pcap file"); + return; + } + d.linktype = d.u32(header + 20) & 0x0FFFFFFF; // upper bits: FCS length flags + switch(d.linktype) + { + case kLinkNull: + case kLinkEthernet: + case kLinkRaw: + case kLinkLoop: + case kLinkLinuxSll: + case kLinkIpv4: + case kLinkLinuxSll2: + break; + default: + d.fail("unsupported pcap link type " + std::to_string(d.linktype)); + return; + } + + d.open = true; +} + +OusterPcapReader::~OusterPcapReader() = default; + +bool OusterPcapReader::isOpen() const +{ + return impl_->open; +} + +const std::string& OusterPcapReader::error() const +{ + return impl_->error; +} + +void OusterPcapReader::requestStop() +{ + impl_->stop = true; +} + +const OusterPcapStats& OusterPcapReader::stats() const +{ + return impl_->stats; +} + +bool OusterPcapReader::read(const Callbacks& callbacks) +{ + Impl& d = *impl_; + if(!d.open) + return false; + d.onPoints = callbacks.onPoints; + d.onImu = callbacks.onImu; + + uint8_t record_header[16]; + while(!d.stop) + { + if(!d.in.read(reinterpret_cast(record_header), sizeof(record_header))) + break; // end of file; a torn final header (capture killed mid-write) ends it too + const uint32_t captured = d.u32(record_header + 8); + if(captured > kMaxRecordBytes) + return d.fail("corrupt pcap record #" + std::to_string(d.stats.records + 1) + ": " + std::to_string(captured) + " bytes"); + d.record.resize(captured); + if(!d.in.read(reinterpret_cast(d.record.data()), captured)) + break; // torn final record + ++d.stats.records; + d.handleFrame(d.record.data(), d.record.size()); + } + for(const auto& [key, datagram] : d.pending) + d.stats.dropped_fragments += datagram.parts.size(); + d.pending.clear(); + return true; +} + +} // namespace rosbags diff --git a/rosbags/OusterPcapReader.h b/rosbags/OusterPcapReader.h new file mode 100644 index 00000000..b4194e8a --- /dev/null +++ b/rosbags/OusterPcapReader.h @@ -0,0 +1,140 @@ +#pragma once +#include "McapWriter.h" // McapPoint, McapImuSample -- the structs MandeyeSessionWriter consumes +#include +#include +#include +#include +#include +#include +#include + +namespace rosbags +{ + +// Ouster sensor description, parsed from the metadata JSON the Ouster SDK / +// Ouster Studio / the sensor's HTTP API (`/api/v1/sensor/metadata`) write next +// to a recording. Only the non-legacy layout (FW 2.4+: config_params, +// lidar_data_format, beam_intrinsics, ...) is understood. +struct OusterMetadata +{ + std::string prod_sn; + std::string prod_line; + std::string firmware; + uint64_t initialization_id = 0; + bool has_initialization_id = false; + + uint16_t udp_port_lidar = 7502; + uint16_t udp_port_imu = 7503; + std::string udp_profile_lidar; + std::string udp_profile_imu; + std::string header_type; + + int pixels_per_column = 0; + int columns_per_packet = 0; + int columns_per_frame = 0; + + // Channel data block layout: bytes per pixel and where the first-return range + // (mm) and reflectivity live inside it. From `pixel_data_format` when the + // metadata carries one (FW 4.0 does, including pre-release profiles no SDK + // knows), otherwise from a table of known profile names. + int pixel_bytes = 0; + int range_start_bit = 0; + int range_width = 0; + int reflectivity_start_bit = 0; + int reflectivity_width = 0; // 0 = profile has no reflectivity + std::array rgb_start_bit{}; // red, green, blue + int rgb_width = 0; // 0 = profile has no colour + + std::vector beam_altitude_angles_deg; + std::vector beam_azimuth_angles_deg; + std::array beam_to_lidar_transform{}; // row-major 4x4, mm + double lidar_origin_to_beam_origin_mm = 0.0; + std::array lidar_to_sensor_transform{}; // row-major 4x4, mm + std::array imu_to_sensor_transform{}; // row-major 4x4, mm + + // Bytes in one lidar UDP payload: packet header + columns + packet footer. + size_t lidarPacketSize() const; +}; + +// Parses `json_text`. On failure returns false and fills `error`. +bool parseOusterMetadata(const std::string& json_text, OusterMetadata& out, std::string& error); + +struct OusterPcapReaderOptions +{ + // 0 = take the port from the metadata. + uint16_t lidar_port = 0; + uint16_t imu_port = 0; + + // Keep every Nth measurement column (1 = all). Thins the cloud horizontally + // without touching the vertical beam pattern. + int column_step = 1; +}; + +struct OusterPcapStats +{ + uint64_t records = 0; + uint64_t lidar_packets = 0; + uint64_t imu_packets = 0; + uint64_t other_packets = 0; // non-UDP, non-IPv4, or a port that is neither lidar nor IMU + uint64_t bad_size_packets = 0; // on the lidar/IMU port but the wrong length for the metadata's profile + uint64_t foreign_packets = 0; // header init_id / serial number does not match the metadata + uint64_t invalid_columns = 0; // column status bit 0 clear (no data, e.g. outside the azimuth window) + uint64_t reassembled_datagrams = 0; + uint64_t dropped_fragments = 0; // IPv4 fragments of a datagram that never completed + double first_lidar_timestamp = 0.0; + double last_lidar_timestamp = 0.0; +}; + +// Streams lidar points and IMU samples out of a classic libpcap capture of an +// Ouster sensor (tcpdump, Wireshark saved as "pcap", ouster-cli / Ouster SDK +// recordings). Ethernet / Linux cooked (SLL, SLL2) / raw-IP link types; IPv4 +// only; fragmented UDP datagrams (a 128-beam lidar packet is ~20-25 kB, far +// above a 1500-byte MTU) are reassembled. +// +// Points come out in the SENSOR frame (lidar_to_sensor_transform applied) in +// metres, each stamped with its column's timestamp in seconds on the sensor +// clock. Zero-range (no return) pixels are dropped. Intensity carries the 8-bit +// calibrated reflectivity, the scale mandeye's own LAZ files use. On an RGB +// profile each point's colour comes alongside, as the sensor's raw 16-bit words. +// +// IMU samples (LEGACY IMU profile only) are rotated into the sensor frame, +// gyro in rad/s, acceleration in g -- the units mandeye's imuNNNN.csv uses, +// NOT the m/s^2 McapImuSample documents for bags; MandeyeSessionWriter writes +// the values through unchanged. +class OusterPcapReader +{ +public: + struct Callbacks + { + // One lidar packet's worth of points, in column (= time) order. `rgb` is + // parallel to `points` (raw red, green, blue words) on an RGB profile and + // empty otherwise. + std::function&& points, std::vector>&& rgb)> onPoints; + std::function onImu; + }; + + OusterPcapReader(const std::filesystem::path& pcap_path, const OusterMetadata& metadata, const OusterPcapReaderOptions& options = {}); + ~OusterPcapReader(); + + OusterPcapReader(const OusterPcapReader&) = delete; + OusterPcapReader& operator=(const OusterPcapReader&) = delete; + + bool isOpen() const; + const std::string& error() const; + + // Reads the whole capture front to back. Returns false on an I/O or format + // error; packets it cannot use are counted in stats(), not treated as errors. + bool read(const Callbacks& callbacks); + + // Makes read() return after the packet being processed -- callable from a + // callback to end the read early. + void requestStop(); + + const OusterPcapStats& stats() const; + +private: + struct Impl; + std::unique_ptr impl_; +}; + +} // namespace rosbags diff --git a/rosbags/tests/CMakeLists.txt b/rosbags/tests/CMakeLists.txt index d0ac8dff..cdf3a1c5 100644 --- a/rosbags/tests/CMakeLists.txt +++ b/rosbags/tests/CMakeLists.txt @@ -3,24 +3,28 @@ cmake_minimum_required(VERSION 4.0.0) project(hdmapping_rosbags_tests) # Unit tests for rosbags/ (LAZ/IMU <-> MCAP/ROS2-bag support code, both -# directions). Uses doctest, same as shared/tests -- see that CMakeLists.txt for +# directions, and the Ouster pcap reader). Uses doctest, same as shared/tests -- see that CMakeLists.txt for # why. Only McapWriter.cpp defines MCAP_IMPLEMENTATION; McapReader.cpp relies on # it being linked in alongside. add_executable(hdmapping_rosbags_tests test_mcap_writer.cpp test_mcap_reader.cpp + test_ouster_pcap_reader.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../McapWriter.h ${CMAKE_CURRENT_SOURCE_DIR}/../McapWriter.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../McapReader.h ${CMAKE_CURRENT_SOURCE_DIR}/../McapReader.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../MandeyeSessionWriter.h ${CMAKE_CURRENT_SOURCE_DIR}/../MandeyeSessionWriter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../OusterPcapReader.h + ${CMAKE_CURRENT_SOURCE_DIR}/../OusterPcapReader.cpp ) target_include_directories(hdmapping_rosbags_tests PRIVATE ${THIRDPARTY_DIRECTORY}/doctest ${CMAKE_CURRENT_SOURCE_DIR}/.. ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/json/include ) target_link_libraries(hdmapping_rosbags_tests PRIVATE mcap ${PLATFORM_LASZIP_LIB}) diff --git a/rosbags/tests/test_mcap_reader.cpp b/rosbags/tests/test_mcap_reader.cpp index e2efc861..773af5eb 100644 --- a/rosbags/tests/test_mcap_reader.cpp +++ b/rosbags/tests/test_mcap_reader.cpp @@ -79,13 +79,16 @@ std::vector makeImuSamples(size_t n, double t0) // One point as stored in a LAZ file, decoded exactly the way // load_point_cloud() (apps/lidar_odometry_step_1/lidar_odometry_utils.cpp) does: // coordinates from the header scale/offset, timestamp straight out of gps_time, -// lidar id out of user_data. +// lidar id out of user_data. point_format and rgb are not read by +// load_point_cloud(); they are here to check what the writer stores. struct LazPoint { double x, y, z; double gps_time; uint16_t intensity; uint8_t user_data; + uint8_t point_format; + uint16_t rgb[3]; }; std::vector readLaz(const fs::path& path) @@ -115,6 +118,9 @@ std::vector readLaz(const fs::path& path) p.gps_time = point->gps_time; p.intensity = point->intensity; p.user_data = point->user_data; + p.point_format = header->point_data_format; + for (int k = 0; k < 3; ++k) + p.rgb[k] = point->rgb[k]; out.push_back(p); } @@ -363,6 +369,51 @@ TEST_CASE("MandeyeSessionWriter: LAZ stores unscaled gps_time, zero lidar id and // with no calibration entry would make it drop the points. CHECK(decoded[i].user_data == 0); CHECK(decoded[i].intensity == static_cast(points[i].intensity)); + CHECK(decoded[i].point_format == 1); + } + + fs::remove_all(dir); +} + +TEST_CASE("MandeyeSessionWriter: with_rgb writes point format 3 with 16-bit colour") +{ + const auto dir = tempPath("hdmapping_session_laz_rgb"); + fs::remove_all(dir); + + const auto points = makeReaderPoints(50, 1234.5); + std::vector rgb; + for (size_t i = 0; i < points.size(); ++i) + rgb.push_back({static_cast(i * 1000), static_cast(65535 - i), static_cast(i)}); + { + rosbags::MandeyeSessionWriter writer(dir, /*with_rgb=*/true); + REQUIRE_MESSAGE(writer.isOpen(), writer.error()); + REQUIRE(writer.beginChunk(0)); + REQUIRE(writer.addPoints(points, rgb)); + REQUIRE(writer.addPoints(points)); // no colours given: black + CHECK_FALSE(writer.addPoints(points, std::vector(rgb.begin(), rgb.begin() + 3))); // not one colour per point + REQUIRE(writer.endChunk()); + } + + const auto decoded = readLaz(dir / "lidar0000.laz"); + REQUIRE(decoded.size() == 2 * points.size()); + for (size_t i = 0; i < points.size(); ++i) + { + CHECK(decoded[i].point_format == 3); + CHECK(decoded[i].gps_time == points[i].timestamp); + CHECK(decoded[i].intensity == static_cast(points[i].intensity)); + for (int k = 0; k < 3; ++k) + { + CHECK(decoded[i].rgb[k] == rgb[i][k]); + CHECK(decoded[points.size() + i].rgb[k] == 0); + } + } + + SUBCASE("colours on a writer opened without with_rgb are refused") + { + rosbags::MandeyeSessionWriter writer(dir); + REQUIRE(writer.beginChunk(1)); + CHECK_FALSE(writer.addPoints(points, rgb)); + CHECK(writer.error().find("with_rgb") != std::string::npos); } fs::remove_all(dir); diff --git a/rosbags/tests/test_ouster_pcap_reader.cpp b/rosbags/tests/test_ouster_pcap_reader.cpp new file mode 100644 index 00000000..bcbeb59e --- /dev/null +++ b/rosbags/tests/test_ouster_pcap_reader.cpp @@ -0,0 +1,416 @@ +// See rosbags/McapWriter.h for why this needs to come before any standard +// header (including , which pulls in plenty of its own) gets a +// chance to lock in the project-wide _HAS_STD_BYTE=0 MSVC workaround. +#if defined(_MSC_VER) +# undef _HAS_STD_BYTE +# define _HAS_STD_BYTE 1 +#endif + +// No DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN here: test_mcap_writer.cpp (linked into +// the same binary) provides doctest's main once. +#include + +#include "OusterPcapReader.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fs = std::filesystem; + +namespace +{ + +constexpr uint64_t kInitId = 12345; +constexpr uint64_t kSerial = 992612000030; + +// A 4-beam sensor, 4 columns per frame, 2 columns per packet. Beams look +// straight out at 0, 0, +30 and -30 degrees with no beam offset, so the SDK's +// XYZ model reduces to plain spherical coordinates: measurement id 0 points +// along +X, id 1 along -Y. The lidar sits 10 mm above the sensor origin; the +// IMU is turned 90 degrees about Z. +std::string metadataJson(const std::string& profile, const std::string& pixel_data_format = "") +{ + std::string json = R"({ + "sensor_info": {"prod_line": "OS-TEST", "prod_sn": ")" + + std::to_string(kSerial) + R"(", "image_rev": "test", "initialization_id": )" + std::to_string(kInitId) + R"(}, + "config_params": {"udp_port_lidar": 7502, "udp_port_imu": 7503, "udp_profile_lidar": ")" + + profile + R"(", "udp_profile_imu": "LEGACY"}, + "lidar_data_format": {"pixels_per_column": 4, "columns_per_packet": 2, "columns_per_frame": 4, + "udp_profile_lidar": ")" + + profile + R"(", "header_type": "STANDARD"}, + "beam_intrinsics": {"beam_altitude_angles": [0, 0, 30, -30], "beam_azimuth_angles": [0, 0, 0, 0], + "beam_to_lidar_transform": [1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1], + "lidar_origin_to_beam_origin_mm": 0}, + "lidar_intrinsics": {"lidar_to_sensor_transform": [1,0,0,0, 0,1,0,0, 0,0,1,10, 0,0,0,1]}, + "imu_intrinsics": {"imu_to_sensor_transform": [0,-1,0,0, 1,0,0,0, 0,0,1,0, 0,0,0,1]})"; + if (!pixel_data_format.empty()) + json += R"(, "pixel_data_format": )" + pixel_data_format; + return json + "}"; +} + +rosbags::OusterMetadata parseOrFail(const std::string& json) +{ + rosbags::OusterMetadata m; + std::string error; + const bool ok = rosbags::parseOusterMetadata(json, m, error); + INFO(error); + REQUIRE(ok); + return m; +} + +void putLe(std::vector& out, size_t offset, uint64_t value, int bytes) +{ + for (int i = 0; i < bytes; ++i) + out[offset + i] = static_cast(value >> (8 * i)); +} + +void putBe16(std::vector& out, size_t offset, uint16_t value) +{ + out[offset] = static_cast(value >> 8); + out[offset + 1] = static_cast(value); +} + +struct Column +{ + uint64_t timestamp_ns; + uint16_t measurement_id; + bool valid; + uint32_t range_mm[4]; + uint8_t reflectivity[4]; + uint16_t rgb[4][3]; // only written into 16-byte (RGB16) pixels +}; + +// One lidar packet for the sensor above: RNG19_RFL8_SIG16_NIR16 (12-byte pixels) +// or, with pixel_bytes = 16, RNG19_RFL8_SIG16_NIR16_RGB16. +std::vector lidarPacket(const Column (&columns)[2], uint64_t init_id = kInitId, size_t pixel_bytes = 12) +{ + const size_t kPixel = pixel_bytes; + const size_t kColumn = 12 + 4 * kPixel; + std::vector p(32 + 2 * kColumn + 32, 0); + putLe(p, 0, 1, 2); // packet type + putLe(p, 4, init_id, 3); + putLe(p, 7, kSerial, 5); + for (size_t c = 0; c < 2; ++c) + { + const size_t col = 32 + c * kColumn; + putLe(p, col, columns[c].timestamp_ns, 8); + putLe(p, col + 8, columns[c].measurement_id, 2); + putLe(p, col + 10, columns[c].valid ? 1 : 0, 2); + for (size_t u = 0; u < 4; ++u) + { + const size_t px = col + 12 + u * kPixel; + // Bit 19 set on purpose: reserved/bloom bits must not leak into the range. + putLe(p, px, columns[c].range_mm[u] | (1u << 19) | (uint32_t{columns[c].reflectivity[u]} << 24), 4); + putLe(p, px + 4, 0xBEEF, 2); // signal + if (pixel_bytes >= 14) + { + for (size_t k = 0; k < 3; ++k) + putLe(p, px + 8 + 2 * k, columns[c].rgb[u][k], 2); + } + } + } + return p; +} + +std::vector legacyImuPacket(uint64_t gyro_ts_ns, const float (&acc_g)[3], const float (&gyro_dps)[3]) +{ + std::vector p(48, 0); + putLe(p, 0, 1, 8); // system timestamp: a different clock, must be ignored + putLe(p, 8, gyro_ts_ns - 1000, 8); + putLe(p, 16, gyro_ts_ns, 8); + for (int i = 0; i < 3; ++i) + { + std::memcpy(&p[24 + 4 * i], &acc_g[i], 4); + std::memcpy(&p[36 + 4 * i], &gyro_dps[i], 4); + } + return p; +} + +// Wraps a UDP payload in Ethernet/IPv4/UDP, split into IPv4 fragments carrying at +// most `fragment_bytes` (a multiple of 8) of IP payload each; 0 = unfragmented. +std::vector> udpFrames(const std::vector& payload, uint16_t dport, uint16_t ip_id, size_t fragment_bytes = 0) +{ + std::vector datagram(8 + payload.size()); + putBe16(datagram, 0, 7502); + putBe16(datagram, 2, dport); + putBe16(datagram, 4, static_cast(datagram.size())); + std::copy(payload.begin(), payload.end(), datagram.begin() + 8); + + if (fragment_bytes == 0) + fragment_bytes = datagram.size(); + std::vector> frames; + for (size_t offset = 0; offset < datagram.size(); offset += fragment_bytes) + { + const size_t n = std::min(fragment_bytes, datagram.size() - offset); + const bool more = offset + n < datagram.size(); + std::vector f(14 + 20 + n, 0); + putBe16(f, 12, 0x0800); + f[14] = 0x45; + putBe16(f, 16, static_cast(20 + n)); + putBe16(f, 18, ip_id); + putBe16(f, 20, static_cast((more ? 0x2000 : 0) | (offset / 8))); + f[22] = 64; + f[23] = 17; + const uint8_t src[4] = {169, 254, 0, 2}; + const uint8_t dst[4] = {169, 254, 0, 1}; + std::memcpy(&f[26], src, 4); + std::memcpy(&f[30], dst, 4); + std::copy(datagram.begin() + offset, datagram.begin() + offset + n, f.begin() + 34); + frames.push_back(std::move(f)); + } + return frames; +} + +void writePcap(const fs::path& path, const std::vector>& frames) +{ + std::vector out(24, 0); + putLe(out, 0, 0xA1B2C3D4, 4); + putLe(out, 4, 2, 2); + putLe(out, 6, 4, 2); + putLe(out, 16, 65535, 4); + putLe(out, 20, 1, 4); // Ethernet + for (size_t i = 0; i < frames.size(); ++i) + { + std::vector record(16, 0); + putLe(record, 0, 1000 + i, 4); + putLe(record, 8, frames[i].size(), 4); + putLe(record, 12, frames[i].size(), 4); + out.insert(out.end(), record.begin(), record.end()); + out.insert(out.end(), frames[i].begin(), frames[i].end()); + } + std::ofstream(path, std::ios::binary).write(reinterpret_cast(out.data()), static_cast(out.size())); +} + +struct ReadResult +{ + std::vector points; + std::vector> rgb; + std::vector imu; + rosbags::OusterPcapStats stats; +}; + +ReadResult readAll(const fs::path& path, const rosbags::OusterMetadata& meta, const rosbags::OusterPcapReaderOptions& options = {}) +{ + rosbags::OusterPcapReader reader(path, meta, options); + INFO(reader.error()); + REQUIRE(reader.isOpen()); + ReadResult r; + rosbags::OusterPcapReader::Callbacks callbacks; + callbacks.onPoints = [&](std::vector&& points, std::vector>&& rgb) + { + if (!rgb.empty()) + REQUIRE(rgb.size() == points.size()); + r.points.insert(r.points.end(), points.begin(), points.end()); + r.rgb.insert(r.rgb.end(), rgb.begin(), rgb.end()); + }; + callbacks.onImu = [&](const rosbags::McapImuSample& s) + { + r.imu.push_back(s); + }; + REQUIRE(reader.read(callbacks)); + r.stats = reader.stats(); + return r; +} + +const Column kColumns[2] = { + {5'000'000'000, 0, true, {1000, 0, 2000, 3000}, {10, 0, 30, 40}, {{100, 200, 300}, {1, 2, 3}, {400, 500, 600}, {0xFFFF, 0xFFFF, 0xFFFF}}}, + {5'000'100'000, 1, true, {0, 2000, 0, 0}, {0, 20, 0, 0}, {{0, 0, 0}, {7, 8, 9}, {0, 0, 0}, {0, 0, 0}}}, +}; + +} // namespace + +TEST_CASE("parseOusterMetadata: profile table, pixel_data_format, and refusals") +{ + SUBCASE("a known profile name gives the RNG19 word-0 layout") + { + const auto m = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16")); + CHECK(m.pixel_bytes == 12); + CHECK(m.range_start_bit == 0); + CHECK(m.range_width == 19); + CHECK(m.reflectivity_start_bit == 24); + CHECK(m.reflectivity_width == 8); + CHECK(m.lidarPacketSize() == 32 + 2 * (12 + 4 * 12) + 32); + CHECK(m.initialization_id == kInitId); + CHECK(m.rgb_width == 0); + } + SUBCASE("the RGB16 profile name brings red, green, blue in words 2 and 3") + { + const auto m = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16_RGB16")); + CHECK(m.pixel_bytes == 16); + CHECK(m.rgb_width == 16); + CHECK(m.rgb_start_bit == std::array{64, 80, 96}); + } + SUBCASE("pixel_data_format describes a profile no table knows") + { + const auto m = parseOrFail(metadataJson( + "SOME_FUTURE_PROFILE", + R"({"pixel_bytes": 20, "fields": [ + {"field_name": "range_capped_mm_0", "start_bit": 0, "width": 19, "type": "uint"}, + {"field_name": "bloom_0", "start_bit": 19, "width": 1, "type": "bool"}, + {"field_name": "reflectivity_0", "start_bit": 24, "width": 8, "type": "uint"}, + {"field_name": "range_capped_mm_1", "start_bit": 32, "width": 19, "type": "uint"}, + {"field_name": "rgb_r1", "start_bit": 64, "width": 16, "type": "uint"}, + {"field_name": "rgb_r", "start_bit": 112, "width": 16, "type": "uint"}, + {"field_name": "rgb_g", "start_bit": 128, "width": 16, "type": "uint"}, + {"field_name": "rgb_b", "start_bit": 144, "width": 16, "type": "uint"}]})")); + CHECK(m.pixel_bytes == 20); + CHECK(m.range_start_bit == 0); // the first return, not range_capped_mm_1 + CHECK(m.reflectivity_start_bit == 24); + CHECK(m.rgb_width == 16); + CHECK(m.rgb_start_bit == std::array{112, 128, 144}); // rgb_r, not rgb_r1 + } + SUBCASE("an unknown profile without pixel_data_format is refused by name") + { + rosbags::OusterMetadata m; + std::string error; + CHECK_FALSE(rosbags::parseOusterMetadata(metadataJson("SOME_FUTURE_PROFILE"), m, error)); + CHECK(error.find("SOME_FUTURE_PROFILE") != std::string::npos); + } + SUBCASE("LEGACY lidar packets are refused") + { + rosbags::OusterMetadata m; + std::string error; + CHECK_FALSE(rosbags::parseOusterMetadata(metadataJson("LEGACY"), m, error)); + } +} + +TEST_CASE("OusterPcapReader: points follow the SDK XYZ model, from fragmented datagrams in any order") +{ + const auto meta = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16")); + const fs::path path = fs::temp_directory_path() / "hdmapping_ouster_reader_points.pcap"; + + auto frames = udpFrames(lidarPacket(kColumns), 7502, 1, 64); + REQUIRE(frames.size() > 2); + std::swap(frames.front(), frames.back()); // last fragment first + writePcap(path, frames); + + const auto r = readAll(path, meta); + CHECK(r.rgb.empty()); // no colour in this profile + CHECK(r.stats.reassembled_datagrams == 1); + CHECK(r.stats.dropped_fragments == 0); + CHECK(r.stats.lidar_packets == 1); + REQUIRE(r.points.size() == 4); // zero ranges dropped + + const double s30 = std::sin(std::numbers::pi / 6), c30 = std::cos(std::numbers::pi / 6); + struct Expected + { + double x, y, z; + float intensity; + uint16_t ring; + double t; + }; + const Expected expected[4] = { + {1.0, 0.0, 0.010, 10, 0, 5.0}, // id 0, level beam: +X, lifted by lidar_to_sensor + {2.0 * c30, 0.0, 2.0 * s30 + 0.010, 30, 2, 5.0}, + {3.0 * c30, 0.0, -3.0 * s30 + 0.010, 40, 3, 5.0}, + {0.0, -2.0, 0.010, 20, 1, 5.0001}, // id 1 of 4: a quarter turn clockwise, -Y + }; + for (size_t i = 0; i < 4; ++i) + { + CAPTURE(i); + CHECK(r.points[i].x == doctest::Approx(expected[i].x).epsilon(1e-6)); + CHECK(r.points[i].y == doctest::Approx(expected[i].y).epsilon(1e-6)); + CHECK(r.points[i].z == doctest::Approx(expected[i].z).epsilon(1e-6)); + CHECK(r.points[i].intensity == expected[i].intensity); + CHECK(r.points[i].ring == expected[i].ring); + CHECK(r.points[i].timestamp == doctest::Approx(expected[i].t).epsilon(1e-12)); + } + + SUBCASE("column_step keeps only every Nth measurement id") + { + rosbags::OusterPcapReaderOptions options; + options.column_step = 2; + const auto thinned = readAll(path, meta, options); + REQUIRE(thinned.points.size() == 3); + for (const auto& p : thinned.points) + CHECK(p.timestamp == doctest::Approx(5.0)); + } + fs::remove(path); +} + +TEST_CASE("OusterPcapReader: an RGB profile hands each point's colour words alongside it") +{ + const auto meta = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16_RGB16")); + const fs::path path = fs::temp_directory_path() / "hdmapping_ouster_reader_rgb.pcap"; + writePcap(path, udpFrames(lidarPacket(kColumns, kInitId, 16), 7502, 1)); + + const auto r = readAll(path, meta); + REQUIRE(r.points.size() == 4); + REQUIRE(r.rgb.size() == 4); + // Same point order as the 12-byte profile; zero-range pixels drop their colour too. + const std::array expected[4] = {{100, 200, 300}, {400, 500, 600}, {0xFFFF, 0xFFFF, 0xFFFF}, {7, 8, 9}}; + for (size_t i = 0; i < 4; ++i) + { + CAPTURE(i); + CHECK(r.rgb[i] == expected[i]); + } + fs::remove(path); +} + +TEST_CASE("OusterPcapReader: LEGACY IMU is rotated into the sensor frame, deg/s -> rad/s, g kept") +{ + const auto meta = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16")); + const fs::path path = fs::temp_directory_path() / "hdmapping_ouster_reader_imu.pcap"; + writePcap(path, udpFrames(legacyImuPacket(7'000'000'000, {1.0f, 0.0f, 0.0f}, {90.0f, 0.0f, 0.0f}), 7503, 2)); + + const auto r = readAll(path, meta); + REQUIRE(r.imu.size() == 1); + const auto& s = r.imu.front(); + CHECK(s.timestamp == doctest::Approx(7.0)); // gyro read time, not the system timestamp + CHECK(s.acc_x == doctest::Approx(0.0)); + CHECK(s.acc_y == doctest::Approx(1.0)); // +X of the IMU is +Y of the sensor + CHECK(s.acc_z == doctest::Approx(0.0)); + CHECK(s.gyro_x == doctest::Approx(0.0)); + CHECK(s.gyro_y == doctest::Approx(std::numbers::pi / 2)); + CHECK(s.gyro_z == doctest::Approx(0.0)); + fs::remove(path); +} + +TEST_CASE("OusterPcapReader: foreign, mis-sized and unrelated packets are counted, not decoded") +{ + const auto meta = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16")); + const fs::path path = fs::temp_directory_path() / "hdmapping_ouster_reader_skips.pcap"; + + std::vector> frames; + auto add = [&](std::vector> more) + { + frames.insert(frames.end(), more.begin(), more.end()); + }; + add(udpFrames(lidarPacket(kColumns, kInitId + 1), 7502, 1)); // sensor re-initialized since the metadata + auto truncated = lidarPacket(kColumns); + truncated.resize(truncated.size() - 4); + add(udpFrames(truncated, 7502, 2)); + add(udpFrames(std::vector(16, 0), 9999, 3)); + add({udpFrames(lidarPacket(kColumns), 7502, 4, 64).front()}); // a fragment whose siblings never arrive + writePcap(path, frames); + + const auto r = readAll(path, meta); + CHECK(r.points.empty()); + CHECK(r.stats.foreign_packets == 1); + CHECK(r.stats.bad_size_packets == 1); + CHECK(r.stats.other_packets == 1); + CHECK(r.stats.dropped_fragments == 1); + CHECK(r.stats.lidar_packets == 0); + fs::remove(path); +} + +TEST_CASE("OusterPcapReader: pcapng is refused with a conversion hint") +{ + const auto meta = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16")); + const fs::path path = fs::temp_directory_path() / "hdmapping_ouster_reader.pcapng"; + std::vector block(28, 0); + putLe(block, 0, 0x0A0D0D0A, 4); + std::ofstream(path, std::ios::binary).write(reinterpret_cast(block.data()), static_cast(block.size())); + + rosbags::OusterPcapReader reader(path, meta); + CHECK_FALSE(reader.isOpen()); + CHECK(reader.error().find("editcap") != std::string::npos); + fs::remove(path); +} From 77207cd66e6f0158e1281343d4bb958555754794 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 12 Oct 2026 19:01:33 +0200 Subject: [PATCH 2/4] Imu only mode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- ..._data_and_drop_here-precision_forestry.cpp | 11 +++++ .../lidar_odometry_gui.cpp | 11 +++++ .../lidar_odometry_utils.h | 3 ++ .../lidar_odometry_utils_optimizers.cpp | 44 +++++++++++++++++++ apps/lidar_odometry_step_1/toml_io.h | 2 + pybind/lidar_odometry_binding.cpp | 1 + 6 files changed, 72 insertions(+) diff --git a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp index f40a1bdd..4bdf4019 100644 --- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp +++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp @@ -746,6 +746,11 @@ void settings_gui() ImGui::NewLine(); ImGui::Checkbox("Use motion from previous step", ¶ms.use_motion_from_previous_step); + ImGui::Checkbox("Lidar-only odometry (no IMU)", ¶ms.lidar_only_odometry); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Predict each window's rotation and translation from the previous one (constant velocity) instead of from " + "the IMU. For recordings without a usable IMU, e.g. a synthetic stationary one."); if (params.use_vqf) { ImGui::InputDouble("VQF tauAcc [s]", ¶ms.vqf_tauAcc, 0.0, 0.0, "%.3f"); @@ -1920,6 +1925,12 @@ void display() { params.ablation_study_use_anisotropic_weighting = !params.ablation_study_use_anisotropic_weighting; } + ImGui::MenuItem("5: No IMU, lidar-only odometry (combine with 1-3)", nullptr, ¶ms.lidar_only_odometry); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "For recordings without IMU data, written with a synthetic stationary IMU (e.g. ouster_pcap_to_laz " + "--stationary-imu): each window's rotation and translation are predicted from the previous window instead of " + "from the IMU."); ImGui::EndMenu(); } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 03099c22..03bb2aff 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -961,6 +961,11 @@ void settings_gui() ImGui::NewLine(); ImGui::Checkbox("Use motion from previous step", ¶ms.use_motion_from_previous_step); + ImGui::Checkbox("Lidar-only odometry (no IMU)", ¶ms.lidar_only_odometry); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Predict each window's rotation and translation from the previous one (constant velocity) instead of from " + "the IMU. For recordings without a usable IMU, e.g. a synthetic stationary one."); ImGui::Checkbox("Use IMU preintegration", ¶ms.use_imu_preintegration); if (params.use_imu_preintegration) { @@ -2326,6 +2331,12 @@ void display() { params.ablation_study_use_anisotropic_weighting = !params.ablation_study_use_anisotropic_weighting; } + ImGui::MenuItem("5: No IMU, lidar-only odometry (combine with 1-3)", nullptr, ¶ms.lidar_only_odometry); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "For recordings without IMU data, written with a synthetic stationary IMU (e.g. ouster_pcap_to_laz " + "--stationary-imu): each window's rotation and translation are predicted from the previous window instead of " + "from the IMU."); ImGui::EndMenu(); } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 72cbdcff..0a920ecb 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -124,6 +124,9 @@ struct LidarOdometryParams // lidar odometry control bool use_motion_from_previous_step = true; + // No usable IMU: predict each window's rotation and translation from the + // previous window (constant velocity) instead of from the IMU orientation. + bool lidar_only_odometry = false; int nr_iter = 100; NDT::GridParameters in_out_params_indoor; NDT::GridParameters in_out_params_outdoor; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index 7d4f5d98..cc50aec2 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1728,6 +1728,50 @@ bool process_worker_step_1( } } + if (i > 1 && params.lidar_only_odometry) + { + // No IMU to follow (e.g. a synthetic stationary one): carry the previous + // window's motion -- rotation as well as translation, both in its body + // frame -- forward at constant velocity. The prediction is also the + // motion-model target, so the optimizer expects the current turn rate to + // continue rather than no rotation at all. Assumes evenly spaced nodes, as + // a synthetic IMU stream provides. + const auto& prev = prev_worker_data.intermediate_trajectory; + const Eigen::Affine3d delta = prev.front().inverse() * prev.back(); + const Eigen::AngleAxisd rotation(delta.linear()); + const double steps = static_cast(prev.size() - 1); + + Eigen::Affine3d step = Eigen::Affine3d::Identity(); + if (delta.translation().norm() > 1.0 || rotation.angle() > 30.0 * DEG_TO_RAD) + { + spdlog::warn( + "lidar_only_odometry: previous window moved {:.2f} m / {:.1f} deg, predicting no motion instead", + delta.translation().norm(), + rotation.angle() * RAD_TO_DEG); + } + else if (steps > 0) + { + step.linear() = Eigen::AngleAxisd(rotation.angle() / steps, rotation.axis()).toRotationMatrix(); + step.translation() = delta.translation() / steps; + } + + std::vector new_trajectory; + new_trajectory.reserve(worker_data.intermediate_trajectory.size()); + Eigen::Affine3d current_node = prev.back(); + for (size_t tr = 0; tr < worker_data.intermediate_trajectory.size(); tr++) + { + if (tr > 0) + current_node = current_node * step; + new_trajectory.push_back(current_node); + worker_data.intermediate_trajectory_prediction.push_back(current_node); + } + + worker_data.imu_prediction_vector = new_trajectory.back().translation() - new_trajectory.front().translation(); + worker_data.intermediate_trajectory = new_trajectory; + worker_data.intermediate_trajectory_motion_model = new_trajectory; + return true; + } + Eigen::Vector3d mean_shift(0.0, 0.0, 0.0); if (i > 1 && params.use_motion_from_previous_step) { diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index 5d677582..1002562a 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -68,6 +68,7 @@ class TomlIO { "vqf_magMaxRejectionTime", &LidarOdometryParams::vqf_magMaxRejectionTime }, { "vqf_magRejectionFactor", &LidarOdometryParams::vqf_magRejectionFactor }, { "use_motion_from_previous_step", &LidarOdometryParams::use_motion_from_previous_step }, + { "lidar_only_odometry", &LidarOdometryParams::lidar_only_odometry }, { "nr_iter", &LidarOdometryParams::nr_iter }, { "sliding_window_trajectory_length_threshold", &LidarOdometryParams::sliding_window_trajectory_length_threshold }, { "max_distance_lidar", &LidarOdometryParams::max_distance_lidar }, @@ -166,6 +167,7 @@ class TomlIO "vqf_magRejectionFactor" } }, { "lidar_odometry_control", { "use_motion_from_previous_step", + "lidar_only_odometry", "nr_iter", "sliding_window_trajectory_length_threshold", "max_distance_lidar", diff --git a/pybind/lidar_odometry_binding.cpp b/pybind/lidar_odometry_binding.cpp index de880427..0a398ee4 100644 --- a/pybind/lidar_odometry_binding.cpp +++ b/pybind/lidar_odometry_binding.cpp @@ -63,6 +63,7 @@ PYBIND11_MODULE(lidar_odometry_py, m) .def_readwrite("buckets_indoor", &LidarOdometryParams::buckets_indoor) .def_readwrite("buckets_outdoor", &LidarOdometryParams::buckets_outdoor) .def_readwrite("use_motion_from_previous_step", &LidarOdometryParams::use_motion_from_previous_step) + .def_readwrite("lidar_only_odometry", &LidarOdometryParams::lidar_only_odometry) .def_readwrite("consecutive_distance", &LidarOdometryParams::consecutive_distance) .def_readwrite("nr_iter", &LidarOdometryParams::nr_iter) .def_readwrite("useMultithread", &LidarOdometryParams::useMultithread) From 4610a7ecdd7531287dcae4febdc17b23a1fcc0ab Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 13 Oct 2026 11:43:14 +0200 Subject: [PATCH 3/4] Trajectory viewer: color points from LAZ RGB calib::PointCloud reads LAS rgb (16-bit high byte, or unscaled when a file stores 8-bit values; all-zero rgb counts as no colour). The viewer uses it for points no camera image colours, flagging them in the top byte of colorPacked so the RGB render mode keeps them, and enables RGB mode for sessions with LAZ colour but no images. Co-Authored-By: Claude Opus 5.5 --- .../TrajectoryViewer.cpp | 39 +++++-- .../TrajectoryViewerShaders.h | 11 +- apps/console_tools/ouster_pcap_to_laz.cpp | 51 +++++++-- apps/lidar_odometry_step_1/lidar_odometry.cpp | 15 ++- .../lidar_odometry_utils.cpp | 9 ++ calib_core/include/CalibCore/PointCloud.h | 2 + calib_core/src/PointCloud.cpp | 26 +++++ core/include/Core/export_laz.h | 35 ++++-- core/include/Core/structures.h | 6 + rosbags/OusterPcapReader.cpp | 108 ++++++++++++++---- rosbags/OusterPcapReader.h | 10 +- rosbags/tests/test_ouster_pcap_reader.cpp | 67 +++++++++++ 12 files changed, 327 insertions(+), 52 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index f4800d6b..a3a74951 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -263,7 +263,7 @@ using trajectory_viewer_shaders::kVS; struct GpuPoint { float x, y, z; //!< chunk-local position (location 0) - float colorPacked; //!< float bits = 0x00RRGGBB (location 1) + float colorPacked; //!< float bits = 0xFFRRGGBB, FF = 1 when the RGB came from the LAZ (location 1) uint16_t intensity; //!< 0-1 scaled to 0-65535 (location 2) int16_t depthCm; //!< distance from the LiDAR when captured, cm; -1 = unknown (location 4) int32_t cameraId; //!< image that colored it; -1 = projects into no image, -2 = rejected by ROI/mask (location 3) @@ -355,7 +355,7 @@ struct ColorPt uint8_t r, g, b; float intensity; int64_t ts_ns; - bool validColor; //!< RGB sampled from an image; false = intensity-gray fallback + bool validColor; //!< RGB from an image or the LAZ; false = intensity-gray fallback int16_t depthCm; //!< LiDAR range as GpuPoint::depthCm holds it; -1 = unknown }; @@ -466,11 +466,13 @@ struct AppState float maxImageAngSpeedDeg = 60.f; //!< deg/s threshold int angFilteredImgs = 0; //!< images skipped by the filter in the last colorize pass - bool useImageColor = false; //!< true once a colorize pass produced RGB data + bool useImageColor = false; //!< true once a colorize pass produced RGB data (from images or the LAZ) + bool lazHasRgb = false; //!< some scan_lio_*.laz carried RGB; it colors points no image does int colorMode = 0; //!< 0=intensity (jet), 1=RGB by image, 2=camera id, 3=in ROI/mask, 4=local depth (jet) float depthColorMax = 50.f; //!< m: local depth mode maps 0..this onto the jet colormap int coloredPts = 0; //!< points that received RGB from an image int uncoloredPts = 0; //!< points left as intensity-gray (no image / out of frustum / outside ROI) + int lazColoredPts = 0; //!< points no image colored that took their RGB from the LAZ char sessionBuf[512] = {}; char calibBuf[512] = {}; @@ -786,6 +788,7 @@ static void loadSession(AppState& s) static void loadCloud(AppState& s) { + s.lazHasRgb = false; s.exportCloud.clear(); s.exportSegments.clear(); s.cloud.unload(); @@ -864,7 +867,8 @@ static void loadCloud(AppState& s) int cnt = 0; int step = std::max(1, s.cloudDecim); int coloredChunks = 0; - int coloredPts = 0, uncoloredPts = 0; + int coloredPts = 0, uncoloredPts = 0, lazColoredPts = 0; + bool lazHasRgb = false; int angFilteredImgs = 0; // camera frames dropped for excessive pose angular speed const bool dropFastImgs = s.filterFastImages && s.maxImageAngSpeedDeg > 0.f; @@ -973,6 +977,7 @@ static void loadCloud(AppState& s) continue; int nImgs = (int)chunkImgs.size(); + lazHasRgb |= pc.hasRgb; const size_t segBegin = s.exportCloud.size(); std::vector gpuPoints; // this chunk's points, uploaded as its own part below @@ -1137,8 +1142,19 @@ static void loadCloud(AppState& s) } } } + // No image colored it: fall back to the LAZ's own RGB, if any + bool lazColored = false; + if (camIdF < 0.f && pc.hasRgb) + { + // top byte flags it for kFS, which otherwise drops points no image colored + const uint32_t p = (1u << 24) | (uint32_t(pt.r) << 16) | (uint32_t(pt.g) << 8) | uint32_t(pt.b); + std::memcpy(&colorF, &p, 4); + lazColored = true; + } if (camIdF >= 0.f) ++coloredPts; + else if (lazColored) + ++lazColoredPts; else ++uncoloredPts; @@ -1167,7 +1183,7 @@ static void loadCloud(AppState& s) (uint8_t)(packed & 0xFF), rawIntensity, pt.ts_ns, - camIdF >= 0.f, + camIdF >= 0.f || lazColored, depthCm }); float d2 = pw.squaredNorm(); @@ -1187,11 +1203,13 @@ static void loadCloud(AppState& s) // chunkImgs and their cv::Mat memory are released here } - s.useImageColor = canColor && (coloredChunks > 0); + s.lazHasRgb = lazHasRgb; + s.useImageColor = (canColor && coloredChunks > 0) || lazHasRgb; if (s.useImageColor) s.colorMode = 1; // default to RGB display once RGB data is available s.coloredPts = coloredPts; s.uncoloredPts = uncoloredPts; + s.lazColoredPts = lazColoredPts; s.angFilteredImgs = angFilteredImgs; if (cnt > 0) @@ -1214,6 +1232,8 @@ static void loadCloud(AppState& s) double pct = 100.0 * coloredPts / cnt; s.status += " | Colored: " + std::to_string(coloredPts) + " Uncolored: " + std::to_string(uncoloredPts) + " (" + std::to_string((int)std::lround(pct)) + "%)"; + if (lazHasRgb) + s.status += " LAZ RGB: " + std::to_string(lazColoredPts); } if (angFilteredImgs > 0) s.status += " | Fast-img filtered: " + std::to_string(angFilteredImgs); @@ -2731,9 +2751,14 @@ int main(int argc, char* argv[]) ImGui::SetNextItemWidth(140.f); ImGui::DragFloat("Depth color max", &s.depthColorMax, 0.5f, 1.f, 327.f, "%.1f m"); } + if (s.lazHasRgb && s.imagesFilenamesInTime.empty()) + { + if (ImGui::MenuItem("RGB (LAZ)", "Ctrl", s.colorMode == 1)) + s.colorMode = 1; + } if (!s.imagesFilenamesInTime.empty()) { - if (ImGui::MenuItem("RGB (image)", "Ctrl", s.colorMode == 1)) + if (ImGui::MenuItem(s.lazHasRgb ? "RGB (image, LAZ fallback)" : "RGB (image)", "Ctrl", s.colorMode == 1)) s.colorMode = 1; if (ImGui::MenuItem("Camera ID", nullptr, s.colorMode == 2)) s.colorMode = 2; diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h index db76a66b..f0cace56 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h @@ -9,8 +9,8 @@ namespace trajectory_viewer_shaders { - // colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet intensity, 1=RGB, 2=camera id, 3=in ROI/mask, - // 4=jet local depth (LiDAR range) + // colorPacked: float bits = 0xFFRRGGBB where FF is 1 when the RGB came from the LAZ, else 0; colorMode: 0=jet intensity, 1=RGB, + // 2=camera id, 3=in ROI/mask, 4=jet local depth (LiDAR range) inline constexpr const char* kVS = R"( #version 330 // GLSL 330 has no 16-bit types: the integer attributes arrive widened to uint/int, @@ -33,6 +33,7 @@ out float fragDepth; // m; < 0 = unknown out vec4 vertColor; flat out float fragColorCameraId; flat out float fragInRoi; +flat out float fragLazRgb; // 1 = RGB from the LAZ (no image colored the point) void main() { float range = float(lidarDepth) * 0.01; bool outOfRange = lidarDepth >= 0 && (range < minRange || (maxRange > 0.0 && range > maxRange)); @@ -50,6 +51,7 @@ void main() { fragIntensity = float(lidarIntensity) / 65535.0; fragDepth = lidarDepth >= 0 ? range : -1.0; vertColor = vec4(r, g, b, 1.0); + fragLazRgb = ((p >> 24) & 0xFFu) != 0u ? 1.0 : 0.0; fragColorCameraId = float(colorCameraId); // kFS's in-ROI states: 1 = kept, 0 = rejected, -1 = projects into no image fragInRoi = colorCameraId >= 0 ? 1.0 : (colorCameraId == -1 ? -1.0 : 0.0); @@ -63,6 +65,7 @@ in float fragDepth; in vec4 vertColor; flat in float fragColorCameraId; flat in float fragInRoi; +flat in float fragLazRgb; uniform int colorMode; uniform int selectedCamera; // -1 = show all, else keep only points from this image uniform float depthColorMax; // m: colorMode 4 maps 0..this onto the jet colormap @@ -89,8 +92,8 @@ void main() { if (colorMode == 1) { - if (fragColorCameraId < 0.0) - discard; // not colored by any image — draw only colored points + if (fragColorCameraId < 0.0 && fragLazRgb < 0.5) + discard; // colored by no image and no LAZ RGB — draw only colored points finalColor = vertColor; } else if (colorMode == 2) diff --git a/apps/console_tools/ouster_pcap_to_laz.cpp b/apps/console_tools/ouster_pcap_to_laz.cpp index d1c5604c..8396a93f 100644 --- a/apps/console_tools/ouster_pcap_to_laz.cpp +++ b/apps/console_tools/ouster_pcap_to_laz.cpp @@ -68,6 +68,16 @@ namespace raw_ = fitted_ = true; } + // The colour words are IEEE half floats rather than unsigned integers. + void setFloat16(bool float16) + { + for (size_t v = 0; v < decode_.size(); ++v) + { + const double x = float16 ? halfToDouble(static_cast(v)) : static_cast(v); + decode_[v] = std::isfinite(x) && x > 0.0 ? x : 0.0; // NaN / inf / negative -> black + } + } + // Grey-world white balance, white point at the 99th percentile of the // balanced values, gamma 2.2. Fitted once, so colour stays comparable // across chunks. @@ -81,7 +91,7 @@ namespace if (!hasColour(c)) continue; for (int k = 0; k < 3; ++k) - sum[k] += c[k]; + sum[k] += decode_[c[k]]; ++samples_; } } @@ -98,15 +108,15 @@ namespace if (!hasColour(c)) continue; for (int k = 0; k < 3; ++k) - balanced.push_back(static_cast(c[k] * gains_[k])); + balanced.push_back(static_cast(decode_[c[k]] * gains_[k])); } } - white_ = 65535.0; + white_ = 1.0; if (!balanced.empty()) { const auto nth = balanced.begin() + static_cast(0.99 * static_cast(balanced.size() - 1)); std::nth_element(balanced.begin(), nth, balanced.end()); - white_ = std::max(1.0, static_cast(*nth)); + white_ = *nth > 0.0f ? static_cast(*nth) : 1.0; } for (int k = 0; k < 3; ++k) @@ -114,7 +124,7 @@ namespace lut_[k].resize(65536); for (size_t v = 0; v < lut_[k].size(); ++v) { - const double x = std::clamp(static_cast(v) * gains_[k] / white_, 0.0, 1.0); + const double x = std::clamp(decode_[v] * gains_[k] / white_, 0.0, 1.0); lut_[k][v] = static_cast(std::lround(65535.0 * std::pow(x, 1.0 / 2.2))); } } @@ -155,6 +165,26 @@ namespace } private: + static double halfToDouble(uint16_t h) + { + const int exponent = (h >> 10) & 0x1F; + const int mantissa = h & 0x3FF; + const double sign = (h & 0x8000) ? -1.0 : 1.0; + if (exponent == 0) + return sign * std::ldexp(mantissa, -24); // subnormal + if (exponent == 31) + return mantissa ? std::numeric_limits::quiet_NaN() : sign * std::numeric_limits::infinity(); + return sign * std::ldexp(mantissa + 1024, exponent - 25); + } + + static std::array identity() + { + std::array table{}; + for (size_t v = 0; v < table.size(); ++v) + table[v] = static_cast(v); + return table; + } + static bool hasColour(const Rgb& c) { return !(c[0] == 0xFFFF && c[1] == 0xFFFF && c[2] == 0xFFFF); @@ -166,6 +196,7 @@ namespace double white_ = 65535.0; size_t samples_ = 0; std::array, 3> lut_; + std::array decode_ = identity(); // colour word -> linear value }; void print_usage(const char* argv0) @@ -191,7 +222,7 @@ namespace spdlog::error(" --rgb colour on RGB profiles, written as LAS point format 3 (default: auto)"); spdlog::error(" auto: white-balanced, exposed and gamma-corrected, fitted on the"); spdlog::error(" first {:.0f} s of the recording and then kept fixed", kRgbFitSeconds); - spdlog::error(" raw: the sensor's 16-bit colour words unchanged"); + spdlog::error(" raw: the sensor's 16-bit colour words unchanged (half-float bits on RGB16)"); spdlog::error(" off: no colour (point format 1)"); spdlog::error(""); spdlog::error("Points are written in the Ouster SENSOR frame (lidar_to_sensor_transform applied), with"); @@ -415,8 +446,9 @@ int main(const int argc, const char** argv) options.lidar_port ? options.lidar_port : metadata.udp_port_lidar, options.imu_port ? options.imu_port : metadata.udp_port_imu, metadata.udp_profile_imu); - if (metadata.udp_profile_imu != "LEGACY") - spdlog::warn("IMU profile {} is not supported (LEGACY only) - IMU packets will be ignored", metadata.udp_profile_imu); + if (metadata.imuPacketSize() == 0) + spdlog::warn( + "IMU profile {} is not supported (LEGACY or ACCEL32_GYRO32_NMEA only) - IMU packets will be ignored", metadata.udp_profile_imu); rosbags::OusterPcapReader reader(pcap_path, metadata, options); if (!reader.isOpen()) @@ -440,6 +472,7 @@ int main(const int argc, const char** argv) RgbMapper rgb_mapper; if (rgb_mode == RgbMode::Raw) rgb_mapper.setRaw(); + rgb_mapper.setFloat16(metadata.rgb_float16); std::vector mapped_rgb; std::vector imu; @@ -482,7 +515,7 @@ int main(const int argc, const char** argv) samples.push_back(&packet.rgb); rgb_mapper.fit(samples); spdlog::info( - "RGB: white balance x{:.3f}/{:.3f}/{:.3f}, white point {:.0f}, gamma 2.2 - fitted on the first {:.2f} s ({} coloured " + "RGB: white balance x{:.3f}/{:.3f}/{:.3f}, white point {:.4g}, gamma 2.2 - fitted on the first {:.2f} s ({} coloured " "points); --rgb raw writes the sensor's values instead", rgb_mapper.gains()[0], rgb_mapper.gains()[1], diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index d02e0d52..f30488dd 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -955,6 +955,7 @@ void save_result(std::vector& worker_data, LidarOdometryParams& para std::vector global_pointcloud; std::vector intensity; std::vector timestamps; + std::vector> colors; std::vector original_points_to_save; // = worker_data[i].load_points(worker_data[i].original_points_cache_file_name); if (!load_vector_data(worker_data_concatenated[i].original_points_to_save_cache_file_name.string(), original_points_to_save)) @@ -973,8 +974,18 @@ void save_result(std::vector& worker_data, LidarOdometryParams& para intensity, timestamps, true, - params.save_index_pose); - exportLaz(path.string(), global_pointcloud, intensity, timestamps); + params.save_index_pose, + &colors); + + // write rgb only when the input carried colour (all-zero means a colourless source) + const bool has_colors = std::any_of( + colors.begin(), + colors.end(), + [](const auto& c) + { + return c[0] != 0 || c[1] != 0 || c[2] != 0; + }); + exportLaz(path.string(), global_pointcloud, intensity, timestamps, 0.0, 0.0, 0.0, nullptr, has_colors ? &colors : nullptr); if (params.save_index_pose) { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 2a376f95..2bc631f7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -532,6 +532,9 @@ std::vector load_point_cloud( int counter_ts0 = 0; int counter_filtered_points = 0; + // LAS point formats carrying rgb: 2, 3, 5, 7, 8, 10 + const int fmt = header->point_data_format; + const bool has_rgb = fmt == 2 || fmt == 3 || fmt == 5 || fmt == 7 || fmt == 8 || fmt == 10; for (laszip_U32 j = 0; j < header->number_of_point_records; j++) { @@ -566,6 +569,12 @@ std::vector load_point_cloud( p.lidarid = id; p.timestamp = point->gps_time; p.intensity = point->intensity; + if (has_rgb) + { + p.rgb[0] = static_cast(point->rgb[0] >> 8); + p.rgb[1] = static_cast(point->rgb[1] >> 8); + p.rgb[2] = static_cast(point->rgb[2] >> 8); + } if (p.timestamp == 0 && ommit_points_with_timestamp_equals_zero) { diff --git a/calib_core/include/CalibCore/PointCloud.h b/calib_core/include/CalibCore/PointCloud.h index e83b9f2f..1f75a4a0 100644 --- a/calib_core/include/CalibCore/PointCloud.h +++ b/calib_core/include/CalibCore/PointCloud.h @@ -10,6 +10,7 @@ struct Point3D { float x, y, z; float intensity; // normalized to [0,1] int64_t ts_ns = 0; // GPS time cast from gps_time field (0 if unavailable) + uint8_t r = 0, g = 0, b = 0; // LAS rgb (16-bit >> 8); 0 when the file has none }; struct PointCloud { @@ -17,6 +18,7 @@ struct PointCloud { float minX = 0.f, maxX = 0.f; float minY = 0.f, maxY = 0.f; float minZ = 0.f, maxZ = 0.f; + bool hasRgb = false; // point format carries rgb AND at least one point is non-black bool load(const std::string& path); void clear(); diff --git a/calib_core/src/PointCloud.cpp b/calib_core/src/PointCloud.cpp index 05e6f328..dfb4984e 100644 --- a/calib_core/src/PointCloud.cpp +++ b/calib_core/src/PointCloud.cpp @@ -1,5 +1,7 @@ #include #include +#include +#include #include #include #include @@ -9,6 +11,7 @@ namespace calib { void PointCloud::clear() { points.clear(); minX = maxX = minY = maxY = minZ = maxZ = 0.f; + hasRgb = false; } bool PointCloud::load(const std::string& path) { @@ -40,6 +43,12 @@ bool PointCloud::load(const std::string& path) { laszip_get_point_pointer(reader, &point); points.reserve(static_cast(npoints)); + std::vector> rgb16; // raw LAS rgb, scaled once the range is known + laszip_U16 maxRgb = 0; + + // LAS point formats with an rgb field + const int fmt = header->point_data_format; + const bool formatHasRgb = fmt == 2 || fmt == 3 || fmt == 5 || fmt == 7 || fmt == 8 || fmt == 10; float minInt = std::numeric_limits::max(); float maxInt = -std::numeric_limits::max(); @@ -59,6 +68,10 @@ bool PointCloud::load(const std::string& path) { p.z = static_cast(coords[2]); p.intensity = static_cast(point->intensity); p.ts_ns = static_cast(point->gps_time); + if (formatHasRgb) { + rgb16.push_back({ point->rgb[0], point->rgb[1], point->rgb[2] }); + maxRgb = std::max({ maxRgb, point->rgb[0], point->rgb[1], point->rgb[2] }); + } if (p.x < xmin) xmin = p.x; if (p.x > xmax) xmax = p.x; if (p.y < ymin) ymin = p.y; if (p.y > ymax) ymax = p.y; @@ -69,6 +82,19 @@ bool PointCloud::load(const std::string& path) { points.push_back(p); } + // LAS rgb is 16-bit, but some writers store 8-bit values unscaled: no + // value above 255 means 8-bit, else take the high byte. All zeros = a + // writer with no colour source that still emitted an rgb format. + if (maxRgb > 0 && rgb16.size() == points.size()) { + hasRgb = true; + const int shift = maxRgb > 255 ? 8 : 0; + for (size_t i = 0; i < points.size(); ++i) { + points[i].r = static_cast(rgb16[i][0] >> shift); + points[i].g = static_cast(rgb16[i][1] >> shift); + points[i].b = static_cast(rgb16[i][2] >> shift); + } + } + // Normalize intensity to [0,1] float intRange = (maxInt > minInt) ? (maxInt - minInt) : 1.f; for (auto& p : points) diff --git a/core/include/Core/export_laz.h b/core/include/Core/export_laz.h index 95084dc0..18a91322 100644 --- a/core/include/Core/export_laz.h +++ b/core/include/Core/export_laz.h @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -28,7 +29,8 @@ class LazWriter LazWriter(const LazWriter&) = delete; LazWriter& operator=(const LazWriter&) = delete; - bool open(const std::string& filename, double offset_x, double offset_y, double offset_z) + //! with_rgb writes LAS point format 3 (format 1 + rgb) instead of format 1. + bool open(const std::string& filename, double offset_x, double offset_y, double offset_z, bool with_rgb = false) { constexpr double scale = 0.0001; @@ -49,8 +51,8 @@ class LazWriter header->global_encoding = (1 << 0); header->version_major = 1; header->version_minor = 2; - header->point_data_format = 1; - header->point_data_record_length = 28; + header->point_data_format = with_rgb ? 3 : 1; + header->point_data_record_length = with_rgb ? 34 : 28; header->number_of_point_records = 0; header->number_of_points_by_return[0] = 0; header->number_of_points_by_return[1] = 0; @@ -86,9 +88,17 @@ class LazWriter double offset_x, double offset_y, double offset_z, - unsigned short point_source_id = 0) + unsigned short point_source_id = 0, + const uint8_t* rgb = nullptr) { point_->intensity = intensity; + if (rgb) + { + // 8-bit -> 16-bit LAS convention (value * 256), matching the readers' value / 256 + point_->rgb[0] = static_cast(rgb[0]) << 8; + point_->rgb[1] = static_cast(rgb[1]) << 8; + point_->rgb[2] = static_cast(rgb[2]) << 8; + } point_->return_number = 1; point_->number_of_returns = 1; point_->gps_time = timestamp * 1e9; @@ -164,16 +174,20 @@ inline bool exportLaz( double offset_x = 0.0, double offset_y = 0.0, double offset_alt = 0.0, - const std::vector* point_source_ids = nullptr) + const std::vector* point_source_ids = nullptr, + const std::vector>* colors = nullptr) { + const bool with_rgb = colors && colors->size() == pointcloud.size() && !colors->empty(); + LazWriter writer; - if (!writer.open(filename, offset_x, offset_y, offset_alt)) + if (!writer.open(filename, offset_x, offset_y, offset_alt, with_rgb)) return false; for (size_t i = 0; i < pointcloud.size(); i++) { const unsigned short psid = (point_source_ids && i < point_source_ids->size()) ? (*point_source_ids)[i] : 0; - if (!writer.writePoint(pointcloud[i], intensity[i], timestamps[i], offset_x, offset_y, offset_alt, psid)) + const uint8_t* rgb = with_rgb ? (*colors)[i].data() : nullptr; + if (!writer.writePoint(pointcloud[i], intensity[i], timestamps[i], offset_x, offset_y, offset_alt, psid, rgb)) return false; } @@ -372,7 +386,8 @@ inline void points_to_vector( std::vector& intensity, std::vector& timestamps, bool use_first_pose, - bool save_index_pose) + bool save_index_pose, + std::vector>* colors = nullptr) { Eigen::Affine3d m_pose = trajectory[0].inverse(); for (const auto& org_p : points) @@ -391,6 +406,10 @@ inline void points_to_vector( pointcloud.push_back(p.point); intensity.push_back(p.intensity); timestamps.push_back(p.timestamp); + if (colors) + { + colors->push_back({ p.rgb[0], p.rgb[1], p.rgb[2] }); + } if (save_index_pose) { if (index_poses) diff --git a/core/include/Core/structures.h b/core/include/Core/structures.h index d7a3388c..88dc535a 100644 --- a/core/include/Core/structures.h +++ b/core/include/Core/structures.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -70,6 +71,8 @@ struct Point3Di float intensity; int index_pose; uint8_t lidarid; + // 8-bit colour from LAS rgb (16-bit >> 8); all zero when the source has none. Sits in the padding after lidarid. + uint8_t rgb[3] = { 0, 0, 0 }; int index_point; }; @@ -84,6 +87,7 @@ struct Point3DiDisk float intensity; int32_t index_pose; uint8_t lidarid; + uint8_t rgb[3]; int32_t index_point; }; #pragma pack(pop) @@ -126,6 +130,7 @@ inline void convert_to_disk(const Point3Di& src, Point3DiDisk& dst) dst.intensity = src.intensity; dst.index_pose = src.index_pose; dst.lidarid = src.lidarid; + std::copy(std::begin(src.rgb), std::end(src.rgb), dst.rgb); dst.index_point = src.index_point; } @@ -136,6 +141,7 @@ inline void convert_from_disk(const Point3DiDisk& src, Point3Di& dst) dst.intensity = src.intensity; dst.index_pose = src.index_pose; dst.lidarid = src.lidarid; + std::copy(std::begin(src.rgb), std::end(src.rgb), dst.rgb); dst.index_point = src.index_point; } diff --git a/rosbags/OusterPcapReader.cpp b/rosbags/OusterPcapReader.cpp index 0453252b..e23e9fc3 100644 --- a/rosbags/OusterPcapReader.cpp +++ b/rosbags/OusterPcapReader.cpp @@ -23,6 +23,11 @@ constexpr size_t kPacketHeaderBytes = 32; constexpr size_t kPacketFooterBytes = 32; constexpr size_t kColumnHeaderBytes = 12; // u64 timestamp ns, u16 measurement_id, u16 status constexpr size_t kLegacyImuPacketBytes = 48; +// ACCEL32_GYRO32_NMEA IMU packet: the lidar packet header, a 100-byte NMEA +// block (ignored), N measurements, the packet footer. +constexpr size_t kNmeaBlockBytes = 100; +constexpr size_t kImuMeasurementBytes = 36; // u64 timestamp ns, u16 measurement_id, u16 status, 6 x f32 +constexpr double kStandardGravity = 9.80665; // Profiles whose channel data block is known without a pixel_data_format. All of // them carry the first return in word 0 -- range [18:0] in mm, reflectivity @@ -139,6 +144,15 @@ size_t OusterMetadata::lidarPacketSize() const kPacketFooterBytes; } +size_t OusterMetadata::imuPacketSize() const +{ + if(udp_profile_imu == "LEGACY") + return kLegacyImuPacketBytes; + if(udp_profile_imu == "ACCEL32_GYRO32_NMEA") + return kPacketHeaderBytes + kNmeaBlockBytes + static_cast(imu_measurements_per_packet) * kImuMeasurementBytes + kPacketFooterBytes; + return 0; +} + bool parseOusterMetadata(const std::string& json_text, OusterMetadata& out, std::string& error) { OusterMetadata m; @@ -169,6 +183,13 @@ bool parseOusterMetadata(const std::string& json_text, OusterMetadata& out, std: m.udp_port_lidar = config.value("udp_port_lidar", uint16_t{7502}); m.udp_port_imu = config.value("udp_port_imu", uint16_t{7503}); m.udp_profile_imu = config.value("udp_profile_imu", std::string("LEGACY")); + if(j.contains("imu_data_format")) + m.imu_measurements_per_packet = j["imu_data_format"].value("imu_measurements_per_packet", m.imu_measurements_per_packet); + if(m.imu_measurements_per_packet <= 0) + { + error = "metadata: imu_data_format.imu_measurements_per_packet must be positive"; + return false; + } const json& format = j["lidar_data_format"]; m.pixels_per_column = format.at("pixels_per_column").get(); @@ -285,6 +306,8 @@ bool parseOusterMetadata(const std::string& json_text, OusterMetadata& out, std: } } + m.rgb_float16 = m.rgb_width == 16; + const json& beams = j["beam_intrinsics"]; const size_t beam_count = static_cast(m.pixels_per_column); if(!readAngles(beams, "beam_altitude_angles", beam_count, m.beam_altitude_angles_deg, error) || @@ -465,11 +488,23 @@ struct OusterPcapReader::Impl if(dport == options.lidar_port) handleLidar(payload, length); else if(dport == options.imu_port && meta.udp_profile_imu == "LEGACY") - handleImu(payload, length); + handleLegacyImu(payload, length); + else if(dport == options.imu_port && meta.udp_profile_imu == "ACCEL32_GYRO32_NMEA") + handleAccel32Gyro32NmeaImu(payload, length); else ++stats.other_packets; } + // Packet header init_id / serial number differ from the metadata's. + bool isForeign(const uint8_t* p) const + { + const uint64_t init_id = p[4] | (p[5] << 8) | (static_cast(p[6]) << 16); + uint64_t sn = 0; + for(int i = 11; i >= 7; --i) + sn = (sn << 8) | p[i]; + return (meta.has_initialization_id && init_id != meta.initialization_id) || (check_sn && sn != prod_sn); + } + void handleLidar(const uint8_t* p, size_t size) { if(size != meta.lidarPacketSize()) @@ -477,11 +512,7 @@ struct OusterPcapReader::Impl ++stats.bad_size_packets; return; } - const uint64_t init_id = p[4] | (p[5] << 8) | (static_cast(p[6]) << 16); - uint64_t sn = 0; - for(int i = 11; i >= 7; --i) - sn = (sn << 8) | p[i]; - if((meta.has_initialization_id && init_id != meta.initialization_id) || (check_sn && sn != prod_sn)) + if(isForeign(p)) { ++stats.foreign_packets; return; @@ -551,7 +582,24 @@ struct OusterPcapReader::Impl rgb = {}; } - void handleImu(const uint8_t* p, size_t size) + // Rotates one sample (acceleration in g, gyro in rad/s, IMU frame) into the + // sensor frame and hands it on. + void emitImu(double timestamp, const double (&acc)[3], const double (&gyro)[3]) + { + const auto& r = imu_rotation; + McapImuSample s; + s.timestamp = timestamp; + s.acc_x = static_cast(r[0] * acc[0] + r[1] * acc[1] + r[2] * acc[2]); + s.acc_y = static_cast(r[3] * acc[0] + r[4] * acc[1] + r[5] * acc[2]); + s.acc_z = static_cast(r[6] * acc[0] + r[7] * acc[1] + r[8] * acc[2]); + s.gyro_x = static_cast(r[0] * gyro[0] + r[1] * gyro[1] + r[2] * gyro[2]); + s.gyro_y = static_cast(r[3] * gyro[0] + r[4] * gyro[1] + r[5] * gyro[2]); + s.gyro_z = static_cast(r[6] * gyro[0] + r[7] * gyro[1] + r[8] * gyro[2]); + if(onImu) + onImu(s); + } + + void handleLegacyImu(const uint8_t* p, size_t size) { // LEGACY IMU packet: u64 system / accel / gyro read times (ns), then // accel x y z (g) and gyro x y z (deg/s) as float32. @@ -560,24 +608,42 @@ struct OusterPcapReader::Impl ++stats.bad_size_packets; return; } - const double acc[3] = {leFloat(p + 24), leFloat(p + 28), leFloat(p + 32)}; - const double gyro[3] = {leFloat(p + 36), leFloat(p + 40), leFloat(p + 44)}; constexpr double kDegToRad = std::numbers::pi / 180.0; - const auto& r = imu_rotation; - - McapImuSample s; + const double acc[3] = {leFloat(p + 24), leFloat(p + 28), leFloat(p + 32)}; + const double gyro[3] = {leFloat(p + 36) * kDegToRad, leFloat(p + 40) * kDegToRad, leFloat(p + 44) * kDegToRad}; + ++stats.imu_packets; // The gyro read time follows the configured timestamp_mode, like the lidar // column timestamps; the system timestamp is a diagnostic counter. - s.timestamp = static_cast(le64(p + 16)) * 1e-9; - s.acc_x = static_cast(r[0] * acc[0] + r[1] * acc[1] + r[2] * acc[2]); - s.acc_y = static_cast(r[3] * acc[0] + r[4] * acc[1] + r[5] * acc[2]); - s.acc_z = static_cast(r[6] * acc[0] + r[7] * acc[1] + r[8] * acc[2]); - s.gyro_x = static_cast((r[0] * gyro[0] + r[1] * gyro[1] + r[2] * gyro[2]) * kDegToRad); - s.gyro_y = static_cast((r[3] * gyro[0] + r[4] * gyro[1] + r[5] * gyro[2]) * kDegToRad); - s.gyro_z = static_cast((r[6] * gyro[0] + r[7] * gyro[1] + r[8] * gyro[2]) * kDegToRad); + emitImu(static_cast(le64(p + 16)) * 1e-9, acc, gyro); + } + + void handleAccel32Gyro32NmeaImu(const uint8_t* p, size_t size) + { + // ACCEL32_GYRO32_NMEA (FW 3.1+): standard packet header, NMEA block, then + // per measurement u64 timestamp (ns, same clock as the lidar columns), + // u16 measurement_id, u16 status (bit 0 = valid), accel x y z (m/s^2) + // and gyro x y z (rad/s) as float32. + if(size != meta.imuPacketSize()) + { + ++stats.bad_size_packets; + return; + } + if(isForeign(p)) + { + ++stats.foreign_packets; + return; + } ++stats.imu_packets; - if(onImu) - onImu(s); + const uint8_t* m = p + kPacketHeaderBytes + kNmeaBlockBytes; + for(int i = 0; i < meta.imu_measurements_per_packet; ++i, m += kImuMeasurementBytes) + { + if(!(le16(m + 10) & 1)) + continue; + const double acc[3] = { + leFloat(m + 12) / kStandardGravity, leFloat(m + 16) / kStandardGravity, leFloat(m + 20) / kStandardGravity}; + const double gyro[3] = {leFloat(m + 24), leFloat(m + 28), leFloat(m + 32)}; + emitImu(static_cast(le64(m)) * 1e-9, acc, gyro); + } } // Feeds one IPv4 fragment in; once every byte of its datagram has arrived, diff --git a/rosbags/OusterPcapReader.h b/rosbags/OusterPcapReader.h index b4194e8a..dfc8c67e 100644 --- a/rosbags/OusterPcapReader.h +++ b/rosbags/OusterPcapReader.h @@ -44,6 +44,9 @@ struct OusterMetadata int reflectivity_width = 0; // 0 = profile has no reflectivity std::array rgb_start_bit{}; // red, green, blue int rgb_width = 0; // 0 = profile has no colour + // The 16-bit colour words are IEEE half floats (linear, HDR) -- how the Ouster + // SDK decodes FW 4.0 RGB16 -- not unsigned integers. + bool rgb_float16 = false; std::vector beam_altitude_angles_deg; std::vector beam_azimuth_angles_deg; @@ -52,8 +55,13 @@ struct OusterMetadata std::array lidar_to_sensor_transform{}; // row-major 4x4, mm std::array imu_to_sensor_transform{}; // row-major 4x4, mm + // ACCEL32_GYRO32_NMEA only: samples per IMU packet (imu_data_format). + int imu_measurements_per_packet = 8; + // Bytes in one lidar UDP payload: packet header + columns + packet footer. size_t lidarPacketSize() const; + // Bytes in one IMU UDP payload for udp_profile_imu; 0 = unsupported profile. + size_t imuPacketSize() const; }; // Parses `json_text`. On failure returns false and fills `error`. @@ -97,7 +105,7 @@ struct OusterPcapStats // calibrated reflectivity, the scale mandeye's own LAZ files use. On an RGB // profile each point's colour comes alongside, as the sensor's raw 16-bit words. // -// IMU samples (LEGACY IMU profile only) are rotated into the sensor frame, +// IMU samples (LEGACY or ACCEL32_GYRO32_NMEA profile) are rotated into the sensor frame, // gyro in rad/s, acceleration in g -- the units mandeye's imuNNNN.csv uses, // NOT the m/s^2 McapImuSample documents for bags; MandeyeSessionWriter writes // the values through unchanged. diff --git a/rosbags/tests/test_ouster_pcap_reader.cpp b/rosbags/tests/test_ouster_pcap_reader.cpp index bcbeb59e..8e5e0399 100644 --- a/rosbags/tests/test_ouster_pcap_reader.cpp +++ b/rosbags/tests/test_ouster_pcap_reader.cpp @@ -133,6 +133,36 @@ std::vector legacyImuPacket(uint64_t gyro_ts_ns, const float (&acc_g)[3 return p; } +struct ImuMeasurement +{ + uint64_t timestamp_ns; + bool valid; + float acc_ms2[3]; + float gyro_rads[3]; +}; + +// ACCEL32_GYRO32_NMEA with two measurements per packet. +std::vector accel32Gyro32NmeaImuPacket(const ImuMeasurement (&measurements)[2], uint64_t init_id = kInitId) +{ + std::vector p(32 + 100 + 2 * 36 + 32, 0); + putLe(p, 0, 2, 2); // packet type + putLe(p, 4, init_id, 3); + putLe(p, 7, kSerial, 5); + for (size_t i = 0; i < 2; ++i) + { + const size_t m = 32 + 100 + i * 36; + putLe(p, m, measurements[i].timestamp_ns, 8); + putLe(p, m + 8, 31 + 32 * i, 2); + putLe(p, m + 10, measurements[i].valid ? 1 : 0, 2); + for (int k = 0; k < 3; ++k) + { + std::memcpy(&p[m + 12 + 4 * k], &measurements[i].acc_ms2[k], 4); + std::memcpy(&p[m + 24 + 4 * k], &measurements[i].gyro_rads[k], 4); + } + } + return p; +} + // Wraps a UDP payload in Ethernet/IPv4/UDP, split into IPv4 fragments carrying at // most `fragment_bytes` (a multiple of 8) of IP payload each; 0 = unfragmented. std::vector> udpFrames(const std::vector& payload, uint16_t dport, uint16_t ip_id, size_t fragment_bytes = 0) @@ -239,12 +269,14 @@ TEST_CASE("parseOusterMetadata: profile table, pixel_data_format, and refusals") CHECK(m.lidarPacketSize() == 32 + 2 * (12 + 4 * 12) + 32); CHECK(m.initialization_id == kInitId); CHECK(m.rgb_width == 0); + CHECK(!m.rgb_float16); } SUBCASE("the RGB16 profile name brings red, green, blue in words 2 and 3") { const auto m = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16_RGB16")); CHECK(m.pixel_bytes == 16); CHECK(m.rgb_width == 16); + CHECK(m.rgb_float16); CHECK(m.rgb_start_bit == std::array{64, 80, 96}); } SUBCASE("pixel_data_format describes a profile no table knows") @@ -264,6 +296,7 @@ TEST_CASE("parseOusterMetadata: profile table, pixel_data_format, and refusals") CHECK(m.range_start_bit == 0); // the first return, not range_capped_mm_1 CHECK(m.reflectivity_start_bit == 24); CHECK(m.rgb_width == 16); + CHECK(m.rgb_float16); CHECK(m.rgb_start_bit == std::array{112, 128, 144}); // rgb_r, not rgb_r1 } SUBCASE("an unknown profile without pixel_data_format is refused by name") @@ -373,6 +406,40 @@ TEST_CASE("OusterPcapReader: LEGACY IMU is rotated into the sensor frame, deg/s fs::remove(path); } +TEST_CASE("OusterPcapReader: ACCEL32_GYRO32_NMEA IMU is rotated into the sensor frame, m/s^2 -> g, invalid samples dropped") +{ + std::string json = metadataJson("RNG19_RFL8_SIG16_NIR16"); + json.replace(json.find(R"("udp_profile_imu": "LEGACY")"), 27, R"("udp_profile_imu": "ACCEL32_GYRO32_NMEA")"); + json.insert(json.rfind('}'), R"(, "imu_data_format": {"imu_measurements_per_packet": 2})"); + const auto meta = parseOrFail(json); + CHECK(meta.imu_measurements_per_packet == 2); + CHECK(meta.imuPacketSize() == 32 + 100 + 2 * 36 + 32); + + const fs::path path = fs::temp_directory_path() / "hdmapping_ouster_reader_imu_nmea.pcap"; + const ImuMeasurement measurements[2] = { + {7'000'000'000, true, {9.80665f, 0.0f, 0.0f}, {0.5f, 0.0f, 0.0f}}, + {7'001'562'500, false, {1.0f, 2.0f, 3.0f}, {1.0f, 2.0f, 3.0f}}, + }; + std::vector> frames = udpFrames(accel32Gyro32NmeaImuPacket(measurements), 7503, 1); + const auto foreign = udpFrames(accel32Gyro32NmeaImuPacket(measurements, kInitId + 1), 7503, 2); + frames.insert(frames.end(), foreign.begin(), foreign.end()); + writePcap(path, frames); + + const auto r = readAll(path, meta); + CHECK(r.stats.imu_packets == 1); + CHECK(r.stats.foreign_packets == 1); + REQUIRE(r.imu.size() == 1); + const auto& s = r.imu.front(); + CHECK(s.timestamp == doctest::Approx(7.0)); + CHECK(s.acc_x == doctest::Approx(0.0)); + CHECK(s.acc_y == doctest::Approx(1.0)); // +X of the IMU is +Y of the sensor + CHECK(s.acc_z == doctest::Approx(0.0)); + CHECK(s.gyro_x == doctest::Approx(0.0)); + CHECK(s.gyro_y == doctest::Approx(0.5)); + CHECK(s.gyro_z == doctest::Approx(0.0)); + fs::remove(path); +} + TEST_CASE("OusterPcapReader: foreign, mis-sized and unrelated packets are counted, not decoded") { const auto meta = parseOrFail(metadataJson("RNG19_RFL8_SIG16_NIR16")); From 27376b9570f96acbef1fe074b4a07225c110462f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 13 Oct 2026 16:06:30 +0200 Subject: [PATCH 4/4] Fix failing test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- rosbags/OusterPcapReader.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rosbags/OusterPcapReader.cpp b/rosbags/OusterPcapReader.cpp index e23e9fc3..880b1789 100644 --- a/rosbags/OusterPcapReader.cpp +++ b/rosbags/OusterPcapReader.cpp @@ -389,6 +389,9 @@ struct OusterPcapReader::Impl { if(error.empty()) error = std::move(message); + // Release the file: a rejected capture must not stay locked (Windows) for the reader's lifetime. + in.close(); + open = false; return false; }