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

Add ZED SVO to MCAP conversion tool

Browse Source
This commit is contained in:
crosstyan
2026-03-17 02:03:59 +00:00
parent ee53d1958e
commit 0fef0595fb
16 changed files with 1430 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/record/mcap_record_sink.cpp
+243 -26
View File
@@ -3,8 +3,10 @@
#include "cvmmap_streamer/record/mcap_record_sink.hpp"
#include "protobuf_descriptor.hpp"
#include "cvmmap_streamer/DepthMap.pb.h"
#include "foxglove/CompressedVideo.pb.h"
#include "proto/cvmmap_streamer/DepthMap.pb.h"
#include "proto/foxglove/CameraCalibration.pb.h"
#include "proto/foxglove/CompressedVideo.pb.h"
#include "proto/foxglove/PoseInFrame.pb.h"
#include <rvl/rvl.hpp>
#include <google/protobuf/timestamp.pb.h>
@@ -293,14 +295,14 @@ std::expected<EncodedDepthPayload, std::string> encode_depth_payload(const RawDe
};
}
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 (source_unit == ipc::DepthUnit::Millimeter && std::isfinite(sample)) {
sample *= 0.001f;
}
if (!std::isfinite(sample) || sample <= 0.0f) {
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 (source_unit == ipc::DepthUnit::Millimeter && std::isfinite(sample)) {
sample *= 0.001f;
}
if (!std::isfinite(sample) || sample <= 0.0f) {
continue;
}
@@ -321,18 +323,108 @@ std::expected<EncodedDepthPayload, std::string> encode_depth_payload(const RawDe
}
}
[[nodiscard]]
std::expected<EncodedDepthPayload, std::string> encode_depth_payload(const RawDepthMapU16View &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");
}
try {
std::vector<std::uint16_t> pixels(depth_map.pixels.begin(), depth_map.pixels.end());
return EncodedDepthPayload{
.encoding = DepthEncoding::RvlU16Lossless,
.storage_unit = ipc::DepthUnit::Millimeter,
.bytes = rvl::compress_image(pixels, depth_map.height, depth_map.width),
};
} catch (const std::exception &error) {
return std::unexpected(std::string("failed to RVL-encode depth map: ") + error.what());
}
}
[[nodiscard]]
std::expected<void, std::string> append_repeated_double(
google::protobuf::RepeatedField<double> *field,
std::span<const double> values,
std::size_t expected_size,
std::string_view field_name) {
if (values.size() != expected_size) {
return std::unexpected(
"expected " + std::to_string(expected_size) + " values for " + std::string(field_name));
}
field->Reserve(static_cast<int>(values.size()));
for (const double value : values) {
field->Add(value);
}
return {};
}
[[nodiscard]]
std::expected<void, std::string> write_depth_message(
mcap::McapWriter &writer,
std::uint64_t timestamp_ns,
std::string_view frame_id,
std::uint32_t width,
std::uint32_t height,
ipc::DepthUnit source_unit,
mcap::ChannelId channel_id,
std::uint32_t &sequence,
const EncodedDepthPayload &encoded) {
cvmmap_streamer::DepthMap message{};
*message.mutable_timestamp() = to_proto_timestamp(timestamp_ns);
message.set_frame_id(std::string(frame_id));
message.set_width(width);
message.set_height(height);
message.set_source_unit(to_proto_depth_unit(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 = channel_id;
record.sequence = sequence++;
record.logTime = timestamp_ns;
record.publishTime = timestamp_ns;
record.data = reinterpret_cast<const std::byte *>(serialized.data());
record.dataSize = serialized.size();
const auto write_status = writer.write(record);
if (!write_status.ok()) {
return std::unexpected("failed to write MCAP depth message: " + write_status.message);
}
return {};
}
}
struct McapRecordSink::State {
mcap::McapWriter writer{};
std::string path{};
std::string frame_id{};
std::string calibration_topic{};
std::string pose_topic{};
std::string body_topic{};
mcap::SchemaId calibration_schema_id{0};
mcap::SchemaId pose_schema_id{0};
mcap::ChannelId video_channel_id{0};
mcap::ChannelId depth_channel_id{0};
mcap::ChannelId calibration_channel_id{0};
mcap::ChannelId pose_channel_id{0};
mcap::ChannelId body_channel_id{0};
std::uint32_t video_sequence{0};
std::uint32_t depth_sequence{0};
std::uint32_t calibration_sequence{0};
std::uint32_t pose_sequence{0};
std::uint32_t body_sequence{0};
CodecType codec{CodecType::H264};
std::vector<std::uint8_t> keyframe_preamble{};
@@ -363,6 +455,8 @@ std::expected<McapRecordSink, std::string> McapRecordSink::create(
auto state = std::make_unique<State>();
state->path = config.record.mcap.path;
state->frame_id = config.record.mcap.frame_id;
state->calibration_topic = config.record.mcap.calibration_topic;
state->pose_topic = config.record.mcap.pose_topic;
state->body_topic = config.record.mcap.body_topic;
mcap::McapWriterOptions options("");
@@ -398,6 +492,26 @@ std::expected<McapRecordSink, std::string> McapRecordSink::create(
state->writer.addChannel(depth_channel);
state->depth_channel_id = depth_channel.id;
const auto calibration_descriptor_set = build_file_descriptor_set(foxglove::CameraCalibration::descriptor());
std::string calibration_schema_bytes{};
if (!calibration_descriptor_set.SerializeToString(&calibration_schema_bytes)) {
return std::unexpected("failed to serialize foxglove.CameraCalibration descriptor set");
}
mcap::Schema calibration_schema("foxglove.CameraCalibration", "protobuf", calibration_schema_bytes);
state->writer.addSchema(calibration_schema);
state->calibration_schema_id = calibration_schema.id;
const auto pose_descriptor_set = build_file_descriptor_set(foxglove::PoseInFrame::descriptor());
std::string pose_schema_bytes{};
if (!pose_descriptor_set.SerializeToString(&pose_schema_bytes)) {
return std::unexpected("failed to serialize foxglove.PoseInFrame descriptor set");
}
mcap::Schema pose_schema("foxglove.PoseInFrame", "protobuf", pose_schema_bytes);
state->writer.addSchema(pose_schema);
state->pose_schema_id = pose_schema.id;
sink.state_ = state.release();
auto update = sink.update_stream_info(stream_info);
if (!update) {
@@ -470,34 +584,137 @@ std::expected<void, std::string> McapRecordSink::write_depth_map(const RawDepthM
return std::unexpected(encoded.error());
}
cvmmap_streamer::DepthMap message{};
*message.mutable_timestamp() = to_proto_timestamp(depth_map.timestamp_ns);
return write_depth_message(
state_->writer,
depth_map.timestamp_ns,
state_->frame_id,
depth_map.width,
depth_map.height,
source_unit,
state_->depth_channel_id,
state_->depth_sequence,
*encoded);
}
std::expected<void, std::string> McapRecordSink::write_depth_map_u16(const RawDepthMapU16View &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());
}
return write_depth_message(
state_->writer,
depth_map.timestamp_ns,
state_->frame_id,
depth_map.width,
depth_map.height,
ipc::DepthUnit::Millimeter,
state_->depth_channel_id,
state_->depth_sequence,
*encoded);
}
std::expected<void, std::string> McapRecordSink::write_camera_calibration(const RawCameraCalibrationView &calibration) {
if (state_ == nullptr) {
return std::unexpected("MCAP sink is not open");
}
if (state_->calibration_topic.empty()) {
return std::unexpected("calibration topic is empty");
}
if (state_->calibration_channel_id == 0) {
mcap::Channel calibration_channel(state_->calibration_topic, "protobuf", state_->calibration_schema_id);
state_->writer.addChannel(calibration_channel);
state_->calibration_channel_id = calibration_channel.id;
}
foxglove::CameraCalibration message{};
*message.mutable_timestamp() = to_proto_timestamp(calibration.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(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()));
message.set_width(calibration.width);
message.set_height(calibration.height);
message.set_distortion_model(std::string(calibration.distortion_model));
for (const double value : calibration.distortion) {
message.add_d(value);
}
if (auto append = append_repeated_double(message.mutable_k(), calibration.intrinsic_matrix, 9, "K"); !append) {
return append;
}
if (auto append = append_repeated_double(message.mutable_r(), calibration.rectification_matrix, 9, "R"); !append) {
return append;
}
if (auto append = append_repeated_double(message.mutable_p(), calibration.projection_matrix, 12, "P"); !append) {
return append;
}
std::string serialized{};
if (!message.SerializeToString(&serialized)) {
return std::unexpected("failed to serialize cvmmap_streamer.DepthMap");
return std::unexpected("failed to serialize foxglove.CameraCalibration");
}
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.channelId = state_->calibration_channel_id;
record.sequence = state_->calibration_sequence++;
record.logTime = calibration.timestamp_ns;
record.publishTime = calibration.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 std::unexpected("failed to write MCAP calibration message: " + write_status.message);
}
return {};
}
std::expected<void, std::string> McapRecordSink::write_pose(const RawPoseView &pose) {
if (state_ == nullptr) {
return std::unexpected("MCAP sink is not open");
}
if (state_->pose_topic.empty()) {
return std::unexpected("pose topic is empty");
}
if (state_->pose_channel_id == 0) {
mcap::Channel pose_channel(state_->pose_topic, "protobuf", state_->pose_schema_id);
state_->writer.addChannel(pose_channel);
state_->pose_channel_id = pose_channel.id;
}
foxglove::PoseInFrame message{};
*message.mutable_timestamp() = to_proto_timestamp(pose.timestamp_ns);
message.set_frame_id(std::string(pose.reference_frame_id));
auto *pose_message = message.mutable_pose();
auto *position = pose_message->mutable_position();
position->set_x(pose.position[0]);
position->set_y(pose.position[1]);
position->set_z(pose.position[2]);
auto *orientation = pose_message->mutable_orientation();
orientation->set_x(pose.orientation[0]);
orientation->set_y(pose.orientation[1]);
orientation->set_z(pose.orientation[2]);
orientation->set_w(pose.orientation[3]);
std::string serialized{};
if (!message.SerializeToString(&serialized)) {
return std::unexpected("failed to serialize foxglove.PoseInFrame");
}
mcap::Message record{};
record.channelId = state_->pose_channel_id;
record.sequence = state_->pose_sequence++;
record.logTime = pose.timestamp_ns;
record.publishTime = pose.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 pose message: " + write_status.message);
}
return {};
}