feat(zed): recover corrupted frame gaps in MCAP export

Make ZED MCAP export skip corrupted frame runs until recovery and

treat unreadable tail frames as end-of-stream instead of hard

failing conversion.

Update bundled nearest-mode export to emit partial bundles during

corruption gaps, extend BundleManifest with explicit member status

and skipped-frame counts, and only write payload messages for

present cameras.

Tighten batch probing so bundled MCAP validation checks /bundle

coverage and per-camera message counts, and improve failure

excerpts to include stderr tail output.

Also add a local cppzmq CMake fallback, refresh the multi-record

tester for the new bundle schema, and document the mixed NVENC

limitations in the README.
This commit is contained in:
2026-03-24 03:49:35 +00:00
parent e3a423433e
commit 807a73b480
8 changed files with 1006 additions and 172 deletions
+12 -2
View File
@@ -28,6 +28,14 @@ if (NOT TARGET OpenSSL::Crypto AND DEFINED OPENSSL_CRYPTO_LIBRARY)
INTERFACE_INCLUDE_DIRECTORIES "${OPENSSL_INCLUDE_DIR}") INTERFACE_INCLUDE_DIRECTORIES "${OPENSSL_INCLUDE_DIR}")
endif() endif()
find_package(cppzmq QUIET) find_package(cppzmq QUIET)
set(CPPZMQ_LOCAL_ROOT "${CMAKE_CURRENT_LIST_DIR}/../cppzmq" CACHE PATH "Path to a local cppzmq checkout")
if (NOT TARGET cppzmq::cppzmq AND NOT TARGET cppzmq)
if (EXISTS "${CPPZMQ_LOCAL_ROOT}/zmq.hpp")
add_library(cppzmq::cppzmq INTERFACE IMPORTED GLOBAL)
set_target_properties(cppzmq::cppzmq PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES "${CPPZMQ_LOCAL_ROOT}")
endif()
endif()
if (DEFINED CVMMAP_STREAMER_USE_SYSTEM_CNATS) if (DEFINED CVMMAP_STREAMER_USE_SYSTEM_CNATS)
message(FATAL_ERROR message(FATAL_ERROR
"CVMMAP_STREAMER_USE_SYSTEM_CNATS was removed; use CVMMAP_CNATS_PROVIDER=system") "CVMMAP_STREAMER_USE_SYSTEM_CNATS was removed; use CVMMAP_CNATS_PROVIDER=system")
@@ -161,8 +169,10 @@ protobuf_generate(
TARGET cvmmap_streamer_depth_proto TARGET cvmmap_streamer_depth_proto
LANGUAGE cpp LANGUAGE cpp
PROTOC_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}" PROTOC_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}"
PROTOS "${CMAKE_CURRENT_LIST_DIR}/proto/cvmmap_streamer/DepthMap.proto" PROTOS
IMPORT_DIRS "${CMAKE_CURRENT_LIST_DIR}") "${CMAKE_CURRENT_LIST_DIR}/proto/cvmmap_streamer/DepthMap.proto"
"${CMAKE_CURRENT_LIST_DIR}/proto/cvmmap_streamer/BundleManifest.proto"
IMPORT_DIRS "${CMAKE_CURRENT_LIST_DIR}/proto")
add_library(cvmmap_streamer_protobuf INTERFACE) add_library(cvmmap_streamer_protobuf INTERFACE)
target_include_directories(cvmmap_streamer_foxglove_proto target_include_directories(cvmmap_streamer_foxglove_proto
PUBLIC PUBLIC
+3 -1
View File
@@ -265,9 +265,11 @@ uv run python scripts/zed_batch_svo_to_mcap.py \
The batch MCAP wrapper writes `<segment>/<segment>.mcap` by default, skips existing outputs unless told otherwise, and returns a nonzero exit code if any segment fails. The batch MCAP wrapper writes `<segment>/<segment>.mcap` by default, skips existing outputs unless told otherwise, and returns a nonzero exit code if any segment fails.
The repo includes a minimal pose config at `config/zed_pose_config.toml` so MCAP conversion does not depend on a separate `cv-mmap` checkout. The repo includes a minimal pose config at `config/zed_pose_config.toml` so MCAP conversion does not depend on a separate `cv-mmap` checkout.
In bundled multi-camera mode, `--start-frame` and `--end-frame` mean the first and last emitted synced frame-group indices from the common start timestamp, inclusive. In bundled multi-camera mode, `--start-frame` and `--end-frame` mean the first and last emitted bundle indices from the common start timestamp, inclusive.
When stderr is attached to a TTY, `zed_batch_svo_to_mcap.py` uses a `progress-table` view by default; otherwise it emits line-oriented start/completion/failure logs plus periodic heartbeat summaries. Use `--progress-ui table` or `--progress-ui text` to override the automatic mode selection. When stderr is attached to a TTY, `zed_batch_svo_to_mcap.py` uses a `progress-table` view by default; otherwise it emits line-oriented start/completion/failure logs plus periodic heartbeat summaries. Use `--progress-ui table` or `--progress-ui text` to override the automatic mode selection.
Bundled MCAP export now defaults to `--bundle-policy nearest`. That mode emits one `/bundle` manifest message per bundle timestamp on the common timeline and keeps the original per-camera timestamps on `/zedN/video`, `/zedN/depth`, and optional `/zedN/pose`. Consumers that care about grouping should follow `/bundle` instead of inferring bundle membership from identical message timestamps. Use `--bundle-policy strict` when you want the older thresholded sync behavior, and `--sync-tolerance-ms` only applies in that strict mode.
### Why Mixed Hardware/Software Mode Exists ### Why Mixed Hardware/Software Mode Exists
Bundled MCAP export opens one video encoder per camera stream. A four-camera segment therefore consumes four H.264/H.265 encoder sessions at once. Bundled MCAP export opens one video encoder per camera stream. A four-camera segment therefore consumes four H.264/H.265 encoder sessions at once.
@@ -8,6 +8,7 @@
#include <cstddef> #include <cstddef>
#include <cstdint> #include <cstdint>
#include <expected> #include <expected>
#include <optional>
#include <span> #include <span>
#include <string> #include <string>
#include <string_view> #include <string_view>
@@ -19,6 +20,16 @@ enum class DepthEncoding {
RvlF32, RvlF32,
}; };
enum class BundlePolicy {
Nearest,
Strict,
};
enum class BundleMemberStatus {
Present,
CorruptedGap,
};
struct RawDepthMapView { struct RawDepthMapView {
std::uint64_t timestamp_ns{0}; std::uint64_t timestamp_ns{0};
std::uint32_t width{0}; std::uint32_t width{0};
@@ -57,6 +68,21 @@ struct RawBodyTrackingMessageView {
std::span<const std::uint8_t> bytes{}; std::span<const std::uint8_t> bytes{};
}; };
struct RawBundleMemberView {
std::string_view camera_label{};
BundleMemberStatus status{BundleMemberStatus::Present};
std::optional<std::uint64_t> timestamp_ns{};
std::int64_t delta_ns{0};
std::uint32_t corrupted_frames_skipped{0};
};
struct RawBundleManifestView {
std::uint64_t timestamp_ns{0};
std::uint64_t bundle_index{0};
BundlePolicy policy{BundlePolicy::Nearest};
std::span<const RawBundleMemberView> members{};
};
struct McapRecordStreamConfig { struct McapRecordStreamConfig {
std::string topic{"/camera/video"}; std::string topic{"/camera/video"};
std::string depth_topic{"/camera/depth"}; std::string depth_topic{"/camera/depth"};
@@ -137,7 +163,8 @@ public:
[[nodiscard]] [[nodiscard]]
static std::expected<MultiMcapRecordSink, std::string> create( static std::expected<MultiMcapRecordSink, std::string> create(
std::string path, std::string path,
McapCompression compression); McapCompression compression,
std::string bundle_topic = "/bundle");
[[nodiscard]] [[nodiscard]]
std::expected<StreamId, std::string> add_stream( std::expected<StreamId, std::string> add_stream(
@@ -179,6 +206,10 @@ public:
StreamId stream_id, StreamId stream_id,
const RawPoseView &pose); const RawPoseView &pose);
[[nodiscard]]
std::expected<void, std::string> write_bundle_manifest(
const RawBundleManifestView &bundle);
[[nodiscard]] [[nodiscard]]
std::expected<void, std::string> write_body_tracking_message( std::expected<void, std::string> write_body_tracking_message(
StreamId stream_id, StreamId stream_id,
@@ -0,0 +1,32 @@
syntax = "proto3";
package cvmmap_streamer;
import "google/protobuf/timestamp.proto";
message BundleManifest {
enum BundlePolicy {
BUNDLE_POLICY_UNKNOWN = 0;
BUNDLE_POLICY_NEAREST = 1;
BUNDLE_POLICY_STRICT = 2;
}
enum BundleMemberStatus {
BUNDLE_MEMBER_STATUS_UNKNOWN = 0;
BUNDLE_MEMBER_STATUS_PRESENT = 1;
BUNDLE_MEMBER_STATUS_CORRUPTED_GAP = 2;
}
message BundleMember {
string camera_label = 1;
google.protobuf.Timestamp timestamp = 2;
sint64 delta_ns = 3;
BundleMemberStatus status = 4;
uint32 corrupted_frames_skipped = 5;
}
google.protobuf.Timestamp timestamp = 1;
uint64 bundle_index = 2;
BundlePolicy policy = 3;
repeated BundleMember members = 4;
}
+171 -11
View File
@@ -10,6 +10,7 @@ import re
import subprocess import subprocess
import sys import sys
import time import time
from collections import Counter
from dataclasses import dataclass from dataclasses import dataclass
from pathlib import Path from pathlib import Path
@@ -34,6 +35,8 @@ class BatchConfig:
mcap_compression: str mcap_compression: str
depth_mode: str depth_mode: str
depth_size: str depth_size: str
bundle_policy: str
bundle_topic: str | None
with_pose: bool with_pose: bool
pose_config: Path | None pose_config: Path | None
world_frame_id: str | None world_frame_id: str | None
@@ -102,6 +105,7 @@ class ActiveJobState:
_MCAP_READER_MODULE = None _MCAP_READER_MODULE = None
_BUNDLE_MANIFEST_CLASS_CACHE: dict[bytes, tuple[object, int | None]] = {}
TABLE_REFRESH_SECONDS = 1.0 TABLE_REFRESH_SECONDS = 1.0
TEXT_HEARTBEAT_SECONDS = 30.0 TEXT_HEARTBEAT_SECONDS = 30.0
@@ -500,7 +504,11 @@ def command_for_job(job: ConversionJob, config: BatchConfig, encoder_device: str
config.depth_mode, config.depth_mode,
"--depth-size", "--depth-size",
config.depth_size, config.depth_size,
"--bundle-policy",
config.bundle_policy,
] ]
if config.bundle_topic:
command.extend(["--bundle-topic", config.bundle_topic])
if config.with_pose: if config.with_pose:
command.append("--with-pose") command.append("--with-pose")
if config.pose_config is not None: if config.pose_config is not None:
@@ -569,22 +577,131 @@ def required_topics_for(camera_labels: tuple[str, ...]) -> set[str]:
return topics return topics
def probe_output(output_path: Path, camera_labels: tuple[str, ...]) -> OutputProbeResult: def load_bundle_manifest_type(schema_data: bytes) -> tuple[object, int | None]:
cached = _BUNDLE_MANIFEST_CLASS_CACHE.get(schema_data)
if cached is not None:
return cached
from google.protobuf import descriptor_pb2, descriptor_pool, message_factory, timestamp_pb2
descriptor_set = descriptor_pb2.FileDescriptorSet()
descriptor_set.ParseFromString(schema_data)
pool = descriptor_pool.DescriptorPool()
has_embedded_timestamp = any(
file_descriptor.name == "google/protobuf/timestamp.proto"
for file_descriptor in descriptor_set.file
)
if has_embedded_timestamp:
for file_descriptor in descriptor_set.file:
if file_descriptor.name == "google/protobuf/timestamp.proto":
pool.Add(file_descriptor)
break
else:
pool.AddSerializedFile(timestamp_pb2.DESCRIPTOR.serialized_pb)
for file_descriptor in descriptor_set.file:
if file_descriptor.name == "google/protobuf/timestamp.proto":
continue
pool.Add(file_descriptor)
message_descriptor = pool.FindMessageTypeByName("cvmmap_streamer.BundleManifest")
message_class = message_factory.GetMessageClass(message_descriptor)
present_value = None
if "BundleMemberStatus" in message_descriptor.enum_types_by_name:
status_enum = message_descriptor.enum_types_by_name["BundleMemberStatus"]
present_value = status_enum.values_by_name["BUNDLE_MEMBER_STATUS_PRESENT"].number
_BUNDLE_MANIFEST_CLASS_CACHE[schema_data] = (message_class, present_value)
return message_class, present_value
def probe_output(
output_path: Path,
camera_labels: tuple[str, ...],
*,
bundle_topic: str | None,
) -> OutputProbeResult:
if not output_path.is_file(): if not output_path.is_file():
return OutputProbeResult(output_path=output_path, status="missing") return OutputProbeResult(output_path=output_path, status="missing")
reader_module = load_mcap_reader() reader_module = load_mcap_reader()
expected_topics = required_topics_for(camera_labels) expected_topics = required_topics_for(camera_labels)
require_bundle = len(camera_labels) > 1 and bool(bundle_topic)
if require_bundle:
expected_topics.add(bundle_topic or "/bundle")
found_topics: set[str] = set() found_topics: set[str] = set()
video_counts: Counter[str] = Counter()
depth_counts: Counter[str] = Counter()
bundle_present_counts: Counter[str] = Counter()
expected_camera_labels = set(camera_labels)
try: try:
with output_path.open("rb") as stream: with output_path.open("rb") as stream:
reader = reader_module.make_reader(stream) reader = reader_module.make_reader(stream)
for _schema, channel, _message in reader.iter_messages(): for schema, channel, message in reader.iter_messages():
if channel.topic in expected_topics: if channel.topic in expected_topics:
found_topics.add(channel.topic) found_topics.add(channel.topic)
if found_topics == expected_topics: if channel.topic.endswith("/video"):
return OutputProbeResult(output_path=output_path, status="valid") video_counts[channel.topic.removeprefix("/").removesuffix("/video")] += 1
continue
if channel.topic.endswith("/depth"):
depth_counts[channel.topic.removeprefix("/").removesuffix("/depth")] += 1
continue
if require_bundle and channel.topic == bundle_topic:
if schema is None or schema.name != "cvmmap_streamer.BundleManifest":
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"bundle topic '{bundle_topic}' is missing the BundleManifest schema",
)
try:
bundle_class, present_value = load_bundle_manifest_type(schema.data)
bundle = bundle_class()
bundle.ParseFromString(message.data)
except Exception as error: # noqa: BLE001
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"failed to parse bundle manifest: {error}",
)
bundle_labels: set[str] = set()
for member in bundle.members:
label = str(member.camera_label)
if label not in expected_camera_labels:
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"bundle manifest referenced unknown camera label '{label}'",
)
if label in bundle_labels:
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"bundle manifest duplicated camera label '{label}'",
)
bundle_labels.add(label)
is_present = member.HasField("timestamp")
if present_value is not None:
is_present = member.status == present_value
if is_present and not member.HasField("timestamp"):
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"bundle member '{label}' is present but missing a timestamp",
)
if is_present:
bundle_present_counts[label] += 1
if bundle_labels != expected_camera_labels:
missing_labels = sorted(expected_camera_labels - bundle_labels)
extra_labels = sorted(bundle_labels - expected_camera_labels)
details = []
if missing_labels:
details.append("missing=" + ",".join(missing_labels))
if extra_labels:
details.append("extra=" + ",".join(extra_labels))
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason="bundle manifest camera coverage mismatch: " + " ".join(details),
)
except Exception as error: # noqa: BLE001 except Exception as error: # noqa: BLE001
return OutputProbeResult(output_path=output_path, status="invalid", reason=str(error)) return OutputProbeResult(output_path=output_path, status="invalid", reason=str(error))
@@ -595,6 +712,27 @@ def probe_output(output_path: Path, camera_labels: tuple[str, ...]) -> OutputPro
status="invalid", status="invalid",
reason="missing expected topics: " + ", ".join(missing_topics), reason="missing expected topics: " + ", ".join(missing_topics),
) )
if require_bundle:
for label in camera_labels:
present_count = bundle_present_counts[label]
if video_counts[label] != present_count:
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=(
f"video count mismatch for {label}: "
f"bundle_present={present_count} video_messages={video_counts[label]}"
),
)
if depth_counts[label] != present_count:
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=(
f"depth count mismatch for {label}: "
f"bundle_present={present_count} depth_messages={depth_counts[label]}"
),
)
return OutputProbeResult(output_path=output_path, status="valid") return OutputProbeResult(output_path=output_path, status="valid")
@@ -635,8 +773,13 @@ def split_lines_for_excerpt(text: str, max_lines: int = 8) -> list[str]:
lines = [line.rstrip() for line in text.splitlines() if line.strip()] lines = [line.rstrip() for line in text.splitlines() if line.strip()]
if len(lines) <= max_lines: if len(lines) <= max_lines:
return lines return lines
excerpt = lines[:max_lines] head_count = max(1, max_lines // 2)
excerpt.append(f"... ({len(lines) - max_lines} more lines)") tail_count = max_lines - head_count
excerpt = lines[:head_count]
omitted = len(lines) - head_count - tail_count
if omitted > 0:
excerpt.append(f"... ({omitted} omitted line(s))")
excerpt.extend(lines[-tail_count:])
return excerpt return excerpt
@@ -1011,6 +1154,19 @@ def build_worker_slots(
default="optimal", default="optimal",
show_default=True, show_default=True,
) )
@click.option(
"--bundle-policy",
type=click.Choice(("nearest", "strict")),
default="nearest",
show_default=True,
help="Bundling policy for multi-camera MCAP export.",
)
@click.option(
"--bundle-topic",
default="/bundle",
show_default=True,
help="Topic used for bundled multi-camera manifest messages.",
)
@click.option("--with-pose", is_flag=True, help="Enable per-camera positional tracking export when available.") @click.option("--with-pose", is_flag=True, help="Enable per-camera positional tracking export when available.")
@click.option( @click.option(
"--pose-config", "--pose-config",
@@ -1026,19 +1182,19 @@ def build_worker_slots(
"--start-frame", "--start-frame",
type=click.IntRange(min=0), type=click.IntRange(min=0),
default=None, default=None,
help="First synced frame group to export (inclusive) in bundled multi-camera mode.", help="First bundle index to export (inclusive) in bundled multi-camera mode.",
) )
@click.option( @click.option(
"--end-frame", "--end-frame",
type=click.IntRange(min=0), type=click.IntRange(min=0),
default=None, default=None,
help="Last synced frame group to export (inclusive) in bundled multi-camera mode.", help="Last bundle index to export (inclusive) in bundled multi-camera mode.",
) )
@click.option( @click.option(
"--sync-tolerance-ms", "--sync-tolerance-ms",
type=click.FloatRange(min=0.0, min_open=True), type=click.FloatRange(min=0.0, min_open=True),
default=None, default=None,
help="Override the maximum timestamp delta used for bundled multi-camera sync.", help="Override the maximum timestamp delta used by strict bundled sync.",
) )
@click.option( @click.option(
"--progress-ui", "--progress-ui",
@@ -1070,6 +1226,8 @@ def main(
mcap_compression: str, mcap_compression: str,
depth_mode: str, depth_mode: str,
depth_size: str, depth_size: str,
bundle_policy: str,
bundle_topic: str,
with_pose: bool, with_pose: bool,
pose_config: Path | None, pose_config: Path | None,
world_frame_id: str | None, world_frame_id: str | None,
@@ -1102,6 +1260,8 @@ def main(
mcap_compression=mcap_compression, mcap_compression=mcap_compression,
depth_mode=depth_mode, depth_mode=depth_mode,
depth_size=depth_size, depth_size=depth_size,
bundle_policy=bundle_policy,
bundle_topic=bundle_topic,
with_pose=with_pose, with_pose=with_pose,
pose_config=pose_config.expanduser().resolve() if pose_config is not None else None, pose_config=pose_config.expanduser().resolve() if pose_config is not None else None,
world_frame_id=world_frame_id, world_frame_id=world_frame_id,
@@ -1156,7 +1316,7 @@ def main(
continue continue
if report_existing: if report_existing:
probe_result = probe_output(output_path, job.camera_labels) probe_result = probe_output(output_path, job.camera_labels, bundle_topic=config.bundle_topic)
if probe_result.status == "valid": if probe_result.status == "valid":
valid_existing.append(probe_result) valid_existing.append(probe_result)
elif probe_result.status == "invalid": elif probe_result.status == "invalid":
@@ -1171,7 +1331,7 @@ def main(
continue continue
if config.probe_existing: if config.probe_existing:
probe_result = probe_output(output_path, job.camera_labels) probe_result = probe_output(output_path, job.camera_labels, bundle_topic=config.bundle_topic)
if probe_result.status == "valid": if probe_result.status == "valid":
valid_existing.append(probe_result) valid_existing.append(probe_result)
skipped_results.append( skipped_results.append(
+111 -1
View File
@@ -3,6 +3,7 @@
#include "cvmmap_streamer/record/mcap_record_sink.hpp" #include "cvmmap_streamer/record/mcap_record_sink.hpp"
#include "protobuf_descriptor.hpp" #include "protobuf_descriptor.hpp"
#include "proto/cvmmap_streamer/BundleManifest.pb.h"
#include "proto/cvmmap_streamer/DepthMap.pb.h" #include "proto/cvmmap_streamer/DepthMap.pb.h"
#include "proto/foxglove/CameraCalibration.pb.h" #include "proto/foxglove/CameraCalibration.pb.h"
#include "proto/foxglove/CompressedVideo.pb.h" #include "proto/foxglove/CompressedVideo.pb.h"
@@ -117,6 +118,28 @@ cvmmap_streamer::DepthMap::Encoding to_proto_depth_encoding(DepthEncoding encodi
} }
} }
[[nodiscard]]
cvmmap_streamer::BundleManifest::BundlePolicy to_proto_bundle_policy(BundlePolicy policy) {
switch (policy) {
case BundlePolicy::Strict:
return cvmmap_streamer::BundleManifest::BUNDLE_POLICY_STRICT;
case BundlePolicy::Nearest:
default:
return cvmmap_streamer::BundleManifest::BUNDLE_POLICY_NEAREST;
}
}
[[nodiscard]]
cvmmap_streamer::BundleManifest::BundleMemberStatus to_proto_bundle_member_status(BundleMemberStatus status) {
switch (status) {
case BundleMemberStatus::CorruptedGap:
return cvmmap_streamer::BundleManifest::BUNDLE_MEMBER_STATUS_CORRUPTED_GAP;
case BundleMemberStatus::Present:
default:
return cvmmap_streamer::BundleManifest::BUNDLE_MEMBER_STATUS_PRESENT;
}
}
void append_start_code(std::vector<std::uint8_t> &output) { void append_start_code(std::vector<std::uint8_t> &output) {
output.push_back(0x00); output.push_back(0x00);
output.push_back(0x00); output.push_back(0x00);
@@ -464,6 +487,60 @@ std::expected<void, std::string> write_calibration_message(
return {}; return {};
} }
[[nodiscard]]
std::expected<void, std::string> write_bundle_manifest_message(
mcap::McapWriter &writer,
mcap::ChannelId channel_id,
std::uint32_t &sequence,
const RawBundleManifestView &bundle) {
if (channel_id == 0) {
return std::unexpected("bundle topic is disabled");
}
if (bundle.members.empty()) {
return std::unexpected("bundle manifest must contain at least one member");
}
cvmmap_streamer::BundleManifest message{};
*message.mutable_timestamp() = to_proto_timestamp(bundle.timestamp_ns);
message.set_bundle_index(bundle.bundle_index);
message.set_policy(to_proto_bundle_policy(bundle.policy));
for (const auto &member_view : bundle.members) {
if (member_view.camera_label.empty()) {
return std::unexpected("bundle member camera label is empty");
}
if (member_view.status == BundleMemberStatus::Present && !member_view.timestamp_ns.has_value()) {
return std::unexpected("present bundle member is missing a timestamp");
}
auto *member = message.add_members();
member->set_camera_label(std::string(member_view.camera_label));
member->set_status(to_proto_bundle_member_status(member_view.status));
if (member_view.timestamp_ns.has_value()) {
*member->mutable_timestamp() = to_proto_timestamp(*member_view.timestamp_ns);
}
member->set_delta_ns(member_view.delta_ns);
member->set_corrupted_frames_skipped(member_view.corrupted_frames_skipped);
}
std::string serialized{};
if (!message.SerializeToString(&serialized)) {
return std::unexpected("failed to serialize cvmmap_streamer.BundleManifest");
}
mcap::Message record{};
record.channelId = channel_id;
record.sequence = sequence++;
record.logTime = bundle.timestamp_ns;
record.publishTime = bundle.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 bundle manifest: " + write_status.message);
}
return {};
}
} }
struct McapRecordSink::State { struct McapRecordSink::State {
@@ -835,8 +912,12 @@ struct MultiMcapRecordSink::State {
std::string path{}; std::string path{};
mcap::SchemaId video_schema_id{0}; mcap::SchemaId video_schema_id{0};
mcap::SchemaId depth_schema_id{0}; mcap::SchemaId depth_schema_id{0};
mcap::SchemaId bundle_schema_id{0};
mcap::SchemaId calibration_schema_id{0}; mcap::SchemaId calibration_schema_id{0};
mcap::SchemaId pose_schema_id{0}; mcap::SchemaId pose_schema_id{0};
std::string bundle_topic{};
mcap::ChannelId bundle_channel_id{0};
std::uint32_t bundle_sequence{0};
std::vector<StreamState> streams{}; std::vector<StreamState> streams{};
}; };
@@ -959,10 +1040,12 @@ MultiMcapRecordSink &MultiMcapRecordSink::operator=(MultiMcapRecordSink &&other)
std::expected<MultiMcapRecordSink, std::string> MultiMcapRecordSink::create( std::expected<MultiMcapRecordSink, std::string> MultiMcapRecordSink::create(
std::string path, std::string path,
McapCompression compression) { McapCompression compression,
std::string bundle_topic) {
MultiMcapRecordSink sink{}; MultiMcapRecordSink sink{};
auto state = std::make_unique<State>(); auto state = std::make_unique<State>();
state->path = std::move(path); state->path = std::move(path);
state->bundle_topic = std::move(bundle_topic);
mcap::McapWriterOptions options(""); mcap::McapWriterOptions options("");
options.compression = to_mcap_compression(compression); options.compression = to_mcap_compression(compression);
@@ -989,6 +1072,21 @@ std::expected<MultiMcapRecordSink, std::string> MultiMcapRecordSink::create(
state->writer.addSchema(depth_schema); state->writer.addSchema(depth_schema);
state->depth_schema_id = depth_schema.id; state->depth_schema_id = depth_schema.id;
if (!state->bundle_topic.empty()) {
const auto bundle_descriptor_set = build_file_descriptor_set(cvmmap_streamer::BundleManifest::descriptor());
std::string bundle_schema_bytes{};
if (!bundle_descriptor_set.SerializeToString(&bundle_schema_bytes)) {
return std::unexpected("failed to serialize cvmmap_streamer.BundleManifest descriptor set");
}
mcap::Schema bundle_schema("cvmmap_streamer.BundleManifest", "protobuf", bundle_schema_bytes);
state->writer.addSchema(bundle_schema);
state->bundle_schema_id = bundle_schema.id;
mcap::Channel bundle_channel(state->bundle_topic, "protobuf", state->bundle_schema_id);
state->writer.addChannel(bundle_channel);
state->bundle_channel_id = bundle_channel.id;
}
const auto calibration_descriptor_set = build_file_descriptor_set(foxglove::CameraCalibration::descriptor()); const auto calibration_descriptor_set = build_file_descriptor_set(foxglove::CameraCalibration::descriptor());
std::string calibration_schema_bytes{}; std::string calibration_schema_bytes{};
if (!calibration_descriptor_set.SerializeToString(&calibration_schema_bytes)) { if (!calibration_descriptor_set.SerializeToString(&calibration_schema_bytes)) {
@@ -1237,6 +1335,18 @@ std::expected<void, std::string> MultiMcapRecordSink::write_pose(
return {}; return {};
} }
std::expected<void, std::string> MultiMcapRecordSink::write_bundle_manifest(
const RawBundleManifestView &bundle) {
if (state_ == nullptr) {
return std::unexpected("MCAP sink is not open");
}
return write_bundle_manifest_message(
state_->writer,
state_->bundle_channel_id,
state_->bundle_sequence,
bundle);
}
std::expected<void, std::string> MultiMcapRecordSink::write_body_tracking_message( std::expected<void, std::string> MultiMcapRecordSink::write_body_tracking_message(
const StreamId stream_id, const StreamId stream_id,
const RawBodyTrackingMessageView &body_message) { const RawBodyTrackingMessageView &body_message) {
+53 -1
View File
@@ -1,5 +1,6 @@
#include <mcap/reader.hpp> #include <mcap/reader.hpp>
#include "proto/cvmmap_streamer/BundleManifest.pb.h"
#include "proto/cvmmap_streamer/DepthMap.pb.h" #include "proto/cvmmap_streamer/DepthMap.pb.h"
#include "cvmmap_streamer/common.h" #include "cvmmap_streamer/common.h"
#include "cvmmap_streamer/record/mcap_record_sink.hpp" #include "cvmmap_streamer/record/mcap_record_sink.hpp"
@@ -73,7 +74,8 @@ int main(int argc, char **argv) {
auto sink = cvmmap_streamer::record::MultiMcapRecordSink::create( auto sink = cvmmap_streamer::record::MultiMcapRecordSink::create(
output_path.string(), output_path.string(),
compression); compression,
"/bundle");
if (!sink) { if (!sink) {
spdlog::error("failed to create MCAP sink: {}", sink.error()); spdlog::error("failed to create MCAP sink: {}", sink.error());
return exit_code(TesterExitCode::CreateError); return exit_code(TesterExitCode::CreateError);
@@ -125,6 +127,31 @@ int main(int argc, char **argv) {
0.0, 500.0, 240.0, 0.0, 0.0, 500.0, 240.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0,
}; };
const std::vector<cvmmap_streamer::record::RawBundleMemberView> bundle_members{
{
.camera_label = "zed1",
.status = cvmmap_streamer::record::BundleMemberStatus::Present,
.timestamp_ns = 100,
.delta_ns = -5,
},
{
.camera_label = "zed2",
.status = cvmmap_streamer::record::BundleMemberStatus::CorruptedGap,
.timestamp_ns = std::nullopt,
.delta_ns = 0,
.corrupted_frames_skipped = 3,
},
};
if (auto write = sink->write_bundle_manifest(cvmmap_streamer::record::RawBundleManifestView{
.timestamp_ns = 105,
.bundle_index = 7,
.policy = cvmmap_streamer::record::BundlePolicy::Nearest,
.members = bundle_members,
}); !write) {
spdlog::error("failed to write bundle manifest: {}", write.error());
return exit_code(TesterExitCode::WriteError);
}
for (const auto [stream_id, label, pose_x] : { for (const auto [stream_id, label, pose_x] : {
std::tuple{*zed1, std::string("zed1"), 1.0}, std::tuple{*zed1, std::string("zed1"), 1.0},
@@ -242,6 +269,30 @@ int main(int argc, char **argv) {
continue; continue;
} }
if (it->schema->name == "cvmmap_streamer.BundleManifest") {
cvmmap_streamer::BundleManifest bundle{};
if (!bundle.ParseFromArray(it->message.data, static_cast<int>(it->message.dataSize))) {
spdlog::error("failed to parse cvmmap_streamer.BundleManifest");
reader.close();
return exit_code(TesterExitCode::VerificationError);
}
if (bundle.bundle_index() != 7 || bundle.members_size() != 2) {
spdlog::error("bundle manifest contents mismatch");
reader.close();
return exit_code(TesterExitCode::VerificationError);
}
if (bundle.members(0).status() != cvmmap_streamer::BundleManifest::BUNDLE_MEMBER_STATUS_PRESENT ||
!bundle.members(0).has_timestamp() ||
bundle.members(1).status() != cvmmap_streamer::BundleManifest::BUNDLE_MEMBER_STATUS_CORRUPTED_GAP ||
bundle.members(1).has_timestamp() ||
bundle.members(1).corrupted_frames_skipped() != 3) {
spdlog::error("bundle manifest member status mismatch");
reader.close();
return exit_code(TesterExitCode::VerificationError);
}
continue;
}
if (it->schema->name == "foxglove.CameraCalibration") { if (it->schema->name == "foxglove.CameraCalibration") {
foxglove::CameraCalibration calibration{}; foxglove::CameraCalibration calibration{};
if (!calibration.ParseFromArray(it->message.data, static_cast<int>(it->message.dataSize))) { if (!calibration.ParseFromArray(it->message.data, static_cast<int>(it->message.dataSize))) {
@@ -266,6 +317,7 @@ int main(int argc, char **argv) {
reader.close(); reader.close();
for (const auto &topic : { for (const auto &topic : {
"/bundle",
"/zed1/video", "/zed1/video",
"/zed1/depth", "/zed1/depth",
"/zed1/calibration", "/zed1/calibration",
+521 -84
View File
@@ -55,6 +55,8 @@ struct CliOptions {
std::string mcap_compression{"zstd"}; std::string mcap_compression{"zstd"};
std::string depth_mode{"neural_plus"}; std::string depth_mode{"neural_plus"};
std::string depth_size{"optimal"}; std::string depth_size{"optimal"};
std::string bundle_policy{"nearest"};
std::string bundle_topic{"/bundle"};
bool with_pose{false}; bool with_pose{false};
std::uint32_t start_frame{0}; std::uint32_t start_frame{0};
std::uint32_t end_frame{0}; std::uint32_t end_frame{0};
@@ -126,6 +128,7 @@ struct CameraStream {
TrackingSample next_tracking{}; TrackingSample next_tracking{};
std::uint64_t current_timestamp_ns{0}; std::uint64_t current_timestamp_ns{0};
std::uint64_t next_timestamp_ns{0}; std::uint64_t next_timestamp_ns{0};
std::uint32_t next_corrupted_frames_skipped{0};
std::uint64_t first_timestamp_ns{0}; std::uint64_t first_timestamp_ns{0};
std::uint64_t last_timestamp_ns{0}; std::uint64_t last_timestamp_ns{0};
std::uint64_t total_frames{0}; std::uint64_t total_frames{0};
@@ -152,6 +155,24 @@ struct CameraStream {
CalibrationData depth_calibration{}; CalibrationData depth_calibration{};
}; };
struct BundledFrameSelection {
std::size_t stream_index{0};
cvmmap_streamer::record::BundleMemberStatus status{
cvmmap_streamer::record::BundleMemberStatus::Present,
};
bool use_next{false};
std::optional<std::uint64_t> timestamp_ns{};
std::int64_t delta_ns{0};
std::uint32_t corrupted_frames_skipped{0};
};
struct GrabResult {
int svo_position{-1};
std::uint32_t corrupted_frames_skipped{0};
int first_corrupted_position{-1};
int last_corrupted_position{-1};
};
[[nodiscard]] [[nodiscard]]
constexpr int exit_code(const ToolExitCode code) { constexpr int exit_code(const ToolExitCode code) {
return static_cast<int>(code); return static_cast<int>(code);
@@ -302,6 +323,18 @@ std::expected<sl::Resolution, std::string> parse_depth_size(const std::string_vi
return sl::Resolution(std::stoi(match[1].str()), std::stoi(match[2].str())); return sl::Resolution(std::stoi(match[1].str()), std::stoi(match[2].str()));
} }
[[nodiscard]]
std::expected<cvmmap_streamer::record::BundlePolicy, std::string> parse_bundle_policy(const std::string_view raw) {
const auto normalized = lowercase(std::string(raw));
if (normalized == "nearest") {
return cvmmap_streamer::record::BundlePolicy::Nearest;
}
if (normalized == "strict") {
return cvmmap_streamer::record::BundlePolicy::Strict;
}
return std::unexpected("invalid bundle policy: '" + std::string(raw) + "' (expected: nearest|strict)");
}
[[nodiscard]] [[nodiscard]]
std::uint64_t frame_period_ns(const float fps) { std::uint64_t frame_period_ns(const float fps) {
if (!(fps > 0.0f)) { if (!(fps > 0.0f)) {
@@ -659,36 +692,118 @@ void maybe_log_tracking_state(CameraStream &stream, const sl::POSITIONAL_TRACKIN
} }
[[nodiscard]] [[nodiscard]]
std::expected<void, std::string> read_frame_data( std::expected<GrabResult, std::string> grab_next_readable_frame(
sl::Camera &camera,
const sl::RuntimeParameters &runtime,
const std::string_view source_label,
const std::string_view source_path,
const std::uint64_t total_frames,
const std::optional<std::uint64_t> current_timestamp_ns,
const std::optional<std::uint64_t> last_timestamp_ns) {
GrabResult result{};
while (true) {
const auto grab_status = camera.grab(runtime);
const auto svo_position = camera.getSVOPosition();
if (grab_status == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) {
return std::unexpected("end-of-svo");
}
if (grab_status == sl::ERROR_CODE::CORRUPTED_FRAME) {
if (result.first_corrupted_position < 0) {
result.first_corrupted_position = svo_position;
}
result.last_corrupted_position = svo_position;
result.corrupted_frames_skipped += 1;
const auto last_frame_position = total_frames > 0
? static_cast<int>(total_frames - 1)
: 0;
const bool at_or_past_tail = svo_position >= last_frame_position;
const bool beyond_last_readable =
last_timestamp_ns.has_value() &&
current_timestamp_ns.has_value() &&
*current_timestamp_ns >= *last_timestamp_ns;
if (at_or_past_tail || beyond_last_readable) {
spdlog::warn(
"treating {} unreadable tail frame(s) as end-of-svo for {} last_corrupted_position={} current_timestamp_ns={} last_timestamp_ns={}",
result.corrupted_frames_skipped,
source_path,
svo_position,
current_timestamp_ns.value_or(0),
last_timestamp_ns.value_or(0));
return std::unexpected("end-of-svo");
}
if (svo_position < 0) {
return std::unexpected(
"failed to advance past corrupted frame for " + std::string(source_label) +
": invalid svo_position=" + std::to_string(svo_position));
}
const auto next_position = svo_position + 1;
if (next_position >= static_cast<int>(total_frames)) {
spdlog::warn(
"treating {} unreadable tail frame(s) as end-of-svo for {} last_corrupted_position={} current_timestamp_ns={} last_timestamp_ns={}",
result.corrupted_frames_skipped,
source_path,
svo_position,
current_timestamp_ns.value_or(0),
last_timestamp_ns.value_or(0));
return std::unexpected("end-of-svo");
}
camera.setSVOPosition(next_position);
continue;
}
if (grab_status != sl::ERROR_CODE::SUCCESS) {
return std::unexpected(
"failed to grab frame for " + std::string(source_label) +
": " + zed_status_string(grab_status) +
" svo_position=" + std::to_string(svo_position) +
" total_frames=" + std::to_string(total_frames) +
" current_timestamp_ns=" + std::to_string(current_timestamp_ns.value_or(0)) +
" last_timestamp_ns=" + std::to_string(last_timestamp_ns.value_or(0)) +
" corrupted_frames_skipped=" + std::to_string(result.corrupted_frames_skipped));
}
result.svo_position = svo_position;
return result;
}
}
void maybe_log_recovered_corruption_gap(
const std::string_view source_label,
const GrabResult &grab_result,
const std::uint64_t recovered_timestamp_ns) {
if (grab_result.corrupted_frames_skipped == 0) {
return;
}
spdlog::warn(
"recovered {} after skipping {} corrupted frame(s) positions={}..{} recovered_position={} recovered_timestamp_ns={}",
source_label,
grab_result.corrupted_frames_skipped,
grab_result.first_corrupted_position,
grab_result.last_corrupted_position,
grab_result.svo_position,
recovered_timestamp_ns);
}
[[nodiscard]]
std::expected<GrabResult, std::string> read_frame_data(
CameraStream &stream, CameraStream &stream,
sl::Mat &left_frame, sl::Mat &left_frame,
sl::Mat &depth_frame, sl::Mat &depth_frame,
TrackingSample &tracking_sample, TrackingSample &tracking_sample,
std::optional<std::uint64_t> fallback_timestamp_ns, std::optional<std::uint64_t> fallback_timestamp_ns,
std::uint64_t &timestamp_ns_out) { std::uint64_t &timestamp_ns_out) {
const auto grab_status = stream.camera->grab(stream.runtime); auto grab = grab_next_readable_frame(
if (grab_status == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) { *stream.camera,
return std::unexpected("end-of-svo"); stream.runtime,
} stream.source.label,
if (grab_status == sl::ERROR_CODE::CORRUPTED_FRAME &&
stream.last_timestamp_ns != 0 &&
stream.current_timestamp_ns >= stream.last_timestamp_ns) {
spdlog::warn(
"treating unreadable tail frame as end-of-svo for {} current_timestamp_ns={} last_timestamp_ns={}",
stream.source.path.string(), stream.source.path.string(),
stream.current_timestamp_ns, stream.total_frames,
stream.last_timestamp_ns); stream.current_timestamp_ns == 0 ? std::nullopt : std::optional<std::uint64_t>{stream.current_timestamp_ns},
return std::unexpected("end-of-svo"); stream.last_timestamp_ns == 0 ? std::nullopt : std::optional<std::uint64_t>{stream.last_timestamp_ns});
} if (!grab) {
if (grab_status != sl::ERROR_CODE::SUCCESS) { return std::unexpected(grab.error());
const auto svo_position = stream.camera->getSVOPosition();
return std::unexpected(
"failed to grab frame for " + stream.source.label +
": " + zed_status_string(grab_status) +
" svo_position=" + std::to_string(svo_position) +
" total_frames=" + std::to_string(stream.total_frames) +
" current_timestamp_ns=" + std::to_string(stream.current_timestamp_ns) +
" last_timestamp_ns=" + std::to_string(stream.last_timestamp_ns));
} }
const auto image_status = stream.camera->retrieveImage(left_frame, sl::VIEW::LEFT_BGR, sl::MEM::CPU); const auto image_status = stream.camera->retrieveImage(left_frame, sl::VIEW::LEFT_BGR, sl::MEM::CPU);
@@ -718,6 +833,7 @@ std::expected<void, std::string> read_frame_data(
return std::unexpected(timestamp_ns.error()); return std::unexpected(timestamp_ns.error());
} }
timestamp_ns_out = *timestamp_ns; timestamp_ns_out = *timestamp_ns;
maybe_log_recovered_corruption_gap(stream.source.label, *grab, timestamp_ns_out);
tracking_sample = {}; tracking_sample = {};
if (stream.tracking_enabled) { if (stream.tracking_enabled) {
@@ -741,7 +857,7 @@ std::expected<void, std::string> read_frame_data(
}; };
} }
} }
return {}; return *grab;
} }
[[nodiscard]] [[nodiscard]]
@@ -756,11 +872,13 @@ std::expected<void, std::string> fill_next_frame(CameraStream &stream) {
if (!next) { if (!next) {
if (next.error() == "end-of-svo") { if (next.error() == "end-of-svo") {
stream.has_next = false; stream.has_next = false;
stream.next_corrupted_frames_skipped = 0;
return {}; return {};
} }
return std::unexpected(next.error()); return std::unexpected(next.error());
} }
stream.has_next = true; stream.has_next = true;
stream.next_corrupted_frames_skipped = next->corrupted_frames_skipped;
return {}; return {};
} }
@@ -775,6 +893,7 @@ std::expected<void, std::string> promote_next_frame(CameraStream &stream) {
std::swap(stream.current_tracking, stream.next_tracking); std::swap(stream.current_tracking, stream.next_tracking);
std::swap(stream.current_timestamp_ns, stream.next_timestamp_ns); std::swap(stream.current_timestamp_ns, stream.next_timestamp_ns);
stream.has_next = false; stream.has_next = false;
stream.next_corrupted_frames_skipped = 0;
return fill_next_frame(stream); return fill_next_frame(stream);
} }
@@ -1049,14 +1168,19 @@ std::expected<void, std::string> sync_streams_to_timestamp(
if (log_shutdown_request(shutdown_logged, "multi-camera sync")) { if (log_shutdown_request(shutdown_logged, "multi-camera sync")) {
return std::unexpected("interrupted"); return std::unexpected("interrupted");
} }
stream.sync_position = stream.camera->getSVOPositionAtTimestamp(sl::Timestamp{effective_start_ts}); const auto sdk_position = stream.camera->getSVOPositionAtTimestamp(sl::Timestamp{effective_start_ts});
if (stream.sync_position < 0) { if (sdk_position < 0) {
return std::unexpected( return std::unexpected(
"failed to compute synced start frame for " + stream.source.path.string() + " at timestamp " + "failed to compute synced start frame for " + stream.source.path.string() + " at timestamp " +
std::to_string(effective_start_ts)); std::to_string(effective_start_ts));
} }
stream.sync_position = std::clamp(
sdk_position,
0,
static_cast<int>(stream.total_frames > 0 ? stream.total_frames - 1 : 0));
stream.camera->setSVOPosition(stream.sync_position); const auto read_at_position = [&](const int position) -> std::expected<void, std::string> {
stream.camera->setSVOPosition(position);
auto current = read_frame_data( auto current = read_frame_data(
stream, stream,
stream.current_left_frame, stream.current_left_frame,
@@ -1071,8 +1195,24 @@ std::expected<void, std::string> sync_streams_to_timestamp(
if (!next) { if (!next) {
return std::unexpected(next.error()); return std::unexpected(next.error());
} }
return {};
};
while (stream.current_timestamp_ns < effective_start_ts && stream.has_next) { if (auto loaded = read_at_position(stream.sync_position); !loaded) {
return std::unexpected(loaded.error());
}
while (stream.sync_position > 0 && stream.current_timestamp_ns > effective_start_ts) {
if (log_shutdown_request(shutdown_logged, "multi-camera sync")) {
return std::unexpected("interrupted");
}
stream.sync_position -= 1;
if (auto loaded = read_at_position(stream.sync_position); !loaded) {
return std::unexpected(loaded.error());
}
}
while (stream.has_next && stream.next_timestamp_ns <= effective_start_ts) {
if (log_shutdown_request(shutdown_logged, "multi-camera sync")) { if (log_shutdown_request(shutdown_logged, "multi-camera sync")) {
return std::unexpected("interrupted"); return std::unexpected("interrupted");
} }
@@ -1107,6 +1247,119 @@ bool have_sync_window(const std::vector<CameraStream> &streams, const std::uint6
return true; return true;
} }
[[nodiscard]]
std::expected<void, std::string> advance_streams_to_timestamp(
std::vector<CameraStream> &streams,
const std::uint64_t target_timestamp_ns) {
for (auto &stream : streams) {
while (stream.has_next && stream.next_timestamp_ns <= target_timestamp_ns) {
stream.dropped_frames += 1;
auto promote = promote_next_frame(stream);
if (!promote) {
return std::unexpected(promote.error());
}
}
}
return {};
}
[[nodiscard]]
std::uint64_t timestamp_delta_abs(const std::uint64_t left, const std::uint64_t right) {
return left >= right ? left - right : right - left;
}
[[nodiscard]]
std::expected<std::vector<BundledFrameSelection>, std::string> select_nearest_bundle(
const std::vector<CameraStream> &streams,
const std::uint64_t bundle_timestamp_ns,
const std::uint64_t common_end_ts) {
std::vector<BundledFrameSelection> selections{};
selections.reserve(streams.size());
for (std::size_t stream_index = 0; stream_index < streams.size(); ++stream_index) {
const auto &stream = streams[stream_index];
if (stream.current_timestamp_ns > common_end_ts) {
return std::unexpected("no bundle frames remain within the common overlap window");
}
const bool blocked_by_corruption_gap =
stream.has_next &&
stream.next_corrupted_frames_skipped > 0 &&
bundle_timestamp_ns > stream.current_timestamp_ns &&
bundle_timestamp_ns < stream.next_timestamp_ns;
if (blocked_by_corruption_gap) {
selections.push_back(BundledFrameSelection{
.stream_index = stream_index,
.status = cvmmap_streamer::record::BundleMemberStatus::CorruptedGap,
.use_next = false,
.timestamp_ns = std::nullopt,
.delta_ns = 0,
.corrupted_frames_skipped = stream.next_corrupted_frames_skipped,
});
continue;
}
bool use_next = false;
std::uint64_t selected_timestamp_ns = stream.current_timestamp_ns;
if (stream.has_next && stream.next_timestamp_ns <= common_end_ts) {
const auto current_delta = timestamp_delta_abs(stream.current_timestamp_ns, bundle_timestamp_ns);
const auto next_delta = timestamp_delta_abs(stream.next_timestamp_ns, bundle_timestamp_ns);
if (next_delta <= current_delta) {
use_next = true;
selected_timestamp_ns = stream.next_timestamp_ns;
}
}
selections.push_back(BundledFrameSelection{
.stream_index = stream_index,
.status = cvmmap_streamer::record::BundleMemberStatus::Present,
.use_next = use_next,
.timestamp_ns = selected_timestamp_ns,
.delta_ns = static_cast<std::int64_t>(selected_timestamp_ns) -
static_cast<std::int64_t>(bundle_timestamp_ns),
.corrupted_frames_skipped = 0,
});
}
return selections;
}
[[nodiscard]]
std::vector<BundledFrameSelection> make_strict_bundle(
const std::vector<CameraStream> &streams,
const std::uint64_t bundle_timestamp_ns) {
std::vector<BundledFrameSelection> selections{};
selections.reserve(streams.size());
for (std::size_t stream_index = 0; stream_index < streams.size(); ++stream_index) {
const auto &stream = streams[stream_index];
selections.push_back(BundledFrameSelection{
.stream_index = stream_index,
.status = cvmmap_streamer::record::BundleMemberStatus::Present,
.use_next = false,
.timestamp_ns = stream.current_timestamp_ns,
.delta_ns = static_cast<std::int64_t>(stream.current_timestamp_ns) -
static_cast<std::int64_t>(bundle_timestamp_ns),
.corrupted_frames_skipped = 0,
});
}
return selections;
}
[[nodiscard]]
const sl::Mat &selected_left_frame(const CameraStream &stream, const BundledFrameSelection &selection) {
return selection.use_next ? stream.next_left_frame : stream.current_left_frame;
}
[[nodiscard]]
const sl::Mat &selected_depth_frame(const CameraStream &stream, const BundledFrameSelection &selection) {
return selection.use_next ? stream.next_depth_frame : stream.current_depth_frame;
}
[[nodiscard]]
const TrackingSample &selected_tracking(const CameraStream &stream, const BundledFrameSelection &selection) {
return selection.use_next ? stream.next_tracking : stream.current_tracking;
}
[[nodiscard]] [[nodiscard]]
std::expected<std::optional<std::uint64_t>, std::string> next_synced_group_timestamp( std::expected<std::optional<std::uint64_t>, std::string> next_synced_group_timestamp(
std::vector<CameraStream> &streams, std::vector<CameraStream> &streams,
@@ -1167,15 +1420,66 @@ std::expected<void, std::string> encode_and_write_group(
cvmmap_streamer::record::MultiMcapRecordSink &sink, cvmmap_streamer::record::MultiMcapRecordSink &sink,
std::vector<CameraStream> &streams, std::vector<CameraStream> &streams,
const CliOptions &options, const CliOptions &options,
const std::uint64_t group_timestamp_ns) { const cvmmap_streamer::record::BundlePolicy bundle_policy,
for (auto &stream : streams) { const std::uint64_t bundle_index,
const auto video_step_bytes = stream.current_left_frame.getStepBytes(sl::MEM::CPU); const std::uint64_t bundle_timestamp_ns,
const std::span<const BundledFrameSelection> selections) {
if (selections.size() != streams.size()) {
return std::unexpected("bundle selection size does not match stream count");
}
std::vector<cvmmap_streamer::record::RawBundleMemberView> bundle_members{};
bundle_members.reserve(selections.size());
for (const auto &selection : selections) {
const auto &stream = streams[selection.stream_index];
bundle_members.push_back(cvmmap_streamer::record::RawBundleMemberView{
.camera_label = stream.source.label,
.status = selection.status,
.timestamp_ns = selection.timestamp_ns,
.delta_ns = selection.delta_ns,
.corrupted_frames_skipped = selection.corrupted_frames_skipped,
});
}
if (auto write = sink.write_bundle_manifest(cvmmap_streamer::record::RawBundleManifestView{
.timestamp_ns = bundle_timestamp_ns,
.bundle_index = bundle_index,
.policy = bundle_policy,
.members = bundle_members,
}); !write) {
return std::unexpected("failed to write bundle manifest: " + write.error());
}
std::vector<BundledFrameSelection> ordered_selections{};
ordered_selections.reserve(selections.size());
for (const auto &selection : selections) {
if (selection.status == cvmmap_streamer::record::BundleMemberStatus::Present &&
selection.timestamp_ns.has_value()) {
ordered_selections.push_back(selection);
}
}
std::sort(
ordered_selections.begin(),
ordered_selections.end(),
[](const auto &left, const auto &right) {
if (left.timestamp_ns != right.timestamp_ns) {
return left.timestamp_ns < right.timestamp_ns;
}
return left.stream_index < right.stream_index;
});
for (const auto &selection : ordered_selections) {
auto &stream = streams[selection.stream_index];
const auto &left_frame = selected_left_frame(stream, selection);
const auto &depth_frame = selected_depth_frame(stream, selection);
const auto &tracking = selected_tracking(stream, selection);
const auto video_step_bytes = left_frame.getStepBytes(sl::MEM::CPU);
const auto video_bytes = std::span<const std::uint8_t>( const auto video_bytes = std::span<const std::uint8_t>(
stream.current_left_frame.getPtr<sl::uchar1>(sl::MEM::CPU), left_frame.getPtr<sl::uchar1>(sl::MEM::CPU),
video_step_bytes * stream.current_left_frame.getHeight()); video_step_bytes * left_frame.getHeight());
cvmmap_streamer::encode::RawVideoFrame raw_video{ cvmmap_streamer::encode::RawVideoFrame raw_video{
.info = stream.frame_info, .info = stream.frame_info,
.source_timestamp_ns = group_timestamp_ns, .source_timestamp_ns = *selection.timestamp_ns,
.row_stride_bytes = video_step_bytes, .row_stride_bytes = video_step_bytes,
.bytes = video_bytes, .bytes = video_bytes,
}; };
@@ -1196,7 +1500,7 @@ std::expected<void, std::string> encode_and_write_group(
if (!stream.calibration_written) { if (!stream.calibration_written) {
cvmmap_streamer::record::RawCameraCalibrationView calibration{ cvmmap_streamer::record::RawCameraCalibrationView calibration{
.timestamp_ns = group_timestamp_ns, .timestamp_ns = *selection.timestamp_ns,
.width = stream.video_calibration.width, .width = stream.video_calibration.width,
.height = stream.video_calibration.height, .height = stream.video_calibration.height,
.distortion_model = "plumb_bob", .distortion_model = "plumb_bob",
@@ -1214,7 +1518,7 @@ std::expected<void, std::string> encode_and_write_group(
if (!stream.depth_calibration_written && if (!stream.depth_calibration_written &&
(stream.depth_width != stream.width || stream.depth_height != stream.height)) { (stream.depth_width != stream.width || stream.depth_height != stream.height)) {
cvmmap_streamer::record::RawCameraCalibrationView depth_calibration{ cvmmap_streamer::record::RawCameraCalibrationView depth_calibration{
.timestamp_ns = group_timestamp_ns, .timestamp_ns = *selection.timestamp_ns,
.width = stream.depth_calibration.width, .width = stream.depth_calibration.width,
.height = stream.depth_calibration.height, .height = stream.depth_calibration.height,
.distortion_model = "plumb_bob", .distortion_model = "plumb_bob",
@@ -1229,15 +1533,15 @@ std::expected<void, std::string> encode_and_write_group(
stream.depth_calibration_written = true; stream.depth_calibration_written = true;
} }
const auto depth_width = static_cast<std::uint32_t>(stream.current_depth_frame.getWidth()); const auto depth_width = static_cast<std::uint32_t>(depth_frame.getWidth());
const auto depth_height = static_cast<std::uint32_t>(stream.current_depth_frame.getHeight()); const auto depth_height = static_cast<std::uint32_t>(depth_frame.getHeight());
if (depth_width != stream.depth_width || depth_height != stream.depth_height) { if (depth_width != stream.depth_width || depth_height != stream.depth_height) {
return std::unexpected( return std::unexpected(
"depth resolution changed unexpectedly for " + stream.source.label + ": " + "depth resolution changed unexpectedly for " + stream.source.label + ": " +
std::to_string(depth_width) + "x" + std::to_string(depth_height) + " vs " + std::to_string(depth_width) + "x" + std::to_string(depth_height) + " vs " +
std::to_string(stream.depth_width) + "x" + std::to_string(stream.depth_height)); std::to_string(stream.depth_width) + "x" + std::to_string(stream.depth_height));
} }
const auto depth_step_bytes = stream.current_depth_frame.getStepBytes(sl::MEM::CPU); const auto depth_step_bytes = depth_frame.getStepBytes(sl::MEM::CPU);
const auto packed_depth_bytes = static_cast<std::size_t>(depth_width) * sizeof(std::uint16_t); const auto packed_depth_bytes = static_cast<std::size_t>(depth_width) * sizeof(std::uint16_t);
if (depth_step_bytes < packed_depth_bytes) { if (depth_step_bytes < packed_depth_bytes) {
return std::unexpected( return std::unexpected(
@@ -1249,15 +1553,15 @@ std::expected<void, std::string> encode_and_write_group(
std::span<const std::uint16_t> depth_pixels{}; std::span<const std::uint16_t> depth_pixels{};
if (depth_step_bytes == packed_depth_bytes) { if (depth_step_bytes == packed_depth_bytes) {
depth_pixels = std::span<const std::uint16_t>( depth_pixels = std::span<const std::uint16_t>(
stream.current_depth_frame.getPtr<sl::ushort1>(sl::MEM::CPU), depth_frame.getPtr<sl::ushort1>(sl::MEM::CPU),
static_cast<std::size_t>(depth_width) * static_cast<std::size_t>(depth_height)); static_cast<std::size_t>(depth_width) * static_cast<std::size_t>(depth_height));
} else { } else {
compact_depth = copy_compact_u16_plane(stream.current_depth_frame); compact_depth = copy_compact_u16_plane(depth_frame);
depth_pixels = *compact_depth; depth_pixels = *compact_depth;
} }
cvmmap_streamer::record::RawDepthMapU16View depth_map{ cvmmap_streamer::record::RawDepthMapU16View depth_map{
.timestamp_ns = group_timestamp_ns, .timestamp_ns = *selection.timestamp_ns,
.width = depth_width, .width = depth_width,
.height = depth_height, .height = depth_height,
.pixels = depth_pixels, .pixels = depth_pixels,
@@ -1266,12 +1570,12 @@ std::expected<void, std::string> encode_and_write_group(
return std::unexpected("failed to write depth map for " + stream.source.label + ": " + write.error()); return std::unexpected("failed to write depth map for " + stream.source.label + ": " + write.error());
} }
if (options.with_pose && stream.current_tracking.has_pose) { if (options.with_pose && tracking.has_pose) {
cvmmap_streamer::record::RawPoseView pose_view{ cvmmap_streamer::record::RawPoseView pose_view{
.timestamp_ns = group_timestamp_ns, .timestamp_ns = *selection.timestamp_ns,
.reference_frame_id = pose_reference_frame_id(stream.pose_tracking, stream.source.label), .reference_frame_id = pose_reference_frame_id(stream.pose_tracking, stream.source.label),
.position = stream.current_tracking.position, .position = tracking.position,
.orientation = stream.current_tracking.orientation, .orientation = tracking.orientation,
}; };
if (auto write = sink.write_pose(stream.mcap_stream_id, pose_view); !write) { if (auto write = sink.write_pose(stream.mcap_stream_id, pose_view); !write) {
return std::unexpected("failed to write pose for " + stream.source.label + ": " + write.error()); return std::unexpected("failed to write pose for " + stream.source.label + ": " + write.error());
@@ -1281,6 +1585,23 @@ std::expected<void, std::string> encode_and_write_group(
return {}; return {};
} }
[[nodiscard]]
std::expected<void, std::string> advance_after_nearest_emit(
std::vector<CameraStream> &streams,
const std::span<const BundledFrameSelection> selections) {
for (const auto &selection : selections) {
if (selection.status != cvmmap_streamer::record::BundleMemberStatus::Present || !selection.use_next) {
continue;
}
auto &stream = streams[selection.stream_index];
auto promote = promote_next_frame(stream);
if (!promote) {
return std::unexpected(promote.error());
}
}
return {};
}
[[nodiscard]] [[nodiscard]]
std::expected<void, std::string> advance_after_emit(std::vector<CameraStream> &streams) { std::expected<void, std::string> advance_after_emit(std::vector<CameraStream> &streams) {
for (auto &stream : streams) { for (auto &stream : streams) {
@@ -1552,31 +1873,33 @@ int run_single_source(
const auto total_frames_to_emit = static_cast<std::uint64_t>(last_frame - options.start_frame + 1); const auto total_frames_to_emit = static_cast<std::uint64_t>(last_frame - options.start_frame + 1);
ProgressBar progress{total_frames_to_emit}; ProgressBar progress{total_frames_to_emit};
while (options.start_frame + emitted_frames <= last_frame) { while (true) {
if (log_shutdown_request(shutdown_logged, "single-camera export")) { if (log_shutdown_request(shutdown_logged, "single-camera export")) {
interrupted = true; interrupted = true;
break; break;
} }
const auto grab_status = camera.grab(runtime_parameters); auto grab = grab_next_readable_frame(
if (grab_status == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) { camera,
break; runtime_parameters,
} input_path.filename().string(),
if (grab_status == sl::ERROR_CODE::CORRUPTED_FRAME &&
camera.getSVOPosition() >= static_cast<int>(last_frame)) {
spdlog::warn(
"treating unreadable tail frame as end-of-svo for {} at frame {}",
input_path.string(), input_path.string(),
camera.getSVOPosition()); static_cast<std::uint64_t>(total_frames),
last_timestamp_ns,
std::nullopt);
if (!grab && grab.error() == "end-of-svo") {
break; break;
} }
if (grab_status != sl::ERROR_CODE::SUCCESS) { if (!grab) {
progress.finish(emitted_frames, false); progress.finish(emitted_frames, false);
sink->close(); sink->close();
backend->shutdown(); backend->shutdown();
close_camera(); close_camera();
spdlog::error("failed to grab SVO frame: {}", zed_status_string(grab_status)); spdlog::error("{}", grab.error());
return exit_code(ToolExitCode::RuntimeError); return exit_code(ToolExitCode::RuntimeError);
} }
if (grab->svo_position > static_cast<int>(last_frame)) {
break;
}
const auto image_status = camera.retrieveImage(left_frame, sl::VIEW::LEFT_BGR, sl::MEM::CPU); const auto image_status = camera.retrieveImage(left_frame, sl::VIEW::LEFT_BGR, sl::MEM::CPU);
if (image_status != sl::ERROR_CODE::SUCCESS) { if (image_status != sl::ERROR_CODE::SUCCESS) {
@@ -1621,6 +1944,7 @@ int run_single_source(
if (last_timestamp_ns && timestamp_ns <= *last_timestamp_ns) { if (last_timestamp_ns && timestamp_ns <= *last_timestamp_ns) {
timestamp_ns = *last_timestamp_ns + 1; timestamp_ns = *last_timestamp_ns + 1;
} }
maybe_log_recovered_corruption_gap(input_path.filename().string(), *grab, timestamp_ns);
last_timestamp_ns = timestamp_ns; last_timestamp_ns = timestamp_ns;
const auto video_step_bytes = left_frame.getStepBytes(sl::MEM::CPU); const auto video_step_bytes = left_frame.getStepBytes(sl::MEM::CPU);
@@ -1783,7 +2107,7 @@ int run_single_source(
spdlog::warn( spdlog::warn(
"pose tracking state changed to {} at frame {}", "pose tracking state changed to {} at frame {}",
zed_tracking_state_string(tracking_state), zed_tracking_state_string(tracking_state),
options.start_frame + emitted_frames); grab->svo_position);
} }
} }
if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK) { if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK) {
@@ -1861,10 +2185,11 @@ int run_multi_source(
const cvmmap_streamer::McapCompression compression, const cvmmap_streamer::McapCompression compression,
const sl::DEPTH_MODE depth_mode, const sl::DEPTH_MODE depth_mode,
const sl::Resolution depth_size, const sl::Resolution depth_size,
const PoseTrackingOptions &pose_tracking) { const PoseTrackingOptions &pose_tracking,
const cvmmap_streamer::record::BundlePolicy bundle_policy) {
if (options.has_end_frame && options.end_frame < options.start_frame) { if (options.has_end_frame && options.end_frame < options.start_frame) {
spdlog::error( spdlog::error(
"invalid bundled group range: start-frame={} end-frame={}", "invalid bundled range: start-frame={} end-frame={}",
options.start_frame, options.start_frame,
options.end_frame); options.end_frame);
return exit_code(ToolExitCode::UsageError); return exit_code(ToolExitCode::UsageError);
@@ -1915,21 +2240,68 @@ int run_multi_source(
[](const auto &left, const auto &right) { [](const auto &left, const auto &right) {
return left.nominal_frame_period_ns < right.nominal_frame_period_ns; return left.nominal_frame_period_ns < right.nominal_frame_period_ns;
})->nominal_frame_period_ns; })->nominal_frame_period_ns;
const auto bundle_policy_name = bundle_policy == cvmmap_streamer::record::BundlePolicy::Nearest
? "nearest"
: "strict";
const auto tolerance_ns = options.has_sync_tolerance const auto tolerance_ns = options.has_sync_tolerance
? static_cast<std::uint64_t>(std::llround(options.sync_tolerance_ms * 1'000'000.0)) ? static_cast<std::uint64_t>(std::llround(options.sync_tolerance_ms * 1'000'000.0))
: std::max<std::uint64_t>(1, slowest_period_ns / 4); : std::max<std::uint64_t>(1, slowest_period_ns);
spdlog::info( spdlog::info(
"multi-camera sync window start_ts={} end_ts={} tolerance_ns={}", "multi-camera bundle window start_ts={} end_ts={} policy={} bundle_period_ns={} tolerance_ns={}",
common_start_ts, common_start_ts,
common_end_ts, common_end_ts,
bundle_policy_name,
slowest_period_ns,
tolerance_ns); tolerance_ns);
const auto render_progress = stderr_supports_progress_bar(); const auto render_progress = stderr_supports_progress_bar();
const std::uint64_t selected_total_groups = options.has_end_frame const auto total_timeline_bundles = common_end_ts >= common_start_ts
? static_cast<std::uint64_t>(options.end_frame - options.start_frame) + 1 ? ((common_end_ts - common_start_ts) / slowest_period_ns) + 1
: 0; : 0;
const bool exact_group_progress = render_progress && options.has_end_frame; if (bundle_policy == cvmmap_streamer::record::BundlePolicy::Nearest &&
const bool approximate_time_progress = render_progress && !options.has_end_frame; options.start_frame >= total_timeline_bundles) {
if (auto synced = sync_streams_to_timestamp(streams, common_start_ts); !synced) { close_camera_streams(streams);
spdlog::error(
"start-frame {} is out of range for bundled multi-camera mode (available bundles: {})",
options.start_frame,
total_timeline_bundles);
return exit_code(ToolExitCode::UsageError);
}
if (bundle_policy == cvmmap_streamer::record::BundlePolicy::Nearest &&
options.has_end_frame &&
options.end_frame >= total_timeline_bundles) {
close_camera_streams(streams);
spdlog::error(
"end-frame {} is out of range for bundled multi-camera mode (available bundles: {})",
options.end_frame,
total_timeline_bundles);
return exit_code(ToolExitCode::UsageError);
}
const auto selected_end_bundle = options.has_end_frame
? static_cast<std::uint64_t>(options.end_frame)
: (bundle_policy == cvmmap_streamer::record::BundlePolicy::Nearest
? total_timeline_bundles - 1
: 0);
const auto selected_total_groups = options.has_end_frame
? static_cast<std::uint64_t>(options.end_frame - options.start_frame) + 1
: (bundle_policy == cvmmap_streamer::record::BundlePolicy::Nearest
? total_timeline_bundles - static_cast<std::uint64_t>(options.start_frame)
: 0);
const bool exact_group_progress =
render_progress &&
(bundle_policy == cvmmap_streamer::record::BundlePolicy::Nearest || options.has_end_frame);
const bool approximate_time_progress =
render_progress &&
bundle_policy == cvmmap_streamer::record::BundlePolicy::Strict &&
!options.has_end_frame;
ProgressBar progress{exact_group_progress ? selected_total_groups : 0};
double last_progress_fraction = 0.0;
std::string last_progress_detail{};
const auto initial_target_ts = bundle_policy == cvmmap_streamer::record::BundlePolicy::Nearest
? common_start_ts + static_cast<std::uint64_t>(options.start_frame) * slowest_period_ns
: common_start_ts;
if (auto synced = sync_streams_to_timestamp(streams, initial_target_ts); !synced) {
close_camera_streams(streams); close_camera_streams(streams);
if (synced.error() == "interrupted") { if (synced.error() == "interrupted") {
return interrupted_exit_code(); return interrupted_exit_code();
@@ -1937,8 +2309,8 @@ int run_multi_source(
spdlog::error("{}", synced.error()); spdlog::error("{}", synced.error());
return exit_code(ToolExitCode::RuntimeError); return exit_code(ToolExitCode::RuntimeError);
} }
auto effective_progress_start_ts = common_start_ts; auto effective_progress_start_ts = initial_target_ts;
if (options.start_frame > 0) { if (bundle_policy == cvmmap_streamer::record::BundlePolicy::Strict && options.start_frame > 0) {
if (auto skipped_to = skip_bundled_start_groups(streams, options.start_frame, tolerance_ns, common_end_ts); !skipped_to) { if (auto skipped_to = skip_bundled_start_groups(streams, options.start_frame, tolerance_ns, common_end_ts); !skipped_to) {
close_camera_streams(streams); close_camera_streams(streams);
if (skipped_to.error() == "interrupted") { if (skipped_to.error() == "interrupted") {
@@ -1950,11 +2322,11 @@ int run_multi_source(
effective_progress_start_ts = *skipped_to; effective_progress_start_ts = *skipped_to;
} }
} }
ProgressBar progress{exact_group_progress ? selected_total_groups : 0};
double last_progress_fraction = 0.0;
std::string last_progress_detail{};
auto sink = cvmmap_streamer::record::MultiMcapRecordSink::create(output_path.string(), compression); auto sink = cvmmap_streamer::record::MultiMcapRecordSink::create(
output_path.string(),
compression,
options.bundle_topic);
if (!sink) { if (!sink) {
if (approximate_time_progress) { if (approximate_time_progress) {
progress.finish_fraction(last_progress_fraction, false, last_progress_detail); progress.finish_fraction(last_progress_fraction, false, last_progress_detail);
@@ -1978,6 +2350,50 @@ int run_multi_source(
} }
std::uint64_t emitted_groups{0}; std::uint64_t emitted_groups{0};
if (bundle_policy == cvmmap_streamer::record::BundlePolicy::Nearest) {
for (std::uint64_t bundle_index = options.start_frame; bundle_index <= selected_end_bundle; ++bundle_index) {
if (log_shutdown_request(shutdown_logged, "multi-camera export")) {
interrupted = true;
break;
}
const auto bundle_timestamp_ns = common_start_ts + bundle_index * slowest_period_ns;
if (auto advanced = advance_streams_to_timestamp(streams, bundle_timestamp_ns); !advanced) {
progress.finish(emitted_groups, false);
sink->close();
close_camera_streams(streams);
spdlog::error("{}", advanced.error());
return exit_code(ToolExitCode::RuntimeError);
}
auto selections = select_nearest_bundle(streams, bundle_timestamp_ns, common_end_ts);
if (!selections) {
break;
}
if (auto write = encode_and_write_group(
*sink,
streams,
options,
bundle_policy,
bundle_index,
bundle_timestamp_ns,
*selections); !write) {
progress.finish(emitted_groups, false);
sink->close();
close_camera_streams(streams);
spdlog::error("{}", write.error());
return exit_code(ToolExitCode::RuntimeError);
}
if (auto advance = advance_after_nearest_emit(streams, *selections); !advance) {
progress.finish(emitted_groups, false);
sink->close();
close_camera_streams(streams);
spdlog::error("{}", advance.error());
return exit_code(ToolExitCode::RuntimeError);
}
emitted_groups += 1;
progress.update(emitted_groups);
}
} else {
while (true) { while (true) {
if (log_shutdown_request(shutdown_logged, "multi-camera export")) { if (log_shutdown_request(shutdown_logged, "multi-camera export")) {
interrupted = true; interrupted = true;
@@ -2002,7 +2418,16 @@ int run_multi_source(
break; break;
} }
if (auto write = encode_and_write_group(*sink, streams, options, **group_timestamp); !write) { const auto bundle_index = static_cast<std::uint64_t>(options.start_frame) + emitted_groups;
const auto selections = make_strict_bundle(streams, **group_timestamp);
if (auto write = encode_and_write_group(
*sink,
streams,
options,
bundle_policy,
bundle_index,
**group_timestamp,
selections); !write) {
if (approximate_time_progress) { if (approximate_time_progress) {
progress.finish_fraction(last_progress_fraction, false, last_progress_detail); progress.finish_fraction(last_progress_fraction, false, last_progress_detail);
} else { } else {
@@ -2041,6 +2466,7 @@ int run_multi_source(
return exit_code(ToolExitCode::RuntimeError); return exit_code(ToolExitCode::RuntimeError);
} }
} }
}
for (auto &stream : streams) { for (auto &stream : streams) {
if (auto flushed = flush_and_write(*sink, stream.mcap_stream_id, *stream.backend); !flushed) { if (auto flushed = flush_and_write(*sink, stream.mcap_stream_id, *stream.backend); !flushed) {
@@ -2083,15 +2509,16 @@ int run_multi_source(
remove_error.message()); remove_error.message());
} }
spdlog::error( spdlog::error(
"no synced frame groups were found across {} camera(s) for '{}'", "no bundled frame groups were found across {} camera(s) for '{}' using policy={}",
sources.size(), sources.size(),
output_path.string()); output_path.string(),
bundle_policy_name);
return exit_code(ToolExitCode::RuntimeError); return exit_code(ToolExitCode::RuntimeError);
} }
if (interrupted) { if (interrupted) {
spdlog::warn( spdlog::warn(
"gracefully stopped after writing {} synced frame group(s) across {} camera(s) to '{}'", "gracefully stopped after writing {} bundled frame group(s) across {} camera(s) to '{}'",
emitted_groups, emitted_groups,
sources.size(), sources.size(),
output_path.string()); output_path.string());
@@ -2099,10 +2526,11 @@ int run_multi_source(
} }
spdlog::info( spdlog::info(
"wrote {} synced frame group(s) across {} camera(s) to '{}'", "wrote {} bundled frame group(s) across {} camera(s) to '{}' using policy={}",
emitted_groups, emitted_groups,
sources.size(), sources.size(),
output_path.string()); output_path.string(),
bundle_policy_name);
return exit_code(ToolExitCode::Success); return exit_code(ToolExitCode::Success);
} }
@@ -2127,20 +2555,23 @@ int main(int argc, char **argv) {
->check(CLI::IsMember({"neural_light", "neural", "neural_plus"})); ->check(CLI::IsMember({"neural_light", "neural", "neural_plus"}));
app.add_option("--depth-size", options.depth_size, "Depth output size (optimal|native|<width>x<height>)") app.add_option("--depth-size", options.depth_size, "Depth output size (optimal|native|<width>x<height>)")
->default_val("optimal"); ->default_val("optimal");
app.add_option("--bundle-policy", options.bundle_policy, "Bundling policy for multi-camera mode (nearest|strict)")
->check(CLI::IsMember({"nearest", "strict"}));
app.add_option( app.add_option(
"--start-frame", "--start-frame",
options.start_frame, options.start_frame,
"First frame/group to export (inclusive): raw SVO frame in single-camera mode, synced group index in bundled multi-camera mode") "First frame/group to export (inclusive): raw SVO frame in single-camera mode, bundle index in multi-camera mode")
->check(CLI::NonNegativeNumber); ->check(CLI::NonNegativeNumber);
auto *end_frame_option = app.add_option( auto *end_frame_option = app.add_option(
"--end-frame", "--end-frame",
options.end_frame, options.end_frame,
"Last frame/group to export (inclusive): raw SVO frame in single-camera mode, synced group index in bundled multi-camera mode") "Last frame/group to export (inclusive): raw SVO frame in single-camera mode, bundle index in multi-camera mode")
->check(CLI::NonNegativeNumber); ->check(CLI::NonNegativeNumber);
app.add_option("--frame-id", options.frame_id, "Frame id for image and depth topics"); app.add_option("--frame-id", options.frame_id, "Frame id for image and depth topics");
app.add_option("--video-topic", options.video_topic, "MCAP topic for foxglove.CompressedVideo"); app.add_option("--video-topic", options.video_topic, "MCAP topic for foxglove.CompressedVideo");
app.add_option("--depth-topic", options.depth_topic, "MCAP topic for cvmmap_streamer.DepthMap"); app.add_option("--depth-topic", options.depth_topic, "MCAP topic for cvmmap_streamer.DepthMap");
app.add_option("--calibration-topic", options.calibration_topic, "MCAP topic for foxglove.CameraCalibration"); app.add_option("--calibration-topic", options.calibration_topic, "MCAP topic for foxglove.CameraCalibration");
app.add_option("--bundle-topic", options.bundle_topic, "MCAP topic for bundled multi-camera manifests");
app.add_option( app.add_option(
"--depth-calibration-topic", "--depth-calibration-topic",
options.depth_calibration_topic, options.depth_calibration_topic,
@@ -2155,7 +2586,7 @@ int main(int argc, char **argv) {
auto *sync_tolerance_option = app.add_option( auto *sync_tolerance_option = app.add_option(
"--sync-tolerance-ms", "--sync-tolerance-ms",
options.sync_tolerance_ms, options.sync_tolerance_ms,
"Maximum allowed timestamp delta between cameras in multi-camera mode"); "Maximum allowed timestamp delta between cameras in strict multi-camera mode");
sync_tolerance_option->check(CLI::PositiveNumber); sync_tolerance_option->check(CLI::PositiveNumber);
try { try {
@@ -2191,6 +2622,11 @@ int main(int argc, char **argv) {
spdlog::error("{}", depth_size.error()); spdlog::error("{}", depth_size.error());
return exit_code(ToolExitCode::UsageError); return exit_code(ToolExitCode::UsageError);
} }
auto bundle_policy = parse_bundle_policy(options.bundle_policy);
if (!bundle_policy) {
spdlog::error("{}", bundle_policy.error());
return exit_code(ToolExitCode::UsageError);
}
auto pose_tracking = load_pose_tracking_options(options); auto pose_tracking = load_pose_tracking_options(options);
if (!pose_tracking) { if (!pose_tracking) {
spdlog::error("{}", pose_tracking.error()); spdlog::error("{}", pose_tracking.error());
@@ -2226,5 +2662,6 @@ int main(int argc, char **argv) {
*compression, *compression,
*depth_mode, *depth_mode,
*depth_size, *depth_size,
*pose_tracking); *pose_tracking,
*bundle_policy);
} }