fix(record): fall back to millimeter for unknown depth unit
This commit is contained in:
@@ -192,6 +192,14 @@ bool frame_info_equal(const ipc::FrameInfo &lhs, const ipc::FrameInfo &rhs) {
|
|||||||
lhs.buffer_size == rhs.buffer_size;
|
lhs.buffer_size == rhs.buffer_size;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
[[nodiscard]]
|
||||||
|
ipc::DepthUnit normalize_depth_unit_for_recording(ipc::DepthUnit unit) {
|
||||||
|
if (unit == ipc::DepthUnit::Unknown) {
|
||||||
|
return ipc::DepthUnit::Millimeter;
|
||||||
|
}
|
||||||
|
return unit;
|
||||||
|
}
|
||||||
|
|
||||||
[[nodiscard]]
|
[[nodiscard]]
|
||||||
std::expected<record::RawDepthMapView, std::string> make_depth_map_view(const ipc::CoherentSnapshot &snapshot) {
|
std::expected<record::RawDepthMapView, std::string> make_depth_map_view(const ipc::CoherentSnapshot &snapshot) {
|
||||||
if (!snapshot.depth_info) {
|
if (!snapshot.depth_info) {
|
||||||
@@ -200,9 +208,6 @@ std::expected<record::RawDepthMapView, std::string> make_depth_map_view(const ip
|
|||||||
if (snapshot.depth.empty()) {
|
if (snapshot.depth.empty()) {
|
||||||
return std::unexpected("depth plane bytes are missing");
|
return std::unexpected("depth plane bytes are missing");
|
||||||
}
|
}
|
||||||
if (snapshot.depth_unit == ipc::DepthUnit::Unknown) {
|
|
||||||
return std::unexpected("depth plane unit is unknown");
|
|
||||||
}
|
|
||||||
|
|
||||||
const auto &depth_info = *snapshot.depth_info;
|
const auto &depth_info = *snapshot.depth_info;
|
||||||
if (depth_info.depth != ipc::Depth::F32 || depth_info.pixel_format != ipc::PixelFormat::GRAY) {
|
if (depth_info.depth != ipc::Depth::F32 || depth_info.pixel_format != ipc::PixelFormat::GRAY) {
|
||||||
@@ -223,7 +228,7 @@ std::expected<record::RawDepthMapView, std::string> make_depth_map_view(const ip
|
|||||||
.timestamp_ns = snapshot.metadata.timestamp_ns,
|
.timestamp_ns = snapshot.metadata.timestamp_ns,
|
||||||
.width = depth_info.width,
|
.width = depth_info.width,
|
||||||
.height = depth_info.height,
|
.height = depth_info.height,
|
||||||
.source_unit = snapshot.depth_unit,
|
.source_unit = normalize_depth_unit_for_recording(snapshot.depth_unit),
|
||||||
.pixels = std::span<const float>(
|
.pixels = std::span<const float>(
|
||||||
reinterpret_cast<const float *>(snapshot.depth.data()),
|
reinterpret_cast<const float *>(snapshot.depth.data()),
|
||||||
pixel_count),
|
pixel_count),
|
||||||
@@ -551,13 +556,15 @@ int run_pipeline(const RuntimeConfig &config) {
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mcap_sink.has_value() && !snapshot->depth.empty()) {
|
if (mcap_sink.has_value() && !snapshot->depth.empty()) {
|
||||||
if (snapshot->depth_unit == ipc::DepthUnit::Unknown) {
|
if (snapshot->depth_unit == ipc::DepthUnit::Unknown) {
|
||||||
if (!warned_unknown_depth_unit) {
|
if (!warned_unknown_depth_unit) {
|
||||||
spdlog::warn("pipeline depth plane present but depth_unit is unknown; skipping depth MCAP recording");
|
spdlog::warn(
|
||||||
warned_unknown_depth_unit = true;
|
"pipeline depth plane present but depth_unit is unknown; assuming millimeters for MCAP recording, producer should upgrade to the ABI with explicit depth_unit metadata");
|
||||||
|
warned_unknown_depth_unit = true;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
} else {
|
|
||||||
auto depth_map = make_depth_map_view(*snapshot);
|
auto depth_map = make_depth_map_view(*snapshot);
|
||||||
if (!depth_map) {
|
if (!depth_map) {
|
||||||
const auto reason = "pipeline depth snapshot invalid: " + depth_map.error();
|
const auto reason = "pipeline depth snapshot invalid: " + depth_map.error();
|
||||||
@@ -572,7 +579,6 @@ int run_pipeline(const RuntimeConfig &config) {
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
stats.pushed_frames += 1;
|
stats.pushed_frames += 1;
|
||||||
auto drain = drain_encoder(
|
auto drain = drain_encoder(
|
||||||
|
|||||||
@@ -39,6 +39,14 @@ struct EncodedDepthPayload {
|
|||||||
std::vector<std::uint8_t> bytes{};
|
std::vector<std::uint8_t> bytes{};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
[[nodiscard]]
|
||||||
|
ipc::DepthUnit normalize_depth_source_unit(ipc::DepthUnit unit) {
|
||||||
|
if (unit == ipc::DepthUnit::Unknown) {
|
||||||
|
return ipc::DepthUnit::Millimeter;
|
||||||
|
}
|
||||||
|
return unit;
|
||||||
|
}
|
||||||
|
|
||||||
[[nodiscard]]
|
[[nodiscard]]
|
||||||
mcap::Compression to_mcap_compression(McapCompression compression) {
|
mcap::Compression to_mcap_compression(McapCompression compression) {
|
||||||
switch (compression) {
|
switch (compression) {
|
||||||
@@ -227,7 +235,7 @@ std::expected<std::vector<std::uint8_t>, std::string> decoder_config_to_annexb(
|
|||||||
|
|
||||||
[[nodiscard]]
|
[[nodiscard]]
|
||||||
bool can_encode_lossless_u16_mm(const RawDepthMapView &depth_map) {
|
bool can_encode_lossless_u16_mm(const RawDepthMapView &depth_map) {
|
||||||
if (depth_map.source_unit != ipc::DepthUnit::Millimeter) {
|
if (normalize_depth_source_unit(depth_map.source_unit) != ipc::DepthUnit::Millimeter) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -249,18 +257,16 @@ bool can_encode_lossless_u16_mm(const RawDepthMapView &depth_map) {
|
|||||||
[[nodiscard]]
|
[[nodiscard]]
|
||||||
std::expected<EncodedDepthPayload, std::string> encode_depth_payload(const RawDepthMapView &depth_map) {
|
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);
|
const auto pixel_count = static_cast<std::size_t>(depth_map.width) * static_cast<std::size_t>(depth_map.height);
|
||||||
|
const auto source_unit = normalize_depth_source_unit(depth_map.source_unit);
|
||||||
if (depth_map.width == 0 || depth_map.height == 0) {
|
if (depth_map.width == 0 || depth_map.height == 0) {
|
||||||
return std::unexpected("depth map dimensions must be non-zero");
|
return std::unexpected("depth map dimensions must be non-zero");
|
||||||
}
|
}
|
||||||
if (pixel_count != depth_map.pixels.size()) {
|
if (pixel_count != depth_map.pixels.size()) {
|
||||||
return std::unexpected("depth map dimensions do not match the pixel buffer");
|
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 {
|
try {
|
||||||
if (can_encode_lossless_u16_mm(depth_map)) {
|
if (source_unit == ipc::DepthUnit::Millimeter && can_encode_lossless_u16_mm(depth_map)) {
|
||||||
std::vector<std::uint16_t> pixels(pixel_count, 0);
|
std::vector<std::uint16_t> pixels(pixel_count, 0);
|
||||||
for (std::size_t index = 0; index < pixel_count; ++index) {
|
for (std::size_t index = 0; index < pixel_count; ++index) {
|
||||||
const float sample = depth_map.pixels[index];
|
const float sample = depth_map.pixels[index];
|
||||||
@@ -277,14 +283,14 @@ std::expected<EncodedDepthPayload, std::string> encode_depth_payload(const RawDe
|
|||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<float> depth_m(pixel_count, std::numeric_limits<float>::quiet_NaN());
|
std::vector<float> depth_m(pixel_count, std::numeric_limits<float>::quiet_NaN());
|
||||||
float finite_max_m = 0.0f;
|
float finite_max_m = 0.0f;
|
||||||
for (std::size_t index = 0; index < pixel_count; ++index) {
|
for (std::size_t index = 0; index < pixel_count; ++index) {
|
||||||
float sample = depth_map.pixels[index];
|
float sample = depth_map.pixels[index];
|
||||||
if (depth_map.source_unit == ipc::DepthUnit::Millimeter && std::isfinite(sample)) {
|
if (source_unit == ipc::DepthUnit::Millimeter && std::isfinite(sample)) {
|
||||||
sample *= 0.001f;
|
sample *= 0.001f;
|
||||||
}
|
}
|
||||||
if (!std::isfinite(sample) || sample <= 0.0f) {
|
if (!std::isfinite(sample) || sample <= 0.0f) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -443,6 +449,7 @@ std::expected<void, std::string> McapRecordSink::write_depth_map(const RawDepthM
|
|||||||
if (state_ == nullptr) {
|
if (state_ == nullptr) {
|
||||||
return std::unexpected("MCAP sink is not open");
|
return std::unexpected("MCAP sink is not open");
|
||||||
}
|
}
|
||||||
|
const auto source_unit = normalize_depth_source_unit(depth_map.source_unit);
|
||||||
|
|
||||||
auto encoded = encode_depth_payload(depth_map);
|
auto encoded = encode_depth_payload(depth_map);
|
||||||
if (!encoded) {
|
if (!encoded) {
|
||||||
@@ -454,7 +461,7 @@ std::expected<void, std::string> McapRecordSink::write_depth_map(const RawDepthM
|
|||||||
message.set_frame_id(state_->frame_id);
|
message.set_frame_id(state_->frame_id);
|
||||||
message.set_width(depth_map.width);
|
message.set_width(depth_map.width);
|
||||||
message.set_height(depth_map.height);
|
message.set_height(depth_map.height);
|
||||||
message.set_source_unit(to_proto_depth_unit(depth_map.source_unit));
|
message.set_source_unit(to_proto_depth_unit(source_unit));
|
||||||
message.set_storage_unit(to_proto_storage_unit(encoded->storage_unit));
|
message.set_storage_unit(to_proto_storage_unit(encoded->storage_unit));
|
||||||
message.set_encoding(to_proto_depth_encoding(encoded->encoding));
|
message.set_encoding(to_proto_depth_encoding(encoded->encoding));
|
||||||
message.set_data(
|
message.set_data(
|
||||||
|
|||||||
@@ -114,6 +114,23 @@ int main(int argc, char **argv) {
|
|||||||
return exit_code(TesterExitCode::WriteError);
|
return exit_code(TesterExitCode::WriteError);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const float depth_unknown_pixels[4] = {
|
||||||
|
1500.0f,
|
||||||
|
2500.0f,
|
||||||
|
std::numeric_limits<float>::quiet_NaN(),
|
||||||
|
0.0f,
|
||||||
|
};
|
||||||
|
cvmmap_streamer::record::RawDepthMapView depth_unknown{};
|
||||||
|
depth_unknown.timestamp_ns = 13;
|
||||||
|
depth_unknown.width = 2;
|
||||||
|
depth_unknown.height = 2;
|
||||||
|
depth_unknown.source_unit = cvmmap_streamer::ipc::DepthUnit::Unknown;
|
||||||
|
depth_unknown.pixels = depth_unknown_pixels;
|
||||||
|
if (auto write = sink->write_depth_map(depth_unknown); !write) {
|
||||||
|
spdlog::error("failed to write unknown-unit depth map: {}", write.error());
|
||||||
|
return exit_code(TesterExitCode::WriteError);
|
||||||
|
}
|
||||||
|
|
||||||
sink->close();
|
sink->close();
|
||||||
|
|
||||||
mcap::McapReader reader{};
|
mcap::McapReader reader{};
|
||||||
@@ -173,7 +190,7 @@ int main(int argc, char **argv) {
|
|||||||
|
|
||||||
reader.close();
|
reader.close();
|
||||||
|
|
||||||
if (video_messages != 1 || depth_messages.size() != 2) {
|
if (video_messages != 1 || depth_messages.size() != 3) {
|
||||||
spdlog::error(
|
spdlog::error(
|
||||||
"unexpected message counts: video={} depth={}",
|
"unexpected message counts: video={} depth={}",
|
||||||
video_messages,
|
video_messages,
|
||||||
@@ -231,6 +248,31 @@ int main(int argc, char **argv) {
|
|||||||
return exit_code(TesterExitCode::VerificationError);
|
return exit_code(TesterExitCode::VerificationError);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const auto &unknown_message = depth_messages[2];
|
||||||
|
if (unknown_message.source_unit() != cvmmap_streamer::DepthMap::DEPTH_UNIT_MILLIMETER ||
|
||||||
|
unknown_message.storage_unit() != cvmmap_streamer::DepthMap::STORAGE_UNIT_MILLIMETER ||
|
||||||
|
unknown_message.encoding() != cvmmap_streamer::DepthMap::RVL_U16_LOSSLESS) {
|
||||||
|
spdlog::error("unknown-unit fallback metadata verification failed");
|
||||||
|
return exit_code(TesterExitCode::VerificationError);
|
||||||
|
}
|
||||||
|
const auto unknown_info = rvl::inspect_image(std::span<const std::uint8_t>(
|
||||||
|
reinterpret_cast<const std::uint8_t *>(unknown_message.data().data()),
|
||||||
|
unknown_message.data().size()));
|
||||||
|
const auto unknown_decoded = rvl::decompress_image(std::span<const std::uint8_t>(
|
||||||
|
reinterpret_cast<const std::uint8_t *>(unknown_message.data().data()),
|
||||||
|
unknown_message.data().size()));
|
||||||
|
if (unknown_info.format != rvl::ImageFormat::UInt16Lossless ||
|
||||||
|
unknown_info.rows != 2 ||
|
||||||
|
unknown_info.cols != 2 ||
|
||||||
|
unknown_decoded.pixels.size() != 4 ||
|
||||||
|
unknown_decoded.pixels[0] != 1500 ||
|
||||||
|
unknown_decoded.pixels[1] != 2500 ||
|
||||||
|
unknown_decoded.pixels[2] != 0 ||
|
||||||
|
unknown_decoded.pixels[3] != 0) {
|
||||||
|
spdlog::error("unknown-unit fallback RVL round-trip verification failed");
|
||||||
|
return exit_code(TesterExitCode::VerificationError);
|
||||||
|
}
|
||||||
|
|
||||||
spdlog::info(
|
spdlog::info(
|
||||||
"validated same-file MCAP video+depth recording at '{}'",
|
"validated same-file MCAP video+depth recording at '{}'",
|
||||||
output_path.string());
|
output_path.string());
|
||||||
|
|||||||
Reference in New Issue
Block a user