From dc7f9f5501ac4ef506a58f32e97b23713d258a82 Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:15:47 -0300 Subject: [PATCH] fix frame timestamps to use a comparable epoch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit steady_clock's epoch is unspecified and typically counts from process start or boot, so the published timestamp_ns could not be compared against another node's timestamps, wall clock or a recording — and it recorded when the node processed the frame rather than when the camera captured it. Publish two keys on every output instead: device_timestamp_ns from sl::Camera::getTimestamp(TIME_REFERENCE::IMAGE), and host_timestamp_ns from system_clock. Both are Unix-epoch nanoseconds, set as dora timestamp parameters. The SDK clock source is pinned to SYSTEM_CLOCK at construction, since it is process-wide and decides the epoch of the device value. Both are kept because the wall clock can step when NTP synchronises after boot on a Jetson, while the device timestamp is what two nodes reading the same frame can agree on. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 2 +- README.md | 27 ++++++++++++++++++++++--- src/main.cc | 50 +++++++++++++++++++++++++++++++---------------- src/zed/camera.cc | 10 ++++++++++ src/zed/camera.h | 2 ++ 5 files changed, 70 insertions(+), 21 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index f37926f..457edd5 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -38,7 +38,7 @@ Camera behavior is configured entirely through environment variables (see README - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). - **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + point cloud), calibration, and close. Exposes `handle()` for engine access. - **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. -- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `point_cloud` and `camera_info`. Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. +- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `point_cloud` and `camera_info`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. diff --git a/README.md b/README.md index 80a5c08..2b6cde3 100644 --- a/README.md +++ b/README.md @@ -119,6 +119,27 @@ All behavior is controlled via environment variables. | Output | Type | Description | |---|---|---| -| `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, `timestamp_ns`. | -| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics. | -| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients. | +| `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, timestamps. | +| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | +| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. | + +### Timestamps + +Every output carries both clocks, as dora `timestamp` metadata parameters: + +| Key | Epoch | Unit | Source | +|---|---|---|---| +| `device_timestamp_ns` | Unix epoch (UTC) | nanoseconds | `sl::Camera::getTimestamp(TIME_REFERENCE::IMAGE)` — when the camera captured the frame | +| `host_timestamp_ns` | Unix epoch (UTC) | nanoseconds | `std::chrono::system_clock` on the host — when this node published the frame | + +Both are needed. The device timestamp is the capture instant and is the value two +nodes reading the same frame agree on; the host timestamp is the one the rest of the +dataflow and the session recorder share. On a Jetson the wall clock can step when NTP +synchronises after boot, so the device timestamp is recorded alongside the host one +rather than in place of it. + +The node calls `sl::setTimestampClock(SYSTEM_CLOCK)` before opening the camera, which +is what makes the device timestamp a Unix-epoch value rather than an SDK-default clock. + +Every output for a frame also carries the same `frame_id`, an integer counter starting +at 0. diff --git a/src/main.cc b/src/main.cc index 8bb8600..edb3ea6 100644 --- a/src/main.cc +++ b/src/main.cc @@ -38,6 +38,32 @@ rust::Vec make_vector_int(std::initializer_list values) { return out; } +void set_intrinsics(Metadata& metadata, const zed::Calibration& calib) { + metadata.set_float("fx", calib.fx); + metadata.set_float("fy", calib.fy); + metadata.set_float("cx", calib.cx); + metadata.set_float("cy", calib.cy); + metadata.set_list_float("distortion", make_distortion(calib.distortion)); +} + +// Both clocks are needed. The device timestamp is the instant the frame was +// captured and is the only value two nodes reading the same frame can agree on. +// The host timestamp is what the rest of the dataflow and the session recorder +// share, and on a Jetson it can step when NTP synchronises after boot — which is +// exactly why the device timestamp is recorded alongside it rather than instead +// of it. +void set_frame_stamps(Metadata& metadata, int64_t frame_id, int64_t device_ns, int64_t host_ns) { + metadata.set_int("frame_id", frame_id); + metadata.set_timestamp("device_timestamp_ns", device_ns); + metadata.set_timestamp("host_timestamp_ns", host_ns); +} + +int64_t host_timestamp_ns() { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} + } // namespace int main() { @@ -80,11 +106,8 @@ int main() { int height = image_mat.getHeight(); int64_t current_frame_id = frame_id++; - int64_t timestamp_ns = static_cast( - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch() - ).count() - ); + int64_t device_ns = camera.device_timestamp_ns(); + int64_t host_ns = host_timestamp_ns(); // --- image_bgr --- cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr()); @@ -100,13 +123,8 @@ int main() { image_metadata->set_string("encoding", "bgr8"); image_metadata->set_string("layout", "HWC"); image_metadata->set_list_int("shape", make_shape(height, width, 3)); - image_metadata->set_float("fx", calib.fx); - image_metadata->set_float("fy", calib.fy); - image_metadata->set_float("cx", calib.cx); - image_metadata->set_float("cy", calib.cy); - image_metadata->set_list_float("distortion", make_distortion(calib.distortion)); - image_metadata->set_int("frame_id", current_frame_id); - image_metadata->set_int("timestamp_ns", timestamp_ns); + set_intrinsics(*image_metadata, calib); + set_frame_stamps(*image_metadata, current_frame_id, device_ns, host_ns); auto image_result = send_output_with_metadata( dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata)); @@ -125,11 +143,8 @@ int main() { pc_metadata->set_string("dtype", "float32"); pc_metadata->set_string("channels", "XYZRGBA"); pc_metadata->set_string("units", "mm"); - pc_metadata->set_float("fx", calib.fx); - pc_metadata->set_float("fy", calib.fy); - pc_metadata->set_float("cx", calib.cx); - pc_metadata->set_float("cy", calib.cy); - pc_metadata->set_list_float("distortion", make_distortion(calib.distortion)); + set_intrinsics(*pc_metadata, calib); + set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns); if (pc_mode == PcMode::HALF) { int hw = pc_width / 2, hh = pc_height / 2; @@ -171,6 +186,7 @@ int main() { cam_metadata->set_string("layout", "C"); cam_metadata->set_list_int("shape", make_vector_int({4})); cam_metadata->set_list_float("distortion", make_distortion(calib.distortion)); + set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns); auto cam_result = send_output_with_metadata( dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata)); diff --git a/src/zed/camera.cc b/src/zed/camera.cc index ecaaa9b..c89b502 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -71,6 +71,11 @@ Config Config::from_env() { } Camera::Camera(const Config& config) : config_(config) { + // Process-wide, and must be set before any camera is opened. Without it the + // epoch of the timestamps published alongside every frame depends on the SDK + // default rather than on anything this node states. + sl::setTimestampClock(sl::TIMESTAMP_CLOCK::SYSTEM_CLOCK); + init_params_.camera_resolution = parse_resolution(config_.resolution); init_params_.camera_fps = config_.fps; init_params_.depth_mode = parse_depth_mode(config_.depth_mode); @@ -135,6 +140,11 @@ Calibration Camera::calibration() const { return result; } +int64_t Camera::device_timestamp_ns() { + return static_cast( + zed_.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds()); +} + sl::Mat& Camera::image() { return image_mat_; } diff --git a/src/zed/camera.h b/src/zed/camera.h index 2b2f2a4..9223562 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -35,6 +36,7 @@ public: void warmup(); bool grab(); Calibration calibration() const; + int64_t device_timestamp_ns(); sl::Mat& image(); sl::Mat& point_cloud(); sl::Camera& handle();