diff --git a/CMakeLists.txt b/CMakeLists.txt index 63a74c7..dae3f84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,11 +8,13 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) find_package(Threads REQUIRED) find_package(cppzmq QUIET) +set(CVMMAP_LOCAL_ROOT "${CMAKE_CURRENT_LIST_DIR}/../cv-mmap" CACHE PATH "Path to a local cv-mmap checkout") +set(CVMMAP_LOCAL_BUILD "${CVMMAP_LOCAL_ROOT}/build/core" CACHE PATH "Path to local cv-mmap build artifacts") if ( NOT cvmmap-core_DIR - AND EXISTS "${CMAKE_CURRENT_LIST_DIR}/../cv-mmap/build/core/cvmmap-coreConfig.cmake" - AND EXISTS "${CMAKE_CURRENT_LIST_DIR}/../cv-mmap/build/core/cvmmap-coreTargets.cmake") - set(cvmmap-core_DIR "${CMAKE_CURRENT_LIST_DIR}/../cv-mmap/build/core") + AND EXISTS "${CVMMAP_LOCAL_BUILD}/cvmmap-coreConfig.cmake" + AND EXISTS "${CVMMAP_LOCAL_BUILD}/cvmmap-coreTargets.cmake") + set(cvmmap-core_DIR "${CVMMAP_LOCAL_BUILD}") endif() if (cvmmap-core_DIR) find_package(cvmmap-core CONFIG QUIET) @@ -21,6 +23,7 @@ find_package(ZeroMQ QUIET) find_package(spdlog REQUIRED) find_package(Protobuf REQUIRED) find_package(PkgConfig REQUIRED) +find_package(rvl CONFIG QUIET) if (EXISTS "${CMAKE_CURRENT_LIST_DIR}/lib/CLI11/CMakeLists.txt") add_subdirectory("${CMAKE_CURRENT_LIST_DIR}/lib/CLI11" "${CMAKE_CURRENT_BINARY_DIR}/vendor/cli11") @@ -32,8 +35,6 @@ pkg_check_modules(ZSTD REQUIRED IMPORTED_TARGET libzstd) pkg_check_modules(LZ4 REQUIRED IMPORTED_TARGET liblz4) if (NOT TARGET cvmmap::client) - set(CVMMAP_LOCAL_ROOT "${CMAKE_CURRENT_LIST_DIR}/../cv-mmap") - set(CVMMAP_LOCAL_BUILD "${CVMMAP_LOCAL_ROOT}/build/core") if ( EXISTS "${CVMMAP_LOCAL_ROOT}/core/include/cvmmap/client.hpp" AND EXISTS "${CVMMAP_LOCAL_BUILD}/libcvmmap_client.a" @@ -48,6 +49,21 @@ if (NOT TARGET cvmmap::client) endif() endif() +if (NOT TARGET rvl::rvl) + set(RVL_LOCAL_ROOT "/home/crosstyan/Code/rvl_impl") + set(RVL_LOCAL_BUILD "${RVL_LOCAL_ROOT}/build/core") + if ( + EXISTS "${RVL_LOCAL_ROOT}/core/include/rvl/rvl.hpp" + AND EXISTS "${RVL_LOCAL_BUILD}/librvl_core.a") + add_library(rvl::rvl INTERFACE IMPORTED) + set_target_properties(rvl::rvl PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${RVL_LOCAL_ROOT}/core/include" + INTERFACE_LINK_LIBRARIES "${RVL_LOCAL_BUILD}/librvl_core.a") + else() + message(FATAL_ERROR "rvl::rvl target is unavailable and local rvl_impl build artifacts were not found") + endif() +endif() + set(CVMMAP_PROXY_INCLUDE_DIR "${CMAKE_CURRENT_LIST_DIR}/lib/proxy/include") if (NOT EXISTS "${CVMMAP_PROXY_INCLUDE_DIR}/proxy/proxy.h") message(FATAL_ERROR "proxy headers not found at ${CVMMAP_PROXY_INCLUDE_DIR}") @@ -59,11 +75,21 @@ protobuf_generate( LANGUAGE cpp PROTOS "${CMAKE_CURRENT_LIST_DIR}/proto/foxglove/CompressedVideo.proto" IMPORT_DIRS "${CMAKE_CURRENT_LIST_DIR}/proto") +add_library(cvmmap_streamer_depth_proto STATIC) +protobuf_generate( + TARGET cvmmap_streamer_depth_proto + LANGUAGE cpp + PROTOS "${CMAKE_CURRENT_LIST_DIR}/proto/cvmmap_streamer/DepthMap.proto" + IMPORT_DIRS "${CMAKE_CURRENT_LIST_DIR}/proto") add_library(cvmmap_streamer_protobuf INTERFACE) target_include_directories(cvmmap_streamer_foxglove_proto PUBLIC "${CMAKE_CURRENT_BINARY_DIR}" ${Protobuf_INCLUDE_DIRS}) +target_include_directories(cvmmap_streamer_depth_proto + PUBLIC + "${CMAKE_CURRENT_BINARY_DIR}" + ${Protobuf_INCLUDE_DIRS}) target_include_directories(cvmmap_streamer_protobuf INTERFACE ${Protobuf_INCLUDE_DIRS}) @@ -78,6 +104,17 @@ if (TARGET PkgConfig::PROTOBUF_PKG) target_link_libraries(cvmmap_streamer_protobuf INTERFACE PkgConfig::PROTOBUF_PKG) endif() target_link_libraries(cvmmap_streamer_foxglove_proto PUBLIC cvmmap_streamer_protobuf) +target_link_libraries(cvmmap_streamer_depth_proto PUBLIC cvmmap_streamer_protobuf) + +add_library(cvmmap_streamer_mcap_runtime STATIC + src/record/mcap_runtime.cpp) +target_include_directories(cvmmap_streamer_mcap_runtime + PUBLIC + "${CMAKE_CURRENT_LIST_DIR}/lib/mcap/include") +target_link_libraries(cvmmap_streamer_mcap_runtime + PUBLIC + PkgConfig::ZSTD + PkgConfig::LZ4) add_library(cvmmap_streamer_common STATIC src/ipc/help.cpp @@ -107,9 +144,12 @@ target_include_directories(cvmmap_streamer_common set(CVMMAP_STREAMER_LINK_DEPS Threads::Threads cvmmap_streamer_foxglove_proto + cvmmap_streamer_depth_proto + cvmmap_streamer_mcap_runtime PkgConfig::FFMPEG PkgConfig::ZSTD PkgConfig::LZ4 + rvl::rvl cvmmap::client) if (TARGET cppzmq::cppzmq) @@ -154,7 +194,9 @@ function(add_cvmmap_binary target source) target_link_libraries(${target} PRIVATE cvmmap_streamer_common) - set_target_properties(${target} PROPERTIES OUTPUT_NAME "${target}") + set_target_properties(${target} PROPERTIES + OUTPUT_NAME "${target}" + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/bin") endfunction() add_cvmmap_binary(cvmmap_streamer src/main_streamer.cpp) @@ -163,6 +205,7 @@ add_cvmmap_binary(rtp_output_tester src/testers/rtp_output_tester.cpp) add_cvmmap_binary(rtmp_stub_tester src/testers/rtmp_stub_tester.cpp) add_cvmmap_binary(rtmp_output_tester src/testers/rtmp_output_tester.cpp) add_cvmmap_binary(ipc_snapshot_tester src/testers/ipc_snapshot_tester.cpp) +add_cvmmap_binary(mcap_depth_record_tester src/testers/mcap_depth_record_tester.cpp) add_executable(mcap_reader_tester src/testers/mcap_reader_tester.cpp) target_include_directories(mcap_reader_tester @@ -175,6 +218,8 @@ target_include_directories(mcap_reader_tester target_link_libraries(mcap_reader_tester PRIVATE cvmmap_streamer_foxglove_proto + cvmmap_streamer_depth_proto + cvmmap_streamer_mcap_runtime PkgConfig::ZSTD PkgConfig::LZ4) if (TARGET spdlog::spdlog) @@ -189,6 +234,9 @@ target_link_libraries(mcap_reader_tester PRIVATE cvmmap_streamer_protobuf) if (TARGET PkgConfig::PROTOBUF_PKG) target_link_libraries(mcap_reader_tester PRIVATE PkgConfig::PROTOBUF_PKG) endif() +set_target_properties(mcap_reader_tester PROPERTIES + OUTPUT_NAME "mcap_reader_tester" + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/bin") add_executable(mcap_replay_tester src/testers/mcap_replay_tester.cpp) target_include_directories(mcap_replay_tester @@ -202,6 +250,7 @@ target_link_libraries(mcap_replay_tester PRIVATE Threads::Threads cvmmap_streamer_foxglove_proto + cvmmap_streamer_mcap_runtime PkgConfig::ZSTD PkgConfig::LZ4) if (TARGET spdlog::spdlog) @@ -216,3 +265,6 @@ target_link_libraries(mcap_replay_tester PRIVATE cvmmap_streamer_protobuf) if (TARGET PkgConfig::PROTOBUF_PKG) target_link_libraries(mcap_replay_tester PRIVATE PkgConfig::PROTOBUF_PKG) endif() +set_target_properties(mcap_replay_tester PROPERTIES + OUTPUT_NAME "mcap_replay_tester" + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/bin") diff --git a/include/cvmmap_streamer/config/runtime_config.hpp b/include/cvmmap_streamer/config/runtime_config.hpp index 3f47144..6004206 100644 --- a/include/cvmmap_streamer/config/runtime_config.hpp +++ b/include/cvmmap_streamer/config/runtime_config.hpp @@ -83,6 +83,7 @@ struct McapRecordConfig { bool enabled{false}; std::string path{"capture.mcap"}; std::string topic{"/camera/video"}; + std::string depth_topic{"/camera/depth"}; std::string frame_id{"camera"}; McapCompression compression{McapCompression::Zstd}; }; diff --git a/include/cvmmap_streamer/ipc/contracts.hpp b/include/cvmmap_streamer/ipc/contracts.hpp index 1096ffc..cae4012 100644 --- a/include/cvmmap_streamer/ipc/contracts.hpp +++ b/include/cvmmap_streamer/ipc/contracts.hpp @@ -45,6 +45,12 @@ enum class PixelFormat : std::uint8_t { YUYV, }; +enum class DepthUnit : std::uint8_t { + Unknown = 0, + Millimeter = 1, + Meter = 2, +}; + enum class ModuleStatus : std::int32_t { Online = 0xa1, Offline = 0xa0, @@ -57,6 +63,7 @@ enum class ParseError { InvalidMagic, UnsupportedVersion, InvalidDepth, + InvalidDepthUnit, InvalidPixelFormat, InvalidModuleStatus, PayloadLengthMismatch, @@ -155,11 +162,23 @@ struct ControlResponseMessage { struct ValidatedShmView { FrameMetadata metadata; + DepthUnit depth_unit{DepthUnit::Unknown}; std::span payload; + std::span left; + std::optional depth_info{}; + std::span depth{}; + std::optional confidence_info{}; + std::span confidence{}; }; struct CoherentSnapshot { FrameMetadata metadata; + DepthUnit depth_unit{DepthUnit::Unknown}; + std::span left; + std::optional depth_info{}; + std::span depth{}; + std::optional confidence_info{}; + std::span confidence{}; std::size_t bytes_copied; }; diff --git a/include/cvmmap_streamer/record/mcap_record_sink.hpp b/include/cvmmap_streamer/record/mcap_record_sink.hpp index f5356a5..9fd29e8 100644 --- a/include/cvmmap_streamer/record/mcap_record_sink.hpp +++ b/include/cvmmap_streamer/record/mcap_record_sink.hpp @@ -2,13 +2,30 @@ #include "cvmmap_streamer/config/runtime_config.hpp" #include "cvmmap_streamer/encode/encoded_access_unit.hpp" +#include "cvmmap_streamer/ipc/contracts.hpp" +#include +#include #include +#include #include #include namespace cvmmap_streamer::record { +enum class DepthEncoding { + RvlU16Lossless, + RvlF32, +}; + +struct RawDepthMapView { + std::uint64_t timestamp_ns{0}; + std::uint32_t width{0}; + std::uint32_t height{0}; + ipc::DepthUnit source_unit{ipc::DepthUnit::Unknown}; + std::span pixels{}; +}; + class McapRecordSink { public: McapRecordSink() = default; @@ -31,6 +48,9 @@ public: [[nodiscard]] std::expected write_access_unit(const encode::EncodedAccessUnit &access_unit); + [[nodiscard]] + std::expected write_depth_map(const RawDepthMapView &depth_map); + [[nodiscard]] bool is_open() const; diff --git a/proto/cvmmap_streamer/DepthMap.proto b/proto/cvmmap_streamer/DepthMap.proto new file mode 100644 index 0000000..1cb8a41 --- /dev/null +++ b/proto/cvmmap_streamer/DepthMap.proto @@ -0,0 +1,34 @@ +syntax = "proto3"; + +package cvmmap_streamer; + +import "google/protobuf/timestamp.proto"; + +message DepthMap { + enum DepthUnit { + DEPTH_UNIT_UNKNOWN = 0; + DEPTH_UNIT_MILLIMETER = 1; + DEPTH_UNIT_METER = 2; + } + + enum StorageUnit { + STORAGE_UNIT_UNKNOWN = 0; + STORAGE_UNIT_MILLIMETER = 1; + STORAGE_UNIT_METER = 2; + } + + enum Encoding { + ENCODING_UNKNOWN = 0; + RVL_U16_LOSSLESS = 1; + RVL_F32 = 2; + } + + google.protobuf.Timestamp timestamp = 1; + string frame_id = 2; + uint32 width = 3; + uint32 height = 4; + DepthUnit source_unit = 5; + StorageUnit storage_unit = 6; + Encoding encoding = 7; + bytes data = 8; +} diff --git a/src/config/runtime_config.cpp b/src/config/runtime_config.cpp index 8e6cd2e..98e9c6e 100644 --- a/src/config/runtime_config.cpp +++ b/src/config/runtime_config.cpp @@ -358,6 +358,10 @@ std::expected apply_toml_file(RuntimeConfig &config, const st config.record.mcap.enabled = true; config.record.mcap.topic = *value; } + if (auto value = toml_value(table, "record.mcap.depth_topic")) { + config.record.mcap.enabled = true; + config.record.mcap.depth_topic = *value; + } if (auto value = toml_value(table, "record.mcap.frame_id")) { config.record.mcap.enabled = true; config.record.mcap.frame_id = *value; @@ -555,6 +559,7 @@ std::expected parse_runtime_config(int argc, char ** std::string rtp_sdp_raw{}; std::string mcap_path_raw{}; std::string mcap_topic_raw{}; + std::string mcap_depth_topic_raw{}; std::string mcap_frame_id_raw{}; std::string mcap_compression_raw{}; std::string queue_size_raw{}; @@ -594,6 +599,7 @@ std::expected parse_runtime_config(int argc, char ** app.add_flag("--mcap", mcap_enabled); app.add_option("--mcap-path", mcap_path_raw); app.add_option("--mcap-topic", mcap_topic_raw); + app.add_option("--mcap-depth-topic", mcap_depth_topic_raw); app.add_option("--mcap-frame-id", mcap_frame_id_raw); app.add_option("--mcap-compression", mcap_compression_raw); app.add_option("--queue-size", queue_size_raw); @@ -702,6 +708,10 @@ std::expected parse_runtime_config(int argc, char ** config.record.mcap.enabled = true; config.record.mcap.topic = mcap_topic_raw; } + if (!mcap_depth_topic_raw.empty()) { + config.record.mcap.enabled = true; + config.record.mcap.depth_topic = mcap_depth_topic_raw; + } if (!mcap_frame_id_raw.empty()) { config.record.mcap.enabled = true; config.record.mcap.frame_id = mcap_frame_id_raw; @@ -833,6 +843,9 @@ std::expected validate_runtime_config(const RuntimeConfig &co if (config.record.mcap.topic.empty()) { return std::unexpected("invalid MCAP config: topic must not be empty"); } + if (config.record.mcap.depth_topic.empty()) { + return std::unexpected("invalid MCAP config: depth_topic must not be empty"); + } if (config.record.mcap.frame_id.empty()) { return std::unexpected("invalid MCAP config: frame_id must not be empty"); } @@ -871,6 +884,7 @@ std::string summarize_runtime_config(const RuntimeConfig &config) { ss << ", mcap.enabled=" << (config.record.mcap.enabled ? "true" : "false"); ss << ", mcap.path=" << config.record.mcap.path; ss << ", mcap.topic=" << config.record.mcap.topic; + ss << ", mcap.depth_topic=" << config.record.mcap.depth_topic; ss << ", mcap.frame_id=" << config.record.mcap.frame_id; ss << ", mcap.compression=" << to_string(config.record.mcap.compression); ss << ", latency.queue_size=" << config.latency.queue_size; diff --git a/src/ipc/contracts.cpp b/src/ipc/contracts.cpp index d102efc..af84d93 100644 --- a/src/ipc/contracts.cpp +++ b/src/ipc/contracts.cpp @@ -97,6 +97,19 @@ namespace { return static_cast(pixel_format_raw); } + [[nodiscard]] + DepthUnit from_core_depth_unit(const cvmmap::DepthUnit unit) { + switch (unit) { + case cvmmap::DepthUnit::Millimeter: + return DepthUnit::Millimeter; + case cvmmap::DepthUnit::Meter: + return DepthUnit::Meter; + case cvmmap::DepthUnit::Unknown: + default: + return DepthUnit::Unknown; + } + } + [[nodiscard]] std::expected validate_module_status(std::int32_t status_raw) { switch (status_raw) { @@ -119,6 +132,9 @@ namespace { if (error.contains("version")) { return ParseError::UnsupportedVersion; } + if (error.contains("depth_unit")) { + return ParseError::InvalidDepthUnit; + } if (error.contains("depth")) { return ParseError::InvalidDepth; } @@ -145,6 +161,58 @@ namespace { return converted; } + [[nodiscard]] + FrameInfo from_core_frame_info(const cvmmap::frame_info_t &info) { + FrameInfo converted{}; + converted.width = info.width; + converted.height = info.height; + converted.channels = info.channels; + converted.depth = static_cast(info.depth); + converted.pixel_format = static_cast(info.pixel_format); + converted.buffer_size = info.buffer_size; + return converted; + } + + [[nodiscard]] + std::size_t span_offset( + const std::span outer, + const std::span inner) { + if (inner.empty()) { + return 0; + } + return static_cast(inner.data() - outer.data()); + } + + [[nodiscard]] + std::size_t payload_bytes_for( + const std::span payload_region, + const cvmmap::parsed_frame_metadata_t &metadata) { + std::size_t payload_bytes = metadata.left_plane.size(); + const auto consider = [&](std::span plane) { + if (plane.empty()) { + return; + } + payload_bytes = std::max( + payload_bytes, + span_offset(payload_region, plane) + plane.size()); + }; + consider(metadata.depth_plane); + consider(metadata.confidence_plane); + return payload_bytes; + } + + [[nodiscard]] + std::span translate_span( + const std::span source_payload, + const std::span destination_payload, + const std::span source_plane) { + if (source_plane.empty()) { + return {}; + } + const auto offset = span_offset(source_payload, source_plane); + return destination_payload.subspan(offset, source_plane.size()); + } + } std::string_view to_string(ParseError error) { @@ -159,6 +227,8 @@ std::string_view to_string(ParseError error) { return "unsupported version"; case ParseError::InvalidDepth: return "invalid depth"; + case ParseError::InvalidDepthUnit: + return "invalid depth unit"; case ParseError::InvalidPixelFormat: return "invalid pixel format"; case ParseError::InvalidModuleStatus: @@ -344,9 +414,22 @@ std::expected validate_shm_region(std::spannormalized_metadata), - .payload = metadata_result->left_plane}; + .depth_unit = from_core_depth_unit(metadata_result->depth_unit), + .payload = payload_region.first(payload_bytes), + .left = metadata_result->left_plane, + .depth_info = metadata_result->depth_info + ? std::optional(from_core_frame_info(*metadata_result->depth_info)) + : std::nullopt, + .depth = metadata_result->depth_plane, + .confidence_info = metadata_result->confidence_info + ? std::optional(from_core_frame_info(*metadata_result->confidence_info)) + : std::nullopt, + .confidence = metadata_result->confidence_plane}; } std::expected read_coherent_snapshot( @@ -378,8 +461,16 @@ std::expected read_coherent_snapshot( return std::unexpected(SnapshotError::TornRead); } + const auto copied_payload = std::span(destination.data(), first->payload.size()); + return CoherentSnapshot{ .metadata = first->metadata, + .depth_unit = first->depth_unit, + .left = translate_span(first->payload, copied_payload, first->left), + .depth_info = first->depth_info, + .depth = translate_span(first->payload, copied_payload, first->depth), + .confidence_info = first->confidence_info, + .confidence = translate_span(first->payload, copied_payload, first->confidence), .bytes_copied = first->payload.size()}; } diff --git a/src/ipc/help.cpp b/src/ipc/help.cpp index cee6a2b..9f5eece 100644 --- a/src/ipc/help.cpp +++ b/src/ipc/help.cpp @@ -34,6 +34,7 @@ constexpr std::array kHelpLines{ " --mcap\t\tenable MCAP recording", " --mcap-path \tMCAP output file", " --mcap-topic \tMCAP topic name", + " --mcap-depth-topic \tMCAP depth topic name", " --mcap-frame-id \tFoxglove CompressedVideo frame_id", " --mcap-compression \tnone|lz4|zstd", "", diff --git a/src/pipeline/pipeline_runtime.cpp b/src/pipeline/pipeline_runtime.cpp index a3a76b5..b999d5d 100644 --- a/src/pipeline/pipeline_runtime.cpp +++ b/src/pipeline/pipeline_runtime.cpp @@ -192,6 +192,44 @@ bool frame_info_equal(const ipc::FrameInfo &lhs, const ipc::FrameInfo &rhs) { lhs.buffer_size == rhs.buffer_size; } +[[nodiscard]] +std::expected make_depth_map_view(const ipc::CoherentSnapshot &snapshot) { + if (!snapshot.depth_info) { + return std::unexpected("depth plane metadata is missing"); + } + if (snapshot.depth.empty()) { + return std::unexpected("depth plane bytes are missing"); + } + if (snapshot.depth_unit == ipc::DepthUnit::Unknown) { + return std::unexpected("depth plane unit is unknown"); + } + + const auto &depth_info = *snapshot.depth_info; + if (depth_info.depth != ipc::Depth::F32 || depth_info.pixel_format != ipc::PixelFormat::GRAY) { + return std::unexpected("depth plane must be GRAY/F32"); + } + + const auto pixel_count = static_cast(depth_info.width) * static_cast(depth_info.height); + const auto expected_bytes = pixel_count * sizeof(float); + if (snapshot.depth.size() != expected_bytes) { + return std::unexpected( + "depth plane byte size does not match width*height*sizeof(float)"); + } + if ((reinterpret_cast(snapshot.depth.data()) % alignof(float)) != 0) { + return std::unexpected("depth plane is not aligned for float access"); + } + + return record::RawDepthMapView{ + .timestamp_ns = snapshot.metadata.timestamp_ns, + .width = depth_info.width, + .height = depth_info.height, + .source_unit = snapshot.depth_unit, + .pixels = std::span( + reinterpret_cast(snapshot.depth.data()), + pixel_count), + }; +} + [[nodiscard]] Status publish_access_units( const RuntimeConfig &config, @@ -338,6 +376,7 @@ int run_pipeline(const RuntimeConfig &config) { std::optional active_info{}; std::optional restart_target_info{}; bool restart_pending{false}; + bool warned_unknown_depth_unit{false}; const auto restart_backend = [&](std::string_view reason, std::optional target_info) { if (started) { @@ -352,6 +391,7 @@ int run_pipeline(const RuntimeConfig &config) { started = false; restart_pending = true; restart_target_info = target_info; + warned_unknown_depth_unit = false; rtmp_output.reset(); }; @@ -404,6 +444,7 @@ int run_pipeline(const RuntimeConfig &config) { started = true; restart_pending = false; restart_target_info.reset(); + warned_unknown_depth_unit = false; active_info = target_info; return {}; }; @@ -510,6 +551,29 @@ int run_pipeline(const RuntimeConfig &config) { continue; } + if (mcap_sink.has_value() && !snapshot->depth.empty()) { + if (snapshot->depth_unit == ipc::DepthUnit::Unknown) { + if (!warned_unknown_depth_unit) { + spdlog::warn("pipeline depth plane present but depth_unit is unknown; skipping depth MCAP recording"); + warned_unknown_depth_unit = true; + } + } else { + auto depth_map = make_depth_map_view(*snapshot); + if (!depth_map) { + const auto reason = "pipeline depth snapshot invalid: " + depth_map.error(); + restart_backend(reason, active_info); + continue; + } + + auto write_depth = mcap_sink->write_depth_map(*depth_map); + if (!write_depth) { + const auto reason = "pipeline depth MCAP write failed: " + write_depth.error(); + restart_backend(reason, active_info); + continue; + } + } + } + stats.pushed_frames += 1; auto drain = drain_encoder( config, diff --git a/src/record/mcap_record_sink.cpp b/src/record/mcap_record_sink.cpp index 0791e5e..016ac09 100644 --- a/src/record/mcap_record_sink.cpp +++ b/src/record/mcap_record_sink.cpp @@ -1,16 +1,19 @@ -#define MCAP_IMPLEMENTATION #include #include "cvmmap_streamer/record/mcap_record_sink.hpp" #include "protobuf_descriptor.hpp" +#include "cvmmap_streamer/DepthMap.pb.h" #include "foxglove/CompressedVideo.pb.h" +#include #include +#include #include #include #include +#include #include #include #include @@ -22,11 +25,20 @@ namespace cvmmap_streamer::record { namespace { +constexpr float kRvlDepthQuantization = 200.0f; +constexpr float kMinDepthMaxMeters = 20.0f; + [[nodiscard]] std::string codec_format(CodecType codec) { return codec == CodecType::H265 ? "h265" : "h264"; } +struct EncodedDepthPayload { + DepthEncoding encoding{DepthEncoding::RvlF32}; + ipc::DepthUnit storage_unit{ipc::DepthUnit::Unknown}; + std::vector bytes{}; +}; + [[nodiscard]] mcap::Compression to_mcap_compression(McapCompression compression) { switch (compression) { @@ -48,6 +60,43 @@ google::protobuf::Timestamp to_proto_timestamp(std::uint64_t timestamp_ns) { return timestamp; } +[[nodiscard]] +cvmmap_streamer::DepthMap::DepthUnit to_proto_depth_unit(ipc::DepthUnit unit) { + switch (unit) { + case ipc::DepthUnit::Millimeter: + return cvmmap_streamer::DepthMap::DEPTH_UNIT_MILLIMETER; + case ipc::DepthUnit::Meter: + return cvmmap_streamer::DepthMap::DEPTH_UNIT_METER; + case ipc::DepthUnit::Unknown: + default: + return cvmmap_streamer::DepthMap::DEPTH_UNIT_UNKNOWN; + } +} + +[[nodiscard]] +cvmmap_streamer::DepthMap::StorageUnit to_proto_storage_unit(ipc::DepthUnit unit) { + switch (unit) { + case ipc::DepthUnit::Millimeter: + return cvmmap_streamer::DepthMap::STORAGE_UNIT_MILLIMETER; + case ipc::DepthUnit::Meter: + return cvmmap_streamer::DepthMap::STORAGE_UNIT_METER; + case ipc::DepthUnit::Unknown: + default: + return cvmmap_streamer::DepthMap::STORAGE_UNIT_UNKNOWN; + } +} + +[[nodiscard]] +cvmmap_streamer::DepthMap::Encoding to_proto_depth_encoding(DepthEncoding encoding) { + switch (encoding) { + case DepthEncoding::RvlU16Lossless: + return cvmmap_streamer::DepthMap::RVL_U16_LOSSLESS; + case DepthEncoding::RvlF32: + default: + return cvmmap_streamer::DepthMap::RVL_F32; + } +} + void append_start_code(std::vector &output) { output.push_back(0x00); output.push_back(0x00); @@ -176,14 +225,96 @@ std::expected, std::string> decoder_config_to_annexb( return avcc_to_annexb(decoder_config); } +[[nodiscard]] +bool can_encode_lossless_u16_mm(const RawDepthMapView &depth_map) { + if (depth_map.source_unit != ipc::DepthUnit::Millimeter) { + return false; + } + + for (const float sample : depth_map.pixels) { + if (!std::isfinite(sample) || sample <= 0.0f) { + continue; + } + if (sample > static_cast(std::numeric_limits::max())) { + return false; + } + if (std::fabs(sample - std::round(sample)) > 1e-3f) { + return false; + } + } + + return true; +} + +[[nodiscard]] +std::expected encode_depth_payload(const RawDepthMapView &depth_map) { + const auto pixel_count = static_cast(depth_map.width) * static_cast(depth_map.height); + if (depth_map.width == 0 || depth_map.height == 0) { + return std::unexpected("depth map dimensions must be non-zero"); + } + if (pixel_count != depth_map.pixels.size()) { + return std::unexpected("depth map dimensions do not match the pixel buffer"); + } + if (depth_map.source_unit == ipc::DepthUnit::Unknown) { + return std::unexpected("depth source unit is unknown"); + } + + try { + if (can_encode_lossless_u16_mm(depth_map)) { + std::vector pixels(pixel_count, 0); + for (std::size_t index = 0; index < pixel_count; ++index) { + const float sample = depth_map.pixels[index]; + if (!std::isfinite(sample) || sample <= 0.0f) { + continue; + } + pixels[index] = static_cast(std::lrint(sample)); + } + + return EncodedDepthPayload{ + .encoding = DepthEncoding::RvlU16Lossless, + .storage_unit = ipc::DepthUnit::Millimeter, + .bytes = rvl::compress_image(pixels, depth_map.height, depth_map.width), + }; + } + + std::vector depth_m(pixel_count, std::numeric_limits::quiet_NaN()); + float finite_max_m = 0.0f; + for (std::size_t index = 0; index < pixel_count; ++index) { + float sample = depth_map.pixels[index]; + if (depth_map.source_unit == ipc::DepthUnit::Millimeter && std::isfinite(sample)) { + sample *= 0.001f; + } + if (!std::isfinite(sample) || sample <= 0.0f) { + continue; + } + + depth_m[index] = sample; + finite_max_m = std::max(finite_max_m, sample); + } + + const auto parameters = rvl::make_quantization_parameters( + std::max(finite_max_m, kMinDepthMaxMeters), + kRvlDepthQuantization); + return EncodedDepthPayload{ + .encoding = DepthEncoding::RvlF32, + .storage_unit = ipc::DepthUnit::Meter, + .bytes = rvl::compress_float_image(depth_m, depth_map.height, depth_map.width, parameters), + }; + } catch (const std::exception &error) { + return std::unexpected(std::string("failed to RVL-encode depth map: ") + error.what()); + } +} + } struct McapRecordSink::State { mcap::McapWriter writer{}; std::string path{}; std::string frame_id{}; - mcap::ChannelId channel_id{0}; - std::uint32_t sequence{0}; + mcap::ChannelId video_channel_id{0}; + mcap::ChannelId depth_channel_id{0}; + std::uint32_t video_sequence{0}; + std::uint32_t depth_sequence{0}; CodecType codec{CodecType::H264}; std::vector keyframe_preamble{}; }; @@ -232,7 +363,20 @@ std::expected McapRecordSink::create( mcap::Channel channel(config.record.mcap.topic, "protobuf", schema.id); state->writer.addChannel(channel); - state->channel_id = channel.id; + state->video_channel_id = channel.id; + + const auto depth_descriptor_set = build_file_descriptor_set(cvmmap_streamer::DepthMap::descriptor()); + std::string depth_schema_bytes{}; + if (!depth_descriptor_set.SerializeToString(&depth_schema_bytes)) { + return std::unexpected("failed to serialize cvmmap_streamer.DepthMap descriptor set"); + } + + mcap::Schema depth_schema("cvmmap_streamer.DepthMap", "protobuf", depth_schema_bytes); + state->writer.addSchema(depth_schema); + + mcap::Channel depth_channel(config.record.mcap.depth_topic, "protobuf", depth_schema.id); + state->writer.addChannel(depth_channel); + state->depth_channel_id = depth_channel.id; sink.state_ = state.release(); auto update = sink.update_stream_info(stream_info); @@ -281,8 +425,8 @@ std::expected McapRecordSink::write_access_unit(const encode: } mcap::Message record{}; - record.channelId = state_->channel_id; - record.sequence = state_->sequence++; + record.channelId = state_->video_channel_id; + record.sequence = state_->video_sequence++; record.logTime = access_unit.source_timestamp_ns; record.publishTime = access_unit.source_timestamp_ns; record.data = reinterpret_cast(serialized.data()); @@ -295,6 +439,48 @@ std::expected McapRecordSink::write_access_unit(const encode: return {}; } +std::expected McapRecordSink::write_depth_map(const RawDepthMapView &depth_map) { + if (state_ == nullptr) { + return std::unexpected("MCAP sink is not open"); + } + + auto encoded = encode_depth_payload(depth_map); + if (!encoded) { + return std::unexpected(encoded.error()); + } + + cvmmap_streamer::DepthMap message{}; + *message.mutable_timestamp() = to_proto_timestamp(depth_map.timestamp_ns); + message.set_frame_id(state_->frame_id); + message.set_width(depth_map.width); + message.set_height(depth_map.height); + message.set_source_unit(to_proto_depth_unit(depth_map.source_unit)); + message.set_storage_unit(to_proto_storage_unit(encoded->storage_unit)); + message.set_encoding(to_proto_depth_encoding(encoded->encoding)); + message.set_data( + reinterpret_cast(encoded->bytes.data()), + static_cast(encoded->bytes.size())); + + std::string serialized{}; + if (!message.SerializeToString(&serialized)) { + return std::unexpected("failed to serialize cvmmap_streamer.DepthMap"); + } + + mcap::Message record{}; + record.channelId = state_->depth_channel_id; + record.sequence = state_->depth_sequence++; + record.logTime = depth_map.timestamp_ns; + record.publishTime = depth_map.timestamp_ns; + record.data = reinterpret_cast(serialized.data()); + record.dataSize = serialized.size(); + + const auto write_status = state_->writer.write(record); + if (!write_status.ok()) { + return std::unexpected("failed to write MCAP depth message: " + write_status.message); + } + return {}; +} + bool McapRecordSink::is_open() const { return state_ != nullptr; } diff --git a/src/record/mcap_runtime.cpp b/src/record/mcap_runtime.cpp new file mode 100644 index 0000000..65f8167 --- /dev/null +++ b/src/record/mcap_runtime.cpp @@ -0,0 +1,3 @@ +#define MCAP_IMPLEMENTATION +#include +#include diff --git a/src/testers/ipc_snapshot_tester.cpp b/src/testers/ipc_snapshot_tester.cpp index 890f573..79db841 100644 --- a/src/testers/ipc_snapshot_tester.cpp +++ b/src/testers/ipc_snapshot_tester.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -29,12 +30,19 @@ constexpr std::size_t kVersionMajorOffset = 8; constexpr std::size_t kVersionMinorOffset = 9; constexpr std::size_t kFrameCountOffset = 12; constexpr std::size_t kTimestampOffset = 16; -constexpr std::size_t kInfoWidthOffset = 24; -constexpr std::size_t kInfoHeightOffset = 26; -constexpr std::size_t kInfoChannelsOffset = 28; -constexpr std::size_t kInfoDepthOffset = 29; -constexpr std::size_t kInfoPixelFormatOffset = 30; -constexpr std::size_t kInfoBufferSizeOffset = 32; +constexpr std::size_t kPublishSeqOffset = 24; +constexpr std::size_t kPlaneCountOffset = 32; +constexpr std::size_t kPlaneMaskOffset = 33; +constexpr std::size_t kDescriptorOffsetField = 34; +constexpr std::size_t kDescriptorSizeField = 36; +constexpr std::size_t kDescriptorCapField = 38; +constexpr std::size_t kPayloadSizeOffset = 40; +constexpr std::size_t kDepthUnitOffset = 44; +constexpr std::size_t kDescriptorsOffset = 64; +constexpr std::size_t kDescriptorSizeBytes = 24; +constexpr std::size_t kLeftSizeBytes = 4; +constexpr std::size_t kDepthSizeBytes = 16; +constexpr std::size_t kPayloadSizeBytes = kLeftSizeBytes + kDepthSizeBytes; void write_u16_le(std::span buffer, std::size_t offset, std::uint16_t value) { buffer[offset] = static_cast(value & 0xffu); @@ -54,25 +62,71 @@ void write_u64_le(std::span buffer, std::size_t offset, std::uint6 } } +void write_descriptor( + std::span buffer, + std::size_t descriptor_index, + std::uint8_t plane_type, + std::uint8_t pixel_format, + std::uint8_t depth, + std::uint32_t width, + std::uint32_t height, + std::uint32_t stride_bytes, + std::uint32_t offset_bytes, + std::uint32_t size_bytes) { + const auto base = kDescriptorsOffset + descriptor_index * kDescriptorSizeBytes; + buffer[base + 0] = plane_type; + buffer[base + 1] = pixel_format; + buffer[base + 2] = depth; + write_u32_le(buffer, base + 4, width); + write_u32_le(buffer, base + 8, height); + write_u32_le(buffer, base + 12, stride_bytes); + write_u32_le(buffer, base + 16, offset_bytes); + write_u32_le(buffer, base + 20, size_bytes); +} + void write_metadata( std::span buffer, std::uint32_t frame_count, - std::uint64_t timestamp_ns, - std::uint32_t payload_size) { + std::uint64_t timestamp_ns) { std::copy( cvmmap_streamer::ipc::kFrameMetadataMagic.begin(), cvmmap_streamer::ipc::kFrameMetadataMagic.end(), buffer.begin() + kMagicOffset); - buffer[kVersionMajorOffset] = cvmmap_streamer::ipc::kVersionMajor; + buffer[kVersionMajorOffset] = 2; buffer[kVersionMinorOffset] = cvmmap_streamer::ipc::kVersionMinor; write_u32_le(buffer, kFrameCountOffset, frame_count); write_u64_le(buffer, kTimestampOffset, timestamp_ns); - write_u16_le(buffer, kInfoWidthOffset, 2); - write_u16_le(buffer, kInfoHeightOffset, 2); - buffer[kInfoChannelsOffset] = 1; - buffer[kInfoDepthOffset] = static_cast(cvmmap_streamer::ipc::Depth::U8); - buffer[kInfoPixelFormatOffset] = static_cast(cvmmap_streamer::ipc::PixelFormat::GRAY); - write_u32_le(buffer, kInfoBufferSizeOffset, payload_size); + write_u64_le(buffer, kPublishSeqOffset, frame_count); + buffer[kPlaneCountOffset] = 2; + buffer[kPlaneMaskOffset] = 0x03; + write_u16_le(buffer, kDescriptorOffsetField, static_cast(kDescriptorsOffset)); + write_u16_le(buffer, kDescriptorSizeField, static_cast(kDescriptorSizeBytes)); + write_u16_le(buffer, kDescriptorCapField, 4); + write_u32_le(buffer, kPayloadSizeOffset, static_cast(kPayloadSizeBytes)); + buffer[kDepthUnitOffset] = static_cast(cvmmap_streamer::ipc::DepthUnit::Millimeter); + + write_descriptor( + buffer, + 0, + 0, + static_cast(cvmmap_streamer::ipc::PixelFormat::GRAY), + static_cast(cvmmap_streamer::ipc::Depth::U8), + 2, + 2, + 2, + 0, + static_cast(kLeftSizeBytes)); + write_descriptor( + buffer, + 1, + 1, + static_cast(cvmmap_streamer::ipc::PixelFormat::GRAY), + static_cast(cvmmap_streamer::ipc::Depth::F32), + 2, + 2, + 8, + static_cast(kLeftSizeBytes), + static_cast(kDepthSizeBytes)); } } @@ -83,22 +137,38 @@ int main(int argc, char **argv) { return exit_code(TesterExitCode::Success); } - std::array shm{}; + std::array shm{}; auto shm_view = std::span(shm); - write_metadata(shm_view, 7, 2222, 32); - for (std::size_t i = 0; i < 32; ++i) { + write_metadata(shm_view, 7, 2222); + for (std::size_t i = 0; i < kLeftSizeBytes; ++i) { shm[cvmmap_streamer::ipc::kShmPayloadOffset + i] = static_cast(i + 1); } + const float depth_samples[4] = {1000.0f, 2000.0f, 0.0f, -1.0f}; + std::memcpy( + shm.data() + cvmmap_streamer::ipc::kShmPayloadOffset + kLeftSizeBytes, + depth_samples, + sizeof(depth_samples)); - std::array destination{}; + std::array destination{}; auto valid = cvmmap_streamer::ipc::read_coherent_snapshot(shm_view, destination); if (!valid) { spdlog::error("coherent snapshot should succeed: {}", cvmmap_streamer::ipc::to_string(valid.error())); return exit_code(TesterExitCode::ReadError); } - if (valid->bytes_copied != 32 || valid->metadata.frame_count != 7 || valid->metadata.timestamp_ns != 2222) { + float copied_depth_first = 0.0f; + if (valid->depth.size() >= sizeof(float)) { + std::memcpy(&copied_depth_first, valid->depth.data(), sizeof(float)); + } + if (valid->bytes_copied != kPayloadSizeBytes || + valid->metadata.frame_count != 7 || + valid->metadata.timestamp_ns != 2222 || + valid->depth_unit != cvmmap_streamer::ipc::DepthUnit::Millimeter || + valid->left.size() != kLeftSizeBytes || + valid->depth.size() != kDepthSizeBytes || + !valid->depth_info.has_value() || + copied_depth_first != 1000.0f) { spdlog::error("valid snapshot verification failed"); return exit_code(TesterExitCode::VerificationError); } diff --git a/src/testers/mcap_depth_record_tester.cpp b/src/testers/mcap_depth_record_tester.cpp new file mode 100644 index 0000000..ae2be32 --- /dev/null +++ b/src/testers/mcap_depth_record_tester.cpp @@ -0,0 +1,238 @@ +#include + +#include "cvmmap_streamer/DepthMap.pb.h" +#include "cvmmap_streamer/common.h" +#include "cvmmap_streamer/record/mcap_record_sink.hpp" +#include "foxglove/CompressedVideo.pb.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace { + +enum class TesterExitCode : int { + Success = 0, + CreateError = 2, + WriteError = 3, + OpenError = 4, + VerificationError = 5, +}; + +[[nodiscard]] +constexpr int exit_code(TesterExitCode code) { + return static_cast(code); +} + +[[nodiscard]] +bool approx_equal(float lhs, float rhs, float tolerance) { + return std::fabs(lhs - rhs) <= tolerance; +} + +} + +int main(int argc, char **argv) { + if (cvmmap_streamer::has_help_flag(argc, argv)) { + cvmmap_streamer::print_help("mcap_depth_record_tester"); + return exit_code(TesterExitCode::Success); + } + + const std::filesystem::path output_path = + argc > 1 + ? std::filesystem::path(argv[1]) + : std::filesystem::temp_directory_path() / "cvmmap_streamer_depth_record_test.mcap"; + if (output_path.has_parent_path()) { + std::filesystem::create_directories(output_path.parent_path()); + } + + cvmmap_streamer::RuntimeConfig config = cvmmap_streamer::RuntimeConfig::defaults(); + config.record.mcap.enabled = true; + config.record.mcap.path = output_path.string(); + config.record.mcap.topic = "/camera/video"; + config.record.mcap.depth_topic = "/camera/depth"; + config.record.mcap.frame_id = "camera"; + config.record.mcap.compression = cvmmap_streamer::McapCompression::None; + + cvmmap_streamer::encode::EncodedStreamInfo stream_info{}; + stream_info.codec = cvmmap_streamer::CodecType::H264; + + auto sink = cvmmap_streamer::record::McapRecordSink::create(config, stream_info); + if (!sink) { + spdlog::error("failed to create MCAP sink: {}", sink.error()); + return exit_code(TesterExitCode::CreateError); + } + + cvmmap_streamer::encode::EncodedAccessUnit access_unit{}; + access_unit.codec = cvmmap_streamer::CodecType::H264; + access_unit.source_timestamp_ns = 10; + access_unit.stream_pts_ns = 10; + access_unit.keyframe = false; + access_unit.annexb_bytes = {0x00, 0x00, 0x00, 0x01, 0x09, 0x10}; + if (auto write = sink->write_access_unit(access_unit); !write) { + spdlog::error("failed to write video access unit: {}", write.error()); + return exit_code(TesterExitCode::WriteError); + } + + const float depth_mm_pixels[4] = { + 1000.0f, + 2000.0f, + std::numeric_limits::quiet_NaN(), + 0.0f, + }; + cvmmap_streamer::record::RawDepthMapView depth_mm{}; + depth_mm.timestamp_ns = 11; + depth_mm.width = 2; + depth_mm.height = 2; + depth_mm.source_unit = cvmmap_streamer::ipc::DepthUnit::Millimeter; + depth_mm.pixels = depth_mm_pixels; + if (auto write = sink->write_depth_map(depth_mm); !write) { + spdlog::error("failed to write millimeter depth map: {}", write.error()); + return exit_code(TesterExitCode::WriteError); + } + + const float depth_m_pixels[4] = { + 1.0f, + 2.0f, + std::numeric_limits::quiet_NaN(), + 0.0f, + }; + cvmmap_streamer::record::RawDepthMapView depth_m{}; + depth_m.timestamp_ns = 12; + depth_m.width = 2; + depth_m.height = 2; + depth_m.source_unit = cvmmap_streamer::ipc::DepthUnit::Meter; + depth_m.pixels = depth_m_pixels; + if (auto write = sink->write_depth_map(depth_m); !write) { + spdlog::error("failed to write meter depth map: {}", write.error()); + return exit_code(TesterExitCode::WriteError); + } + + sink->close(); + + mcap::McapReader reader{}; + const auto open_status = reader.open(output_path.string()); + if (!open_status.ok()) { + spdlog::error("failed to open MCAP file '{}': {}", output_path.string(), open_status.message); + return exit_code(TesterExitCode::OpenError); + } + + std::uint64_t video_messages{0}; + std::vector depth_messages{}; + + auto messages = reader.readMessages(); + for (auto it = messages.begin(); it != messages.end(); ++it) { + if (it->schema == nullptr || it->channel == nullptr) { + spdlog::error("MCAP message missing schema or channel"); + reader.close(); + return exit_code(TesterExitCode::VerificationError); + } + if (it->schema->encoding != "protobuf" || it->channel->messageEncoding != "protobuf") { + spdlog::error("unexpected schema encoding in MCAP file"); + reader.close(); + return exit_code(TesterExitCode::VerificationError); + } + + if (it->schema->name == "foxglove.CompressedVideo") { + foxglove::CompressedVideo video{}; + if (!video.ParseFromArray(it->message.data, static_cast(it->message.dataSize))) { + spdlog::error("failed to parse foxglove.CompressedVideo payload"); + reader.close(); + return exit_code(TesterExitCode::VerificationError); + } + if (it->channel->topic != "/camera/video" || video.frame_id() != "camera" || video.data().empty()) { + spdlog::error("video MCAP payload verification failed"); + reader.close(); + return exit_code(TesterExitCode::VerificationError); + } + video_messages += 1; + continue; + } + + if (it->schema->name == "cvmmap_streamer.DepthMap") { + cvmmap_streamer::DepthMap depth{}; + if (!depth.ParseFromArray(it->message.data, static_cast(it->message.dataSize))) { + spdlog::error("failed to parse cvmmap_streamer.DepthMap payload"); + reader.close(); + return exit_code(TesterExitCode::VerificationError); + } + if (it->channel->topic != "/camera/depth" || depth.frame_id() != "camera") { + spdlog::error("depth MCAP payload verification failed"); + reader.close(); + return exit_code(TesterExitCode::VerificationError); + } + depth_messages.push_back(std::move(depth)); + } + } + + reader.close(); + + if (video_messages != 1 || depth_messages.size() != 2) { + spdlog::error( + "unexpected message counts: video={} depth={}", + video_messages, + depth_messages.size()); + return exit_code(TesterExitCode::VerificationError); + } + + const auto &mm_message = depth_messages[0]; + if (mm_message.source_unit() != cvmmap_streamer::DepthMap::DEPTH_UNIT_MILLIMETER || + mm_message.storage_unit() != cvmmap_streamer::DepthMap::STORAGE_UNIT_MILLIMETER || + mm_message.encoding() != cvmmap_streamer::DepthMap::RVL_U16_LOSSLESS) { + spdlog::error("millimeter depth metadata verification failed"); + return exit_code(TesterExitCode::VerificationError); + } + const auto mm_info = rvl::inspect_image(std::span( + reinterpret_cast(mm_message.data().data()), + mm_message.data().size())); + const auto mm_decoded = rvl::decompress_image(std::span( + reinterpret_cast(mm_message.data().data()), + mm_message.data().size())); + if (mm_info.format != rvl::ImageFormat::UInt16Lossless || + mm_info.rows != 2 || + mm_info.cols != 2 || + mm_decoded.pixels.size() != 4 || + mm_decoded.pixels[0] != 1000 || + mm_decoded.pixels[1] != 2000 || + mm_decoded.pixels[2] != 0 || + mm_decoded.pixels[3] != 0) { + spdlog::error("millimeter RVL round-trip verification failed"); + return exit_code(TesterExitCode::VerificationError); + } + + const auto &m_message = depth_messages[1]; + if (m_message.source_unit() != cvmmap_streamer::DepthMap::DEPTH_UNIT_METER || + m_message.storage_unit() != cvmmap_streamer::DepthMap::STORAGE_UNIT_METER || + m_message.encoding() != cvmmap_streamer::DepthMap::RVL_F32) { + spdlog::error("meter depth metadata verification failed"); + return exit_code(TesterExitCode::VerificationError); + } + const auto m_info = rvl::inspect_image(std::span( + reinterpret_cast(m_message.data().data()), + m_message.data().size())); + const auto m_decoded = rvl::decompress_float_image(std::span( + reinterpret_cast(m_message.data().data()), + m_message.data().size())); + if (m_info.format != rvl::ImageFormat::Float32InverseDepth || + m_info.rows != 2 || + m_info.cols != 2 || + m_decoded.pixels.size() != 4 || + !approx_equal(m_decoded.pixels[0], 1.0f, 0.02f) || + !approx_equal(m_decoded.pixels[1], 2.0f, 0.02f) || + !std::isnan(m_decoded.pixels[2]) || + !std::isnan(m_decoded.pixels[3])) { + spdlog::error("meter RVL round-trip verification failed"); + return exit_code(TesterExitCode::VerificationError); + } + + spdlog::info( + "validated same-file MCAP video+depth recording at '{}'", + output_path.string()); + return exit_code(TesterExitCode::Success); +} diff --git a/src/testers/mcap_reader_tester.cpp b/src/testers/mcap_reader_tester.cpp index b92b1a4..6af95f6 100644 --- a/src/testers/mcap_reader_tester.cpp +++ b/src/testers/mcap_reader_tester.cpp @@ -1,4 +1,3 @@ -#define MCAP_IMPLEMENTATION #include #include diff --git a/src/testers/mcap_replay_tester.cpp b/src/testers/mcap_replay_tester.cpp index de12412..005deaa 100644 --- a/src/testers/mcap_replay_tester.cpp +++ b/src/testers/mcap_replay_tester.cpp @@ -1,4 +1,3 @@ -#define MCAP_IMPLEMENTATION #include #include