crosstyan/cvmmap-streamer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(record): add depth RVL recording to MCAP

Browse Source
This commit is contained in:
crosstyan
2026-03-11 21:15:25 +08:00
parent 782af9481c
commit 59ff8b79d9
15 changed files with 826 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/record/mcap_record_sink.cpp
+192 -6
View File
@@ -1,16 +1,19 @@
#define MCAP_IMPLEMENTATION
#include <mcap/writer.hpp>
#include "cvmmap_streamer/record/mcap_record_sink.hpp"
#include "protobuf_descriptor.hpp"
#include "cvmmap_streamer/DepthMap.pb.h"
#include "foxglove/CompressedVideo.pb.h"
#include <rvl/rvl.hpp>
#include <google/protobuf/timestamp.pb.h>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <expected>
#include <limits>
#include <memory>
#include <span>
#include <string>
@@ -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<std::uint8_t> 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<std::uint8_t> &output) {
output.push_back(0x00);
output.push_back(0x00);
@@ -176,14 +225,96 @@ std::expected<std::vector<std::uint8_t>, 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<float>(std::numeric_limits<std::uint16_t>::max())) {
return false;
}
if (std::fabs(sample - std::round(sample)) > 1e-3f) {
return false;
}
}
return true;
}
[[nodiscard]]
std::expected<EncodedDepthPayload, std::string> encode_depth_payload(const RawDepthMapView &depth_map) {
const auto pixel_count = static_cast<std::size_t>(depth_map.width) * static_cast<std::size_t>(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<std::uint16_t> 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::uint16_t>(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<float> depth_m(pixel_count, std::numeric_limits<float>::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<std::uint8_t> keyframe_preamble{};
};
@@ -232,7 +363,20 @@ std::expected<McapRecordSink, std::string> 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<void, std::string> 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<const std::byte *>(serialized.data());
@@ -295,6 +439,48 @@ std::expected<void, std::string> McapRecordSink::write_access_unit(const encode:
return {};
}
std::expected<void, std::string> 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<const char *>(encoded->bytes.data()),
static_cast<int>(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<const std::byte *>(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;
}
src/record/mcap_runtime.cpp
+3
View File
@@ -0,0 +1,3 @@
#define MCAP_IMPLEMENTATION
#include <mcap/reader.hpp>
#include <mcap/writer.hpp>