From e4291e16219c87803122f3460f18a193b8864823 Mon Sep 17 00:00:00 2001 From: Xiao Date: Sun, 3 May 2026 22:27:43 +0100 Subject: [PATCH 1/2] refactor(sync): simplify sync state storage --- CameraFrameSync.hpp | 407 +++++++++++++++++++++++----- README.md | 12 +- camera_frame_sync_impl.hpp | 146 ++++++++-- camera_frame_sync_state_machine.hpp | 181 +++++++------ tests/sequence_test.cpp | 141 +++++----- 5 files changed, 626 insertions(+), 261 deletions(-) diff --git a/CameraFrameSync.hpp b/CameraFrameSync.hpp index 69925fb..ba2a973 100644 --- a/CameraFrameSync.hpp +++ b/CameraFrameSync.hpp @@ -60,22 +60,30 @@ template class CameraFrameSync { public: - using Self = CameraFrameSync; - using Base = CameraBase; - using CameraInfo = typename Base::CameraInfo; - using ImageFrame = typename Base::ImageFrame; - using ImuStamped = typename Base::ImuStamped; - using ImuVector = std::array; - using QuatSample = std::array; - using RawImuVector = Eigen::Matrix; - using RawQuatSample = LibXR::Quaternion; - using ImageTopic = LibXR::LinuxSharedTopic; - using ImageData = typename ImageTopic::Data; - using ImageCommitCallback = typename Base::ImageCommitCallback; + using Self = CameraFrameSync; ///< 当前模板实例类型。 + using Base = CameraBase; ///< 上游相机基类类型。 + using CameraInfo = typename Base::CameraInfo; ///< 相机静态标定信息类型。 + using ImageFrame = typename Base::ImageFrame; ///< 共享图像帧类型。 + using ImuStamped = typename Base::ImuStamped; ///< 同步后 IMU 输出类型。 + using ImuVector = std::array; ///< Host 侧三轴平铺 ABI。 + using QuatSample = std::array; ///< Host 侧 wxyz 四元数 ABI。 + using RawImuVector = Eigen::Matrix; ///< MCU 侧三轴 topic ABI。 + using RawQuatSample = LibXR::Quaternion; ///< MCU 侧四元数 topic ABI。 + using ImageTopic = LibXR::LinuxSharedTopic; ///< 图像共享 topic 类型。 + using ImageData = typename ImageTopic::Data; ///< 图像槽位租约类型。 + using ImageCommitCallback = + typename Base::ImageCommitCallback; ///< CameraBase 图像提交回调类型。 using SyncedFrame = CameraFrameSyncSyncedFrame; using Subscriber = CameraFrameSyncSubscriber; + /** + * @brief 当前模块实例使用的相机静态信息。 + */ static inline constexpr CameraInfo camera_info = CameraInfoV; + + /** + * @brief 共享图像 topic 的默认槽位配置。 + */ static constexpr LibXR::LinuxSharedTopicConfig image_topic_config{ .slot_num = 8, .subscriber_num = 8, @@ -106,7 +114,6 @@ class CameraFrameSync "camera_sync_command"; ///< CameraSync 命令 topic。 std::string_view sync_result_topic_name = "camera_sync_result"; ///< CameraSync 回执 topic。 - std::string_view raw_imu_topic_prefix = {}; ///< 原始 IMU topic 前缀，空值沿用相机名。 uint32_t sync_probe_div = 3; ///< MCU 临时分频系数。 uint32_t sync_active_level = 1; ///< 同步触发输出有效电平。 }; @@ -164,17 +171,46 @@ class CameraFrameSync template using SampleHistory = CameraFrameSyncCore::SampleHistory; + /** + * @brief 状态机侧使用的定长 FIFO，满时丢弃最旧样本。 + * + * 回调入口仍然直接写 LibXR::LockFreeQueue；这个包装只用于图像提交路径内的 + * 待处理队列，保证消费不及时不会让后续同步永远卡在旧数据上。 + */ template class DropOldestQueue { public: + /** + * @brief 创建固定容量队列。 + */ explicit DropOldestQueue(size_t capacity) : queue_(capacity) {} + /** + * @brief 队列当前是否为空。 + */ bool Empty() const { return queue_.Size() == 0; } + + /** + * @brief 读取最旧元素但不出队。 + */ bool Front(T& out) { return queue_.Peek(out) == LibXR::ErrorCode::OK; } + + /** + * @brief 丢弃最旧元素。 + */ void PopFront() { queue_.Pop(); } + + /** + * @brief 清空队列。 + */ void Clear() { queue_.Reset(); } + /** + * @brief 追加元素；容量不足时先弹出最旧元素。 + * + * @return true 表示本次追加丢弃了旧样本。 + */ bool PushBackDropOldest(const T& value) { if (queue_.Push(value) == LibXR::ErrorCode::OK) @@ -207,9 +243,7 @@ class CameraFrameSync host_domain_name(runtime.host_topic_domain_name), sync_command_name(runtime.sync_command_topic_name), sync_result_name(runtime.sync_result_topic_name), - raw_imu_prefix(runtime.raw_imu_topic_prefix.empty() - ? camera.NameView() - : runtime.raw_imu_topic_prefix), + raw_imu_prefix(camera.NameView()), gyro_name(raw_imu_prefix + "_gyro"), accl_name(raw_imu_prefix + "_accl"), quat_name(raw_imu_prefix + "_quat"), @@ -269,44 +303,46 @@ class CameraFrameSync LibXR::Topic::Callback sync_result; }; + /** + * @brief gyro 原始样本，timestamp 为 Topic envelope 中的传感器时间。 + */ struct GyroSample { uint64_t sensor_timestamp_us{}; ImuVector angular_velocity_xyz{}; }; + /** + * @brief accl 原始样本，timestamp 必须与同一 IMU 帧的 gyro 对齐。 + */ struct AcclSample { uint64_t sensor_timestamp_us{}; ImuVector linear_acceleration_xyz{}; }; + /** + * @brief quat 原始样本，内部顺序统一规整为 wxyz。 + */ struct QuatReading { uint64_t sensor_timestamp_us{}; QuatSample rotation_wxyz{}; }; - struct QueuedGyro - { - GyroSample sample{}; - }; - - struct QueuedAccl - { - AcclSample sample{}; - }; - - struct QueuedQuat - { - QuatReading sample{}; - }; - + /** + * @brief 已提交图像的传感器时间戳。 + * + * 大图像数据本身已经交给 LinuxSharedTopic，这里只保留同步需要的时间基线。 + */ struct ImageSample { uint64_t sensor_timestamp_us{}; }; + /** + * @brief 由同一 timestamp 的 gyro/accl/quat 组装出的完整 IMU 帧。 + */ struct AssembledImu { uint64_t sensor_timestamp_us{}; @@ -315,132 +351,361 @@ class CameraFrameSync ImuVector linear_acceleration_xyz{}; }; - struct PendingImage + /** + * @brief 已锁定同步基准、但还在等待 offset 后最终 IMU 的挂起匹配。 + */ + struct PendingMatch + { + bool valid{false}; + uint64_t imu_timestamp_us{0}; + uint64_t period_us{0}; + }; + + /** + * @brief 状态机当前持有的一帧图像时间戳。 + * + * 真实图像已经发布到共享槽位；这里不能换下一张图像重试，否则会破坏 + * 图像时间戳、同步基准 IMU 和最终 offset IMU 的对应关系。 + */ + struct PendingFrame { bool valid{false}; - ImageSample sample{}; - bool cadence_observed{false}; - bool sync_candidate_valid{false}; - uint64_t sync_candidate_sensor_timestamp_us{0}; - uint64_t sync_candidate_period_us{0}; + ImageSample image{}; + bool cadence_consumed{false}; + PendingMatch match{}; }; + /** + * @brief 本帧图像锁定的同步基准 IMU。 + */ struct SyncMatch { - const AssembledImu* sync_imu{nullptr}; - uint64_t sync_period_us{0}; + const AssembledImu* imu{nullptr}; + uint64_t period_us{0}; + }; + + /** + * @brief 已观察到的正常发布周期。 + * + * 这些周期只用于识别探针 gap 和在 IMU 时间轴上递推，不用于比较相机与 + * IMU 时间戳的绝对值。 + */ + struct ObservedPeriods + { + uint64_t image_us{0}; + uint64_t imu_us{0}; }; - struct SyncRelation + /** + * @brief RAW_PROBE 锁定后的 IMU 时间轴关系。 + */ + struct LockedSync { - uint64_t image_period_us{0}; - uint64_t imu_period_us{0}; - uint64_t sync_period_us{0}; - uint64_t last_sync_imu_timestamp_us{0}; + uint64_t period_us{0}; + uint64_t last_imu_timestamp_us{0}; }; + /** + * @brief 单帧图像在状态机里的处理结果。 + */ enum class ImageDecision : uint8_t { - WAIT = 0, - DONE = 1, - RESET = 2, + WAIT = 0, ///< 保留当前帧，等待未来 IMU 或 CameraSync 回执。 + DONE = 1, ///< 当前帧已经完成处理，可以切到下一帧。 + RESET = 2, ///< 当前帧打破同步关系，丢帧后重新观察。 }; - static constexpr size_t imu_ingress_length = 128; - static constexpr size_t pending_limit = 128; - static constexpr size_t image_event_limit = 32; - static constexpr size_t history_limit = 128; - static constexpr uint32_t cadence_stable_gaps = 2; - static constexpr uint64_t imu_cadence_tolerance_us = 300ULL; - static constexpr uint64_t image_cadence_tolerance_us = 1500ULL; + static constexpr size_t imu_ingress_length = 128; ///< topic 回调入口队列长度。 + static constexpr size_t pending_limit = 128; ///< 状态机待处理队列长度。 + static constexpr size_t image_event_limit = 32; ///< 图像时间戳待处理队列长度。 + static constexpr size_t history_limit = 128; ///< 可供 offset 查找的 IMU 历史长度。 + static constexpr uint32_t cadence_stable_gaps = 2; ///< 判定周期稳定所需连续 gap 数。 + static constexpr uint64_t imu_cadence_tolerance_us = 300ULL; ///< IMU 周期容差。 + static constexpr uint64_t image_cadence_tolerance_us = 1500ULL; ///< 图像周期容差。 + /** + * @brief 返回同步模式的日志名称。 + */ static const char* SyncModeName(SyncMode mode); + + /** + * @brief 返回 RAW_PROBE 状态机状态的日志名称。 + */ static const char* StateName(SyncState state); + + /** + * @brief 将 MCU 侧 IMU 向量 payload 规整为 Host 侧平铺数组。 + */ static ImuVector ToImuVector(const RawImuVector& data); + + /** + * @brief 将 MCU 侧四元数 payload 规整为 Host 侧 wxyz 顺序。 + */ static QuatSample ToQuatSample(const RawQuatSample& data); + /** + * @brief 启动时从共享图像 topic 租用第一块可写图像槽位。 + */ bool AcquireInitialWritableImage(); + + /** + * @brief CameraBase 图像提交回调适配层。 + */ static void CommitImageAdapter(bool, Self* self, ImageFrame*& next_image); + + /** + * @brief 发布当前图像槽位，并把下一块可写槽位交还给 CameraBase。 + */ ImageFrame* CommitImageAndLeaseNext(); + /** + * @brief gyro topic 回调；只做规整和入队，不推进同步状态机。 + */ static void OnGyroStatic(bool, Self* self, LibXR::MicrosecondTimestamp timestamp, const RawImuVector& data); + + /** + * @brief accl topic 回调；只做规整和入队，不推进同步状态机。 + */ static void OnAcclStatic(bool, Self* self, LibXR::MicrosecondTimestamp timestamp, const RawImuVector& data); + + /** + * @brief quat topic 回调；只做规整和入队，不推进同步状态机。 + */ static void OnQuatStatic(bool, Self* self, LibXR::MicrosecondTimestamp timestamp, const RawQuatSample& data); + + /** + * @brief CameraSync 回执回调；只记录当前 active probe 的一次命中。 + */ static void OnSyncResultStatic(bool, Self* self, LibXR::MicrosecondTimestamp timestamp, const CameraSync::SyncEvent& event); + /** + * @brief 图像成功发布后的同步处理入口。 + */ void ProcessCommittedImage(uint64_t image_timestamp_us); + + /** + * @brief 没有新图像可发布时，也推进已到达的 IMU / 回执数据。 + */ void ProcessSyncWorkWithoutImage(); + + /** + * @brief 将回调入口队列的数据搬到状态机私有 pending 队列。 + */ void CollectIncomingTopics(); + + /** + * @brief 尽可能把 pending gyro/accl/quat 组装为完整 IMU 历史。 + */ void AssembleImuHistory(); + + /** + * @brief 以 gyro timestamp 为主键尝试组装一帧完整 IMU。 + */ bool TryAssembleOneImu(); + + /** + * @brief 更新 IMU 发布周期观察，并在周期破坏时触发重同步。 + */ void ObserveImuCadence(uint64_t sensor_timestamp_us); + /** + * @brief 按图像时间顺序推进同步状态机。 + */ void ProcessImageEvents(); - ImageDecision ProcessLatestImage(PendingImage& image); - ImageDecision ProcessRawProbeImage(PendingImage& image); + + /** + * @brief LATEST_IMU 模式下处理一帧图像。 + */ + ImageDecision ProcessLatestImage(PendingFrame& frame); + + /** + * @brief RAW_PROBE 模式下处理一帧图像。 + */ + ImageDecision ProcessRawProbeImage(PendingFrame& frame); + + /** + * @brief 观察正常图像发布周期；probe gap 不应调用此路径。 + */ CameraFrameSyncCore::CadenceUpdate ObserveNormalImageCadence(uint64_t image_ts); + + /** + * @brief 周期稳定后向 MCU 下发一次 CameraSync 探针命令。 + */ void MaybeStartProbe(); - ImageDecision TryProbeImage(PendingImage& image); - ImageDecision TryLatestImuMatch(PendingImage& image); - ImageDecision TrySyncedImage(PendingImage& image); - ImageDecision ResumePendingMatch(PendingImage& image); - ImageDecision PublishOrRememberMatch(PendingImage& image, + + /** + * @brief 处理探针图像，等待对应 CameraSync 回执和 IMU 历史到达。 + */ + ImageDecision TryProbeImage(PendingFrame& frame); + + /** + * @brief LATEST_IMU 模式下用最新完整 IMU 建立本帧匹配。 + */ + ImageDecision TryLatestImuMatch(PendingFrame& frame); + + /** + * @brief RAW_PROBE 已锁定后，沿 IMU 时间轴递推下一帧同步基准。 + */ + ImageDecision TrySyncedImage(PendingFrame& frame); + + /** + * @brief 继续处理之前已锁定同步基准、但等待 offset IMU 的图像。 + */ + ImageDecision ResumePendingMatch(PendingFrame& frame); + + /** + * @brief 尝试发布匹配结果；若 offset 后 IMU 未到达则把匹配保存在 frame。 + */ + ImageDecision PublishOrRememberMatch(PendingFrame& frame, const SyncMatch& match); + + /** + * @brief 根据同步基准 IMU 和 offset 选择最终 IMU 并发布。 + */ ImageDecision PublishMatchedImage(const ImageSample& image, const SyncMatch& match); + + /** + * @brief 发布 timestamp 与图像一致的同步 IMU topic。 + */ void PublishSyncedImu(uint64_t image_timestamp_us, const AssembledImu& imu); + /** + * @brief 估计相邻图像在 IMU 时间轴上跨过的采样周期。 + */ uint64_t EstimatedSyncPeriodUs() const; + + /** + * @brief 判断图像间隔是否仍符合正常发布周期。 + */ bool IsNormalImageGap(uint64_t image_gap_us) const; + + /** + * @brief 判断图像间隔是否符合 CameraSync 探针制造的异常周期。 + */ bool IsProbeImageGap(uint64_t image_gap_us) const; + + /** + * @brief 判断 IMU 历史是否已经覆盖到目标时间戳。 + */ bool ImuHistoryReached(uint64_t target_timestamp_us) const; + + /** + * @brief 记录最近一帧已被状态机消费的图像时间戳。 + */ void RememberImage(uint64_t image_timestamp_us); + + /** + * @brief 清空图像周期观察，不清空 IMU 历史。 + */ void ResetImageObservation(); + + /** + * @brief 清空当前 CameraSync 探针邮箱和待处理回执。 + */ void ClearPendingProbe(); - void ClearPendingImage(); + + /** + * @brief 丢弃当前挂起图像。 + */ + void ClearPendingFrame(); + + /** + * @brief 解除 RAW_PROBE 锁定关系并回到周期观察状态。 + */ void ResetLock(const char* reason, const char* detail); + + /** + * @brief 清空全部运行时队列、历史和状态。 + */ void ResetRuntimeState(); + + /** + * @brief 入口队列溢出后执行保守恢复。 + */ void HandleOverflowRecovery(); private: + /** + * @brief topic 句柄与回调对象必须先于运行时状态存在。 + */ Topics topics_; TopicCallbacks callbacks_; + /** + * @brief 当前交给 CameraBase 写入的共享图像槽位。 + */ ImageData current_image_{}; - LibXR::LockFreeQueue gyro_ingress_{imu_ingress_length}; - LibXR::LockFreeQueue accl_ingress_{imu_ingress_length}; - LibXR::LockFreeQueue quat_ingress_{imu_ingress_length}; + /** + * @brief topic 回调写入的无锁入口队列。 + */ + LibXR::LockFreeQueue gyro_ingress_{imu_ingress_length}; + LibXR::LockFreeQueue accl_ingress_{imu_ingress_length}; + LibXR::LockFreeQueue quat_ingress_{imu_ingress_length}; + + /** + * @brief 任一路队列溢出都会置位，随后在图像提交路径统一重置。 + */ std::atomic overflowed_{false}; - DropOldestQueue pending_gyros_{pending_limit}; - DropOldestQueue pending_accls_{pending_limit}; - DropOldestQueue pending_quats_{pending_limit}; + /** + * @brief 状态机私有 pending 队列和短 IMU 历史。 + */ + DropOldestQueue pending_gyros_{pending_limit}; + DropOldestQueue pending_accls_{pending_limit}; + DropOldestQueue pending_quats_{pending_limit}; DropOldestQueue image_events_{image_event_limit}; SampleHistory imu_history_{}; + /** + * @brief 保护图像提交路径、参数切换和 pending 队列消费。 + */ LibXR::Mutex sync_state_mutex_{}; + + /** + * @brief 运行时配置副本。 + */ SyncMode sync_mode_{SyncMode::RAW_PROBE}; int32_t offset_us_{0}; uint32_t sync_probe_div_{3}; uint32_t sync_active_level_{1}; uint32_t next_sync_seq_{1}; - // 回执回调不入队，只接受当前 probe 的一次反馈。 + + /** + * @brief CameraSync 回执邮箱；回调只接受 active probe 的一次反馈。 + */ std::atomic active_probe_seq_{0}; std::atomic probe_ack_seq_{0}; std::atomic probe_ack_timestamp_us_{0}; + /** + * @brief RAW_PROBE 状态机当前锁定关系与周期观察结果。 + */ SyncState state_{SyncState::OBSERVING}; CameraFrameSyncCore::CadenceState image_cadence_{}; CameraFrameSyncCore::CadenceState imu_cadence_{}; - SyncRelation relation_{}; + ObservedPeriods periods_{}; + LockedSync locked_sync_{}; + + /** + * @brief 最近一帧被同步状态机接受的图像时间戳。 + */ bool last_image_valid_{false}; uint64_t last_image_timestamp_us_{0}; - PendingImage pending_image_{}; + + /** + * @brief 当前因等待 IMU / 回执而挂起的图像。 + */ + PendingFrame pending_frame_{}; + + /** + * @brief 当前探针命令的本地状态。 + */ uint32_t pending_probe_seq_{0}; bool pending_probe_ack_valid_{false}; uint64_t pending_probe_imu_timestamp_us_{0}; diff --git a/README.md b/README.md index 377faf0..889b506 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ `CameraFrameSync` 是相机帧和 IMU 的同步桥： - 从 `CameraBase` 拿可写图像槽位，把已提交图像发布到 `LinuxSharedTopic` -- 按配置的前缀订阅原始 `gyro / accl / quat` +- 按相机名订阅原始 `gyro / accl / quat` - 发布与图像 `timestamp_us` 相同的同步后 `ImuStamped` 模块不创建线程。原始 IMU 只在 Topic 回调里入队；`CameraSync` 回执只记录当前 probe 的命中结果。状态机只在图像提交时推进。 @@ -13,9 +13,9 @@ 输入： - 图像：`CameraBase::RegisterImageSink(...)` -- 原始陀螺：`_gyro`，payload 为 `Eigen::Matrix`，单位 rad/s -- 原始加速度：`_accl`，payload 为 `Eigen::Matrix`，单位 m/s^2 -- 原始姿态：`_quat`，payload 为 `LibXR::Quaternion` +- 原始陀螺：`_gyro`，payload 为 `Eigen::Matrix`，单位 rad/s +- 原始加速度：`_accl`，payload 为 `Eigen::Matrix`，单位 m/s^2 +- 原始姿态：`_quat`，payload 为 `LibXR::Quaternion` - 同步回执：`camera_sync_result`，payload 为 `CameraSync::SyncEvent` 输出： @@ -24,7 +24,7 @@ - 同步 IMU topic：`camera.ImuTopicName()` - 同步命令：`camera_sync_command`，payload 为 `CameraSync::SyncCommand` -实机和 Webots 都应通过 SharedTopic 转发 `camera_sync_command / camera_sync_result`。Host 侧默认使用 `host` topic domain。`raw_imu_topic_prefix` 为空时沿用 `camera.Name()`，实机如果下位机把云台 IMU 暴露为 `gimbal_gyro / gimbal_accl / gimbal_quat`，应显式配成 `gimbal`。 +实机和 Webots 都应通过 SharedTopic 转发 `camera_sync_command / camera_sync_result`。Host 侧默认使用 `host` topic domain。原始 IMU topic 由 `camera.Name()` 派生；实机如果下位机把云台 IMU 暴露为 `gimbal_gyro / gimbal_accl / gimbal_quat`，相机运行配置里的 `camera_name` 应设为 `gimbal`，图像 topic 和同步后 IMU topic 仍可单独配置。 ## 时间戳契约 @@ -127,7 +127,7 @@ Webots / 实机可显式配置同步 topic： ```yaml runtime: - expr: "CameraFrameSync::RuntimeParam{.mode = CameraFrameSync::SyncMode::RAW_PROBE, .offset_us = 0, .host_topic_domain_name = \"host\", .sync_command_topic_name = \"camera_sync_command\", .sync_result_topic_name = \"camera_sync_result\", .raw_imu_topic_prefix = \"gimbal\", .sync_probe_div = 3, .sync_active_level = 1}" + expr: "CameraFrameSync::RuntimeParam{.mode = CameraFrameSync::SyncMode::RAW_PROBE, .offset_us = 0, .host_topic_domain_name = \"host\", .sync_command_topic_name = \"camera_sync_command\", .sync_result_topic_name = \"camera_sync_result\", .sync_probe_div = 3, .sync_active_level = 1}" ``` ## 日志 diff --git a/camera_frame_sync_impl.hpp b/camera_frame_sync_impl.hpp index 8c07209..a29de9e 100644 --- a/camera_frame_sync_impl.hpp +++ b/camera_frame_sync_impl.hpp @@ -1,5 +1,8 @@ #pragma once +/** + * @brief 使用默认 RAW_PROBE 运行参数构造同步模块。 + */ template CameraFrameSync::CameraFrameSync( LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, @@ -8,6 +11,9 @@ CameraFrameSync::CameraFrameSync( { } +/** + * @brief 完成 topic、图像槽位、CameraBase sink 与原始 IMU 回调绑定。 + */ template CameraFrameSync::CameraFrameSync( LibXR::HardwareContainer&, LibXR::ApplicationManager&, @@ -23,6 +29,7 @@ CameraFrameSync::CameraFrameSync( ASSERT(sync_probe_div_ != 0); + // 共享图像 topic 是后续 Detector/Preview 的图像唯一来源，创建失败直接中止。 if (!topics_.image.Valid()) { XR_LOG_ERROR("CameraFrameSync: image topic creation failed err=%d", @@ -30,12 +37,14 @@ CameraFrameSync::CameraFrameSync( ASSERT(false); return; } + // CameraBase 写入的是当前租约；拿不到初始槽位时不能注册 sink。 if (!AcquireInitialWritableImage()) { XR_LOG_ERROR("CameraFrameSync: initial image slot acquisition failed"); ASSERT(false); return; } + // 注册成功后，CameraBase 每次提交图像都会从 CommitImageAdapter 换下一块槽位。 if (!camera.RegisterImageSink(current_image_.GetData(), ImageCommitCallback::Create(CommitImageAdapter, this))) { @@ -58,24 +67,36 @@ CameraFrameSync::CameraFrameSync( topics_.quat_name.c_str(), SyncModeName(sync_mode_)); } +/** + * @brief 返回共享图像 topic 名称。 + */ template const char* CameraFrameSync::ImageTopicName() const { return topics_.image_name.c_str(); } +/** + * @brief 返回同步后 IMU topic 名称。 + */ template const char* CameraFrameSync::ImuTopicName() const { return topics_.imu_name.c_str(); } +/** + * @brief 返回 Host topic domain 名称。 + */ template const char* CameraFrameSync::HostTopicDomainName() const { return topics_.host_domain_name.c_str(); } +/** + * @brief 读取当前同步模式。 + */ template typename CameraFrameSync::SyncMode CameraFrameSync::GetSyncMode() const @@ -83,6 +104,9 @@ CameraFrameSync::GetSyncMode() const return sync_mode_; } +/** + * @brief 设置最终 IMU 选择 offset。 + */ template void CameraFrameSync::SetOffsetUs(int32_t offset_us) { @@ -90,6 +114,9 @@ void CameraFrameSync::SetOffsetUs(int32_t offset_us) offset_us_ = offset_us; } +/** + * @brief 切换同步模式并清空旧模式留下的运行时关系。 + */ template void CameraFrameSync::SetSyncMode( typename CameraFrameSync::SyncMode mode) @@ -106,6 +133,9 @@ void CameraFrameSync::SetSyncMode( ResetRuntimeState(); } +/** + * @brief 将同步模式转成日志字符串。 + */ template const char* CameraFrameSync::SyncModeName( typename CameraFrameSync::SyncMode mode) @@ -120,6 +150,9 @@ const char* CameraFrameSync::SyncModeName( return "UNKNOWN"; } +/** + * @brief 将 RAW_PROBE 状态转成日志字符串。 + */ template const char* CameraFrameSync::StateName( typename CameraFrameSync::SyncState state) @@ -136,6 +169,9 @@ const char* CameraFrameSync::StateName( return "UNKNOWN"; } +/** + * @brief 把 MCU 侧三轴向量规整成 Host 侧平铺数组。 + */ template typename CameraFrameSync::ImuVector CameraFrameSync::ToImuVector( @@ -144,6 +180,9 @@ CameraFrameSync::ToImuVector( return {data.x(), data.y(), data.z()}; } +/** + * @brief 把 MCU 侧四元数规整成 Host 侧 wxyz 数组。 + */ template typename CameraFrameSync::QuatSample CameraFrameSync::ToQuatSample( @@ -152,6 +191,9 @@ CameraFrameSync::ToQuatSample( return {data.w(), data.x(), data.y(), data.z()}; } +/** + * @brief 从共享图像 topic 获取第一块 CameraBase 可写槽位。 + */ template bool CameraFrameSync::AcquireInitialWritableImage() { @@ -162,6 +204,9 @@ bool CameraFrameSync::AcquireInitialWritableImage() return current_image_.GetData() != nullptr; } +/** + * @brief CameraBase sink 回调适配层。 + */ template void CameraFrameSync::CommitImageAdapter( bool, CameraFrameSync* self, @@ -170,6 +215,9 @@ void CameraFrameSync::CommitImageAdapter( next_image = self->CommitImageAndLeaseNext(); } +/** + * @brief 发布当前槽位图像，并把下一块槽位交给 CameraBase 继续写。 + */ template typename CameraFrameSync::ImageFrame* CameraFrameSync::CommitImageAndLeaseNext() @@ -184,6 +232,7 @@ CameraFrameSync::CommitImageAndLeaseNext() if (topics_.image.CreateData(next_image) != LibXR::ErrorCode::OK || next_image.GetData() == nullptr) { + // 没有新槽位时继续让 CameraBase 写旧槽位，同时推进已到达的同步数据。 ProcessSyncWorkWithoutImage(); return committed_image; } @@ -195,59 +244,73 @@ CameraFrameSync::CommitImageAndLeaseNext() if (publish_ans == LibXR::ErrorCode::OK) { + // 只有共享图像发布成功，图像 timestamp 才进入同步状态机。 ProcessCommittedImage(image_timestamp_us); } else { XR_LOG_WARN("CameraFrameSync: image publish failed err=%d", static_cast(publish_ans)); + // 发布失败时不能制造不存在的图像事件，但仍要处理 IMU / 回执积压。 ProcessSyncWorkWithoutImage(); } return current_image_.GetData(); } +/** + * @brief gyro topic 回调：规整 payload 后写入无锁入口队列。 + */ template void CameraFrameSync::OnGyroStatic( bool, CameraFrameSync* self, LibXR::MicrosecondTimestamp timestamp, const typename CameraFrameSync::RawImuVector& data) { - const QueuedGyro sample{.sample = {.sensor_timestamp_us = - static_cast(timestamp), - .angular_velocity_xyz = ToImuVector(data)}}; + const GyroSample sample{.sensor_timestamp_us = static_cast(timestamp), + .angular_velocity_xyz = ToImuVector(data)}; if (self->gyro_ingress_.Push(sample) != LibXR::ErrorCode::OK) { + // 回调不能做重同步，只标记溢出，后续由图像提交路径统一恢复。 self->overflowed_.store(true, std::memory_order_relaxed); } } +/** + * @brief accl topic 回调：规整 payload 后写入无锁入口队列。 + */ template void CameraFrameSync::OnAcclStatic( bool, CameraFrameSync* self, LibXR::MicrosecondTimestamp timestamp, const typename CameraFrameSync::RawImuVector& data) { - const QueuedAccl sample{.sample = {.sensor_timestamp_us = - static_cast(timestamp), - .linear_acceleration_xyz = ToImuVector(data)}}; + const AcclSample sample{.sensor_timestamp_us = static_cast(timestamp), + .linear_acceleration_xyz = ToImuVector(data)}; if (self->accl_ingress_.Push(sample) != LibXR::ErrorCode::OK) { + // 回调不能做重同步，只标记溢出，后续由图像提交路径统一恢复。 self->overflowed_.store(true, std::memory_order_relaxed); } } +/** + * @brief quat topic 回调：规整 payload 后写入无锁入口队列。 + */ template void CameraFrameSync::OnQuatStatic( bool, CameraFrameSync* self, LibXR::MicrosecondTimestamp timestamp, const typename CameraFrameSync::RawQuatSample& data) { - const QueuedQuat sample{.sample = {.sensor_timestamp_us = - static_cast(timestamp), - .rotation_wxyz = ToQuatSample(data)}}; + const QuatReading sample{.sensor_timestamp_us = static_cast(timestamp), + .rotation_wxyz = ToQuatSample(data)}; if (self->quat_ingress_.Push(sample) != LibXR::ErrorCode::OK) { + // 回调不能做重同步，只标记溢出，后续由图像提交路径统一恢复。 self->overflowed_.store(true, std::memory_order_relaxed); } } +/** + * @brief CameraSync 回执回调：只记录当前 active probe 的匹配 timestamp。 + */ template void CameraFrameSync::OnSyncResultStatic( bool, CameraFrameSync* self, LibXR::MicrosecondTimestamp timestamp, @@ -256,10 +319,12 @@ void CameraFrameSync::OnSyncResultStatic( const uint32_t active_seq = self->active_probe_seq_.load(); if (active_seq == 0 || event.seq != active_seq) { + // 旧回执、串扰回执或没有 outstanding probe 时都不能影响状态机。 return; } if (self->probe_ack_seq_.load() == event.seq) { + // 同一个 probe 只接受第一次命中，避免重复回执改写同步锚点。 return; } @@ -267,6 +332,9 @@ void CameraFrameSync::OnSyncResultStatic( self->probe_ack_seq_.store(event.seq); } +/** + * @brief 图像发布成功后的主同步入口。 + */ template void CameraFrameSync::ProcessCommittedImage(uint64_t image_timestamp_us) { @@ -280,6 +348,9 @@ void CameraFrameSync::ProcessCommittedImage(uint64_t image_timestam ProcessImageEvents(); } +/** + * @brief 没有新图像事件时，仍推进 IMU 组装和挂起帧等待。 + */ template void CameraFrameSync::ProcessSyncWorkWithoutImage() { @@ -289,32 +360,38 @@ void CameraFrameSync::ProcessSyncWorkWithoutImage() ProcessImageEvents(); } +/** + * @brief 把 topic 回调入口队列搬运到状态机 pending 队列。 + */ template void CameraFrameSync::CollectIncomingTopics() { - QueuedGyro gyro{}; + GyroSample gyro{}; while (gyro_ingress_.Pop(gyro) == LibXR::ErrorCode::OK) { if (pending_gyros_.PushBackDropOldest(gyro)) { + // pending 队列丢旧样本后，同步关系不再可信，交给统一恢复处理。 overflowed_.store(true, std::memory_order_relaxed); } } - QueuedAccl accl{}; + AcclSample accl{}; while (accl_ingress_.Pop(accl) == LibXR::ErrorCode::OK) { if (pending_accls_.PushBackDropOldest(accl)) { + // pending 队列丢旧样本后，同步关系不再可信，交给统一恢复处理。 overflowed_.store(true, std::memory_order_relaxed); } } - QueuedQuat quat{}; + QuatReading quat{}; while (quat_ingress_.Pop(quat) == LibXR::ErrorCode::OK) { if (pending_quats_.PushBackDropOldest(quat)) { + // pending 队列丢旧样本后，同步关系不再可信，交给统一恢复处理。 overflowed_.store(true, std::memory_order_relaxed); } } @@ -322,6 +399,9 @@ void CameraFrameSync::CollectIncomingTopics() AssembleImuHistory(); } +/** + * @brief 连续组装所有当前可闭合的 IMU 三路样本。 + */ template void CameraFrameSync::AssembleImuHistory() { @@ -330,55 +410,60 @@ void CameraFrameSync::AssembleImuHistory() } } +/** + * @brief 以 gyro timestamp 为主键尝试组装一帧完整 IMU。 + */ template bool CameraFrameSync::TryAssembleOneImu() { - QueuedGyro queued_gyro{}; + GyroSample queued_gyro{}; if (!pending_gyros_.Front(queued_gyro)) { + // gyro 是主时间轴；没有 gyro 时不消费其他通道。 return false; } if (pending_accls_.Empty() || pending_quats_.Empty()) { + // 等待其他通道，不提前丢 gyro。 return false; } - const uint64_t gyro_ts = queued_gyro.sample.sensor_timestamp_us; - QueuedAccl queued_accl{}; + const uint64_t gyro_ts = queued_gyro.sensor_timestamp_us; + AcclSample queued_accl{}; + // 以 gyro 为主时间轴；早于 gyro 的 accl/quat 已不可能组成当前 IMU 帧。 while (pending_accls_.Front(queued_accl) && - queued_accl.sample.sensor_timestamp_us < gyro_ts) + queued_accl.sensor_timestamp_us < gyro_ts) { pending_accls_.PopFront(); } - QueuedQuat queued_quat{}; + QuatReading queued_quat{}; while (pending_quats_.Front(queued_quat) && - queued_quat.sample.sensor_timestamp_us < gyro_ts) + queued_quat.sensor_timestamp_us < gyro_ts) { pending_quats_.PopFront(); } if (!pending_accls_.Front(queued_accl) || !pending_quats_.Front(queued_quat)) { + // 其他通道暂时还没追上 gyro，保留当前 gyro 等下一轮。 return false; } - const uint64_t accl_ts = queued_accl.sample.sensor_timestamp_us; - const uint64_t quat_ts = queued_quat.sample.sensor_timestamp_us; + const uint64_t accl_ts = queued_accl.sensor_timestamp_us; + const uint64_t quat_ts = queued_quat.sensor_timestamp_us; if (accl_ts > gyro_ts || quat_ts > gyro_ts) { + // Topic timestamp 已是传感器时间；同一 IMU 帧三路时间戳必须精确一致。 pending_gyros_.PopFront(); ResetLock("raw-imu-missing-channel", "exact timestamp join failed"); return true; } - const auto& gyro = queued_gyro.sample; - const auto& accl = queued_accl.sample; - const auto& quat = queued_quat.sample; const AssembledImu imu{ - .sensor_timestamp_us = gyro.sensor_timestamp_us, - .rotation_wxyz = quat.rotation_wxyz, - .angular_velocity_xyz = gyro.angular_velocity_xyz, - .linear_acceleration_xyz = accl.linear_acceleration_xyz, + .sensor_timestamp_us = queued_gyro.sensor_timestamp_us, + .rotation_wxyz = queued_quat.rotation_wxyz, + .angular_velocity_xyz = queued_gyro.angular_velocity_xyz, + .linear_acceleration_xyz = queued_accl.linear_acceleration_xyz, }; pending_gyros_.PopFront(); @@ -388,6 +473,7 @@ bool CameraFrameSync::TryAssembleOneImu() if (!imu_history_.Empty() && imu.sensor_timestamp_us <= imu_history_.Back().sensor_timestamp_us) { + // 已组装 IMU 也必须保持单调，否则后续历史查找和周期观察都会失效。 ResetLock("raw-imu-out-of-order", "assembled imu timestamp"); return true; } @@ -397,6 +483,9 @@ bool CameraFrameSync::TryAssembleOneImu() return true; } +/** + * @brief 观察完整 IMU 帧的发布周期。 + */ template void CameraFrameSync::ObserveImuCadence(uint64_t sensor_timestamp_us) { @@ -405,11 +494,12 @@ void CameraFrameSync::ObserveImuCadence(uint64_t sensor_timestamp_u imu_cadence_tolerance_us); if (update == CameraFrameSyncCore::CadenceUpdate::BROKEN) { + // IMU 周期破坏会使 RAW_PROBE 的递推周期失效，必须回到观察状态。 ResetLock("imu-cadence-broken", "gyro/accl/quat"); return; } if (imu_cadence_.stable) { - relation_.imu_period_us = imu_cadence_.period_us; + periods_.imu_us = imu_cadence_.period_us; } } diff --git a/camera_frame_sync_state_machine.hpp b/camera_frame_sync_state_machine.hpp index f6e5f49..8054929 100644 --- a/camera_frame_sync_state_machine.hpp +++ b/camera_frame_sync_state_machine.hpp @@ -1,87 +1,94 @@ #pragma once +// 图像提交是唯一的状态机时钟。IMU 回调和 CameraSync 回执回调只搬运数据， +// 所有跨队列匹配、等待和重同步都在这里串行完成。 + template void CameraFrameSync::ProcessImageEvents() { - while (pending_image_.valid || !image_events_.Empty()) + while (pending_frame_.valid || !image_events_.Empty()) { - if (!pending_image_.valid) + if (!pending_frame_.valid) { - if (!image_events_.Front(pending_image_.sample)) + if (!image_events_.Front(pending_frame_.image)) { break; } - pending_image_.valid = true; + pending_frame_.valid = true; image_events_.PopFront(); } const ImageDecision decision = - sync_mode_ == SyncMode::LATEST_IMU ? ProcessLatestImage(pending_image_) - : ProcessRawProbeImage(pending_image_); + sync_mode_ == SyncMode::LATEST_IMU ? ProcessLatestImage(pending_frame_) + : ProcessRawProbeImage(pending_frame_); if (decision == ImageDecision::WAIT) { + // 当前图像已经成为同步基线，不能越过它消费下一帧。 break; } if (decision == ImageDecision::RESET) { - ClearPendingImage(); + ClearPendingFrame(); ResetLock("image-rejected", StateName(state_)); continue; } - ClearPendingImage(); + ClearPendingFrame(); } } template typename CameraFrameSync::ImageDecision CameraFrameSync::ProcessLatestImage( - typename CameraFrameSync::PendingImage& image) + typename CameraFrameSync::PendingFrame& frame) { if (last_image_valid_ && - image.sample.sensor_timestamp_us <= last_image_timestamp_us_) + frame.image.sensor_timestamp_us <= last_image_timestamp_us_) { ResetImageObservation(); return ImageDecision::DONE; } - const ImageDecision decision = image.sync_candidate_valid - ? ResumePendingMatch(image) - : TryLatestImuMatch(image); + const ImageDecision decision = frame.match.valid + ? ResumePendingMatch(frame) + : TryLatestImuMatch(frame); if (decision == ImageDecision::WAIT) { return decision; } - RememberImage(image.sample.sensor_timestamp_us); + RememberImage(frame.image.sensor_timestamp_us); return decision; } template typename CameraFrameSync::ImageDecision CameraFrameSync::ProcessRawProbeImage( - typename CameraFrameSync::PendingImage& image) + typename CameraFrameSync::PendingFrame& frame) { - if (image.sync_candidate_valid) + if (frame.match.valid) { - return ResumePendingMatch(image); + // 同步基准已经锁定，只是在等 offset 后的最终 IMU 到达。 + return ResumePendingMatch(frame); } if (!last_image_valid_) { - if (!image.cadence_observed) + // 第一帧只能建立图像时间基线；是否发 probe 取决于 IMU 周期是否也稳定。 + if (!frame.cadence_consumed) { - ObserveNormalImageCadence(image.sample.sensor_timestamp_us); - image.cadence_observed = true; + ObserveNormalImageCadence(frame.image.sensor_timestamp_us); + frame.cadence_consumed = true; } - RememberImage(image.sample.sensor_timestamp_us); + RememberImage(frame.image.sensor_timestamp_us); MaybeStartProbe(); return ImageDecision::DONE; } - const uint64_t image_ts = image.sample.sensor_timestamp_us; + const uint64_t image_ts = frame.image.sensor_timestamp_us; if (image_ts <= last_image_timestamp_us_) { + // 图像时间倒退说明相机侧时间轴不可连续使用，只保留新的观察起点。 ResetImageObservation(); ResetLock("image-out-of-order", "RAW_PROBE"); ObserveNormalImageCadence(image_ts); @@ -92,20 +99,20 @@ CameraFrameSync::ProcessRawProbeImage( const uint64_t image_gap_us = image_ts - last_image_timestamp_us_; if (state_ == SyncState::PROBE_SENT && IsProbeImageGap(image_gap_us)) { - // 主动探针 gap 不参与基线重估，只推进图像 cadence 的最后时间戳。 - if (!image.cadence_observed) + // 这次 gap 是同步探针制造的异常间隔，不能拿来重估正常图像周期。 + if (!frame.cadence_consumed) { image_cadence_.last_timestamp_us = image_ts; - image.cadence_observed = true; + frame.cadence_consumed = true; } - return TryProbeImage(image); + return TryProbeImage(frame); } auto cadence_update = CameraFrameSyncCore::CadenceUpdate::STABLE; - if (!image.cadence_observed) + if (!frame.cadence_consumed) { cadence_update = ObserveNormalImageCadence(image_ts); - image.cadence_observed = true; + frame.cadence_consumed = true; } if (cadence_update == CameraFrameSyncCore::CadenceUpdate::BROKEN) { @@ -124,6 +131,7 @@ CameraFrameSync::ProcessRawProbeImage( case SyncState::PROBE_SENT: if (IsNormalImageGap(image_gap_us)) { + // probe 尚未体现到图像间隔上，继续等真正被拉长的那一帧。 RememberImage(image_ts); return ImageDecision::DONE; } @@ -134,8 +142,7 @@ CameraFrameSync::ProcessRawProbeImage( { return ImageDecision::RESET; } - return image.sync_candidate_valid ? ResumePendingMatch(image) - : TrySyncedImage(image); + return frame.match.valid ? ResumePendingMatch(frame) : TrySyncedImage(frame); } return ImageDecision::RESET; @@ -149,7 +156,7 @@ CameraFrameSync::ObserveNormalImageCadence(uint64_t image_ts) image_cadence_, image_ts, cadence_stable_gaps, image_cadence_tolerance_us); if (image_cadence_.stable) { - relation_.image_period_us = image_cadence_.period_us; + periods_.image_us = image_cadence_.period_us; } return update; } @@ -181,8 +188,7 @@ void CameraFrameSync::MaybeStartProbe() pending_probe_seq_ = cmd.seq; pending_probe_ack_valid_ = false; pending_probe_imu_timestamp_us_ = 0; - relation_.sync_period_us = sync_period_us; - // 先启用当前 probe 再发布命令，避免同步回环里的早到回执丢失。 + // 回环很短时回执可能早于 Publish 返回，因此先打开当前 probe 邮箱。 probe_ack_timestamp_us_.store(0); probe_ack_seq_.store(0); active_probe_seq_.store(cmd.seq); @@ -193,19 +199,20 @@ void CameraFrameSync::MaybeStartProbe() "CameraFrameSync: sync command sent seq=%u div=%u active=%u image_period_us=%u imu_period_us=%u sync_period_us=%u", static_cast(cmd.seq), static_cast(cmd.div), static_cast(cmd.active_level), - static_cast(relation_.image_period_us), - static_cast(relation_.imu_period_us), + static_cast(periods_.image_us), + static_cast(periods_.imu_us), static_cast(sync_period_us)); } template typename CameraFrameSync::ImageDecision CameraFrameSync::TryProbeImage( - typename CameraFrameSync::PendingImage& image) + typename CameraFrameSync::PendingFrame& frame) { const uint64_t sync_period_us = EstimatedSyncPeriodUs(); if (!pending_probe_ack_valid_ && probe_ack_seq_.load() == pending_probe_seq_) { + // 回执 timestamp 来自 IMU 时间轴，是 RAW_PROBE 的唯一跨端同步锚点。 pending_probe_ack_valid_ = true; pending_probe_imu_timestamp_us_ = probe_ack_timestamp_us_.load(); } @@ -222,105 +229,102 @@ CameraFrameSync::TryProbeImage( const AssembledImu* sync_imu = CameraFrameSyncCore::FindBySensorTimestamp( imu_history_, ack_ts, - CameraFrameSyncCore::ImuTimestampToleranceUs(relation_.imu_period_us)); + CameraFrameSyncCore::ImuTimestampToleranceUs(periods_.imu_us)); if (sync_imu == nullptr) { return ImuHistoryReached(ack_ts + CameraFrameSyncCore::ImuTimestampToleranceUs( - relation_.imu_period_us)) + periods_.imu_us)) ? ImageDecision::RESET : ImageDecision::WAIT; } - return PublishOrRememberMatch(image, SyncMatch{.sync_imu = sync_imu, - .sync_period_us = sync_period_us}); + return PublishOrRememberMatch(frame, + SyncMatch{.imu = sync_imu, .period_us = sync_period_us}); } template typename CameraFrameSync::ImageDecision CameraFrameSync::TryLatestImuMatch( - typename CameraFrameSync::PendingImage& image) + typename CameraFrameSync::PendingFrame& frame) { if (imu_history_.Empty()) { return ImageDecision::WAIT; } - const uint64_t period = - relation_.imu_period_us != 0 ? relation_.imu_period_us : 1ULL; + const uint64_t period = periods_.imu_us != 0 ? periods_.imu_us : 1ULL; return PublishOrRememberMatch( - image, SyncMatch{.sync_imu = &imu_history_.Back(), .sync_period_us = period}); + frame, SyncMatch{.imu = &imu_history_.Back(), .period_us = period}); } template typename CameraFrameSync::ImageDecision CameraFrameSync::TrySyncedImage( - typename CameraFrameSync::PendingImage& image) + typename CameraFrameSync::PendingFrame& frame) { - if (relation_.last_sync_imu_timestamp_us == 0 || relation_.sync_period_us == 0) + if (locked_sync_.last_imu_timestamp_us == 0 || locked_sync_.period_us == 0) { return ImageDecision::RESET; } const uint64_t expected_sync_ts = - relation_.last_sync_imu_timestamp_us + relation_.sync_period_us; + locked_sync_.last_imu_timestamp_us + locked_sync_.period_us; if (!ImuHistoryReached(expected_sync_ts)) { + // 图像已到达，但对应的 IMU 时间点可能还没从 SharedTopic 上来。 return ImageDecision::WAIT; } const AssembledImu* sync_imu = CameraFrameSyncCore::FindBySensorTimestamp( imu_history_, expected_sync_ts, - CameraFrameSyncCore::ImuTimestampToleranceUs(relation_.imu_period_us)); + CameraFrameSyncCore::ImuTimestampToleranceUs(periods_.imu_us)); if (sync_imu == nullptr) { return ImageDecision::RESET; } return PublishOrRememberMatch( - image, - SyncMatch{.sync_imu = sync_imu, .sync_period_us = relation_.sync_period_us}); + frame, SyncMatch{.imu = sync_imu, .period_us = locked_sync_.period_us}); } template typename CameraFrameSync::ImageDecision CameraFrameSync::ResumePendingMatch( - typename CameraFrameSync::PendingImage& image) + typename CameraFrameSync::PendingFrame& frame) { - if (!image.sync_candidate_valid) + if (!frame.match.valid) { return ImageDecision::RESET; } const AssembledImu* sync_imu = CameraFrameSyncCore::FindBySensorTimestamp( - imu_history_, image.sync_candidate_sensor_timestamp_us, 0); + imu_history_, frame.match.imu_timestamp_us, 0); if (sync_imu == nullptr) { return ImageDecision::RESET; } return PublishOrRememberMatch( - image, SyncMatch{.sync_imu = sync_imu, - .sync_period_us = image.sync_candidate_period_us}); + frame, + SyncMatch{.imu = sync_imu, .period_us = frame.match.period_us}); } template typename CameraFrameSync::ImageDecision CameraFrameSync::PublishOrRememberMatch( - typename CameraFrameSync::PendingImage& image, + typename CameraFrameSync::PendingFrame& frame, const typename CameraFrameSync::SyncMatch& match) { - const ImageDecision decision = PublishMatchedImage(image.sample, match); + const ImageDecision decision = PublishMatchedImage(frame.image, match); if (decision == ImageDecision::WAIT) { - image.sync_candidate_valid = true; - image.sync_candidate_sensor_timestamp_us = match.sync_imu->sensor_timestamp_us; - image.sync_candidate_period_us = match.sync_period_us; + frame.match.valid = true; + frame.match.imu_timestamp_us = match.imu->sensor_timestamp_us; + frame.match.period_us = match.period_us; return decision; } - image.sync_candidate_valid = false; - image.sync_candidate_sensor_timestamp_us = 0; - image.sync_candidate_period_us = 0; + frame.match = {}; return decision; } @@ -330,22 +334,23 @@ CameraFrameSync::PublishMatchedImage( const typename CameraFrameSync::ImageSample& image, const typename CameraFrameSync::SyncMatch& match) { - if (match.sync_imu == nullptr || match.sync_period_us == 0) + if (match.imu == nullptr || match.period_us == 0) { return ImageDecision::RESET; } const int32_t offset_us = offset_us_; const uint64_t final_ts = - CameraFrameSyncCore::ApplyOffsetUs(match.sync_imu->sensor_timestamp_us, offset_us); + CameraFrameSyncCore::ApplyOffsetUs(match.imu->sensor_timestamp_us, offset_us); if (!ImuHistoryReached(final_ts)) { + // offset 只在 IMU 时间轴内生效；正 offset 会等待未来 IMU。 return ImageDecision::WAIT; } const AssembledImu* final_imu = CameraFrameSyncCore::FindBySensorTimestamp( imu_history_, final_ts, - CameraFrameSyncCore::ImuTimestampToleranceUs(relation_.imu_period_us)); + CameraFrameSyncCore::ImuTimestampToleranceUs(periods_.imu_us)); if (final_imu == nullptr) { return ImageDecision::RESET; @@ -356,8 +361,8 @@ CameraFrameSync::PublishMatchedImage( const SyncState old_state = state_; state_ = SyncState::SYNCED; - relation_.sync_period_us = match.sync_period_us; - relation_.last_sync_imu_timestamp_us = match.sync_imu->sensor_timestamp_us; + locked_sync_.period_us = match.period_us; + locked_sync_.last_imu_timestamp_us = match.imu->sensor_timestamp_us; ClearPendingProbe(); if (old_state != SyncState::SYNCED) @@ -365,9 +370,9 @@ CameraFrameSync::PublishMatchedImage( XR_LOG_PASS( "CameraFrameSync: state %s -> SYNCED mode=%s image_period_us=%u imu_period_us=%u sync_period_us=%u offset_us=%d", StateName(old_state), SyncModeName(sync_mode_), - static_cast(relation_.image_period_us), - static_cast(relation_.imu_period_us), - static_cast(relation_.sync_period_us), static_cast(offset_us)); + static_cast(periods_.image_us), + static_cast(periods_.imu_us), + static_cast(locked_sync_.period_us), static_cast(offset_us)); } return ImageDecision::DONE; } @@ -390,31 +395,31 @@ void CameraFrameSync::PublishSyncedImu( template uint64_t CameraFrameSync::EstimatedSyncPeriodUs() const { - if (relation_.image_period_us == 0 || relation_.imu_period_us == 0) + if (periods_.image_us == 0 || periods_.imu_us == 0) { return 0; } const uint32_t stride = CameraFrameSyncCore::EstimateStrideSamples( - relation_.image_period_us, relation_.imu_period_us); - return stride == 0 ? 0 : relation_.imu_period_us * static_cast(stride); + periods_.image_us, periods_.imu_us); + return stride == 0 ? 0 : periods_.imu_us * static_cast(stride); } template bool CameraFrameSync::IsNormalImageGap(uint64_t image_gap_us) const { - return relation_.image_period_us != 0 && - CameraFrameSyncCore::AbsDiffUs(image_gap_us, relation_.image_period_us) <= - CameraFrameSyncCore::ImageGapToleranceUs(relation_.image_period_us); + return periods_.image_us != 0 && + CameraFrameSyncCore::AbsDiffUs(image_gap_us, periods_.image_us) <= + CameraFrameSyncCore::ImageGapToleranceUs(periods_.image_us); } template bool CameraFrameSync::IsProbeImageGap(uint64_t image_gap_us) const { const uint64_t expected_gap = - CameraFrameSyncCore::ProbeImageGapUs(relation_.image_period_us, sync_probe_div_); + CameraFrameSyncCore::ProbeImageGapUs(periods_.image_us, sync_probe_div_); return expected_gap != 0 && CameraFrameSyncCore::AbsDiffUs(image_gap_us, expected_gap) <= - CameraFrameSyncCore::ImageGapToleranceUs(relation_.image_period_us); + CameraFrameSyncCore::ImageGapToleranceUs(periods_.image_us); } template @@ -436,7 +441,7 @@ template void CameraFrameSync::ResetImageObservation() { image_cadence_ = {}; - relation_.image_period_us = 0; + periods_.image_us = 0; last_image_valid_ = false; last_image_timestamp_us_ = 0; } @@ -453,9 +458,9 @@ void CameraFrameSync::ClearPendingProbe() } template -void CameraFrameSync::ClearPendingImage() +void CameraFrameSync::ClearPendingFrame() { - pending_image_ = {}; + pending_frame_ = {}; } template @@ -463,8 +468,7 @@ void CameraFrameSync::ResetLock(const char* reason, const char* det { const SyncState old_state = state_; state_ = SyncState::OBSERVING; - relation_.sync_period_us = 0; - relation_.last_sync_imu_timestamp_us = 0; + locked_sync_ = {}; ClearPendingProbe(); if (old_state != SyncState::OBSERVING) @@ -472,8 +476,8 @@ void CameraFrameSync::ResetLock(const char* reason, const char* det XR_LOG_WARN( "CameraFrameSync: state %s -> OBSERVING reason=%s detail=%s image_period_us=%u imu_period_us=%u", StateName(old_state), reason, detail, - static_cast(relation_.image_period_us), - static_cast(relation_.imu_period_us)); + static_cast(periods_.image_us), + static_cast(periods_.imu_us)); } } @@ -485,10 +489,11 @@ void CameraFrameSync::ResetRuntimeState() pending_quats_.Clear(); image_events_.Clear(); imu_history_.Clear(); - pending_image_ = {}; + pending_frame_ = {}; image_cadence_ = {}; imu_cadence_ = {}; - relation_ = {}; + periods_ = {}; + locked_sync_ = {}; last_image_valid_ = false; last_image_timestamp_us_ = 0; state_ = SyncState::OBSERVING; diff --git a/tests/sequence_test.cpp b/tests/sequence_test.cpp index f41f392..b3fa03b 100644 --- a/tests/sequence_test.cpp +++ b/tests/sequence_test.cpp @@ -94,8 +94,8 @@ class SequenceHarness SyncState State() const { return state_; } uint32_t ProbeSentCount() const { return probe_sent_count_; } uint32_t LastProbeSeq() const { return last_probe_seq_; } - uint64_t ImagePeriodUs() const { return relation_.image_period_us; } - uint64_t ImuPeriodUs() const { return relation_.imu_period_us; } + uint64_t ImagePeriodUs() const { return periods_.image_us; } + uint64_t ImuPeriodUs() const { return periods_.imu_us; } const std::vector& PublishedTags() const { return published_tags_; } const std::vector& SyncImuTimestamps() const { return sync_imu_timestamps_; } @@ -104,28 +104,37 @@ class SequenceHarness const std::vector& HistoryTimestamps() const { return history_timestamps_; } private: - struct PendingImage + struct PendingMatch + { + bool valid{false}; + uint64_t imu_timestamp_us{0}; + uint64_t period_us{0}; + }; + + struct PendingFrame { bool valid{false}; ImageEvent event{}; bool cadence_observed{false}; - bool sync_candidate_valid{false}; - uint64_t sync_candidate_timestamp_us{0}; - uint64_t sync_period_us{0}; + PendingMatch match{}; + }; + + struct ObservedPeriods + { + uint64_t image_us{0}; + uint64_t imu_us{0}; }; - struct Relation + struct LockedSync { - uint64_t image_period_us{0}; - uint64_t imu_period_us{0}; - uint64_t sync_period_us{0}; - uint64_t last_sync_imu_timestamp_us{0}; + uint64_t period_us{0}; + uint64_t last_imu_timestamp_us{0}; }; struct Match { - const Imu* sync_imu{nullptr}; - uint64_t sync_period_us{0}; + const Imu* imu{nullptr}; + uint64_t period_us{0}; }; enum class Decision : uint8_t @@ -149,10 +158,11 @@ class SequenceHarness images_.Clear(); history_.Clear(); history_timestamps_.clear(); - pending_image_ = {}; + pending_frame_ = {}; image_cadence_ = {}; imu_cadence_ = {}; - relation_ = {}; + periods_ = {}; + locked_sync_ = {}; state_ = SyncState::OBSERVING; last_image_valid_ = false; last_image_timestamp_us_ = 0; @@ -164,8 +174,7 @@ class SequenceHarness void ResetLock() { state_ = SyncState::OBSERVING; - relation_.sync_period_us = 0; - relation_.last_sync_imu_timestamp_us = 0; + locked_sync_ = {}; ClearPendingProbe(); } @@ -180,7 +189,7 @@ class SequenceHarness void ResetImageObservation() { image_cadence_ = {}; - relation_.image_period_us = 0; + periods_.image_us = 0; last_image_valid_ = false; last_image_timestamp_us_ = 0; } @@ -236,41 +245,41 @@ class SequenceHarness } if (imu_cadence_.stable) { - relation_.imu_period_us = imu_cadence_.period_us; + periods_.imu_us = imu_cadence_.period_us; } return true; } void ProcessImages() { - while (pending_image_.valid || !images_.Empty()) + while (pending_frame_.valid || !images_.Empty()) { - if (!pending_image_.valid) + if (!pending_frame_.valid) { - pending_image_.valid = true; - pending_image_.event = images_.Front(); + pending_frame_.valid = true; + pending_frame_.event = images_.Front(); images_.PopFront(); } const Decision decision = - mode_ == SyncMode::LATEST_IMU ? ProcessLatest(pending_image_) - : ProcessRawProbe(pending_image_); + mode_ == SyncMode::LATEST_IMU ? ProcessLatest(pending_frame_) + : ProcessRawProbe(pending_frame_); if (decision == Decision::WAIT) { break; } if (decision == Decision::RESET) { - reset_tags_.push_back(pending_image_.event.tag); - pending_image_ = {}; + reset_tags_.push_back(pending_frame_.event.tag); + pending_frame_ = {}; ResetLock(); continue; } - pending_image_ = {}; + pending_frame_ = {}; } } - Decision ProcessLatest(PendingImage& image) + Decision ProcessLatest(PendingFrame& image) { if (last_image_valid_ && image.event.sensor_timestamp_us <= last_image_timestamp_us_) { @@ -278,7 +287,7 @@ class SequenceHarness return Decision::DONE; } - const Decision decision = image.sync_candidate_valid + const Decision decision = image.match.valid ? Resume(image) : PublishOrRemember(image, LatestMatch()); if (decision != Decision::WAIT) @@ -288,9 +297,9 @@ class SequenceHarness return decision; } - Decision ProcessRawProbe(PendingImage& image) + Decision ProcessRawProbe(PendingFrame& image) { - if (image.sync_candidate_valid) + if (image.match.valid) { return Resume(image); } @@ -354,14 +363,14 @@ class SequenceHarness return Decision::RESET; } - CadenceUpdate ObserveImage(PendingImage& image) + CadenceUpdate ObserveImage(PendingFrame& image) { image.cadence_observed = true; const auto update = ObserveCadence(image_cadence_, image.event.sensor_timestamp_us, stable_gaps, image_tolerance_us); if (image_cadence_.stable) { - relation_.image_period_us = image_cadence_.period_us; + periods_.image_us = image_cadence_.period_us; } return update; } @@ -374,8 +383,7 @@ class SequenceHarness return; } - relation_.sync_period_us = EstimatedSyncPeriod(); - if (relation_.sync_period_us == 0) + if (EstimatedSyncPeriod() == 0) { return; } @@ -389,7 +397,7 @@ class SequenceHarness active_probe_seq_ = last_probe_seq_; } - Decision TryProbeImage(PendingImage& image) + Decision TryProbeImage(PendingFrame& image) { if (!probe_ack_valid_ && probe_ack_seq_ == last_probe_seq_) { @@ -405,7 +413,7 @@ class SequenceHarness } const Imu* sync_imu = FindBySensorTimestamp( - history_, probe_ack_timestamp_us_, ImuTimestampToleranceUs(relation_.imu_period_us)); + history_, probe_ack_timestamp_us_, ImuTimestampToleranceUs(periods_.imu_us)); if (sync_imu == nullptr) { return Decision::RESET; @@ -419,67 +427,64 @@ class SequenceHarness { return {}; } - return Match{&history_.Back(), relation_.imu_period_us != 0 ? relation_.imu_period_us : 1}; + return Match{&history_.Back(), periods_.imu_us != 0 ? periods_.imu_us : 1}; } - Decision TrySyncedImage(PendingImage& image) + Decision TrySyncedImage(PendingFrame& image) { - const uint64_t expected = relation_.last_sync_imu_timestamp_us + relation_.sync_period_us; + const uint64_t expected = locked_sync_.last_imu_timestamp_us + locked_sync_.period_us; if (!HistoryReached(expected)) { return Decision::WAIT; } const Imu* sync_imu = - FindBySensorTimestamp(history_, expected, - ImuTimestampToleranceUs(relation_.imu_period_us)); + FindBySensorTimestamp(history_, expected, ImuTimestampToleranceUs(periods_.imu_us)); if (sync_imu == nullptr) { return Decision::RESET; } - return PublishOrRemember(image, Match{sync_imu, relation_.sync_period_us}); + return PublishOrRemember(image, Match{sync_imu, locked_sync_.period_us}); } - Decision Resume(PendingImage& image) + Decision Resume(PendingFrame& image) { const Imu* sync_imu = - FindBySensorTimestamp(history_, image.sync_candidate_timestamp_us, 0); + FindBySensorTimestamp(history_, image.match.imu_timestamp_us, 0); if (sync_imu == nullptr) { return Decision::RESET; } - return PublishOrRemember(image, Match{sync_imu, image.sync_period_us}); + return PublishOrRemember(image, Match{sync_imu, image.match.period_us}); } - Decision PublishOrRemember(PendingImage& image, Match match) + Decision PublishOrRemember(PendingFrame& image, Match match) { - if (match.sync_imu == nullptr || match.sync_period_us == 0) + if (match.imu == nullptr || match.period_us == 0) { return Decision::WAIT; } - const uint64_t final_ts = - ApplyOffsetUs(match.sync_imu->sensor_timestamp_us, offset_us_); + const uint64_t final_ts = ApplyOffsetUs(match.imu->sensor_timestamp_us, offset_us_); if (!HistoryReached(final_ts)) { - image.sync_candidate_valid = true; - image.sync_candidate_timestamp_us = match.sync_imu->sensor_timestamp_us; - image.sync_period_us = match.sync_period_us; + image.match.valid = true; + image.match.imu_timestamp_us = match.imu->sensor_timestamp_us; + image.match.period_us = match.period_us; return Decision::WAIT; } const Imu* final_imu = - FindBySensorTimestamp(history_, final_ts, - ImuTimestampToleranceUs(relation_.imu_period_us)); + FindBySensorTimestamp(history_, final_ts, ImuTimestampToleranceUs(periods_.imu_us)); if (final_imu == nullptr) { return Decision::RESET; } published_tags_.push_back(image.event.tag); - sync_imu_timestamps_.push_back(match.sync_imu->sensor_timestamp_us); + sync_imu_timestamps_.push_back(match.imu->sensor_timestamp_us); final_imu_timestamps_.push_back(final_imu->sensor_timestamp_us); - relation_.sync_period_us = match.sync_period_us; - relation_.last_sync_imu_timestamp_us = match.sync_imu->sensor_timestamp_us; + locked_sync_.period_us = match.period_us; + locked_sync_.last_imu_timestamp_us = match.imu->sensor_timestamp_us; state_ = SyncState::SYNCED; ClearPendingProbe(); RememberImage(image.event.sensor_timestamp_us); @@ -489,8 +494,8 @@ class SequenceHarness uint64_t EstimatedSyncPeriod() const { const uint32_t stride = - EstimateStrideSamples(relation_.image_period_us, relation_.imu_period_us); - return stride == 0 ? 0 : relation_.imu_period_us * static_cast(stride); + EstimateStrideSamples(periods_.image_us, periods_.imu_us); + return stride == 0 ? 0 : periods_.imu_us * static_cast(stride); } bool HistoryReached(uint64_t timestamp_us) const @@ -500,16 +505,15 @@ class SequenceHarness bool IsNormalGap(uint64_t gap_us) const { - return relation_.image_period_us != 0 && - AbsDiffUs(gap_us, relation_.image_period_us) <= - ImageGapToleranceUs(relation_.image_period_us); + return periods_.image_us != 0 && AbsDiffUs(gap_us, periods_.image_us) <= + ImageGapToleranceUs(periods_.image_us); } bool IsProbeGap(uint64_t gap_us) const { - const uint64_t expected = ProbeImageGapUs(relation_.image_period_us, probe_div); + const uint64_t expected = ProbeImageGapUs(periods_.image_us, probe_div); return expected != 0 && AbsDiffUs(gap_us, expected) <= - ImageGapToleranceUs(relation_.image_period_us); + ImageGapToleranceUs(periods_.image_us); } void RememberImage(uint64_t timestamp_us) @@ -530,8 +534,9 @@ class SequenceHarness SyncState state_{SyncState::OBSERVING}; CadenceState image_cadence_{}; CadenceState imu_cadence_{}; - Relation relation_{}; - PendingImage pending_image_{}; + ObservedPeriods periods_{}; + LockedSync locked_sync_{}; + PendingFrame pending_frame_{}; bool last_image_valid_{false}; uint64_t last_image_timestamp_us_{0}; From 34fb587f55fd3401c17260e1ed437e710dc86a09 Mon Sep 17 00:00:00 2001 From: Xiao Date: Mon, 4 May 2026 01:11:07 +0100 Subject: [PATCH 2/2] fix(sync): avoid startup image publish backpressure --- README.md | 3 +- camera_frame_sync_subscriber.hpp | 137 +++++++++++++++++++++---------- 2 files changed, 94 insertions(+), 46 deletions(-) diff --git a/README.md b/README.md index 889b506..144b0e6 100644 --- a/README.md +++ b/README.md @@ -101,8 +101,9 @@ IMU 组装以 gyro 为主轴： 后续模块通过 `CameraFrameSync::Subscriber` 消费同步结果： - `Wait(out, timeout_ms)` 是阻塞接口，`UINT32_MAX` 表示无限等待 +- `Wait()` 先等待同步后 IMU，再取同 timestamp 的共享图像；RAW_PROBE 启动尚未锁定时，消费者不会持有未同步图像槽位 - 成功返回时，`out.image` 持有共享图像槽位，`out.imu` 是 timestamp 完全相同的同步 IMU -- 如果某张图像已经被 IMU 时间轴越过，Subscriber 会释放这张图并继续等下一张 +- 图像订阅使用丢旧语义；如果某张图像已经被 IMU 时间轴越过，Subscriber 会释放这张图并继续等下一张 - 回调里不做 Detector/Tracker 工作，重计算应放在消费者线程里 ## 配置 diff --git a/camera_frame_sync_subscriber.hpp b/camera_frame_sync_subscriber.hpp index 4614550..47e9fcd 100644 --- a/camera_frame_sync_subscriber.hpp +++ b/camera_frame_sync_subscriber.hpp @@ -7,6 +7,7 @@ #include #include "libxr.hpp" +#include "linux_shared_topic.hpp" /** * @brief 一组已同步的图像共享槽位和 IMU 数据。 @@ -41,6 +42,15 @@ class CameraFrameSyncSubscriber using SyncedFrame = CameraFrameSyncSyncedFrame; using ImageData = typename ImageTopicT::Data; + /** + * @brief 图像订阅使用丢旧语义，避免启动同步或消费滞后时反压相机。 + * + * 图像是同步基线但不是积压队列；如果当前图像等不到同 timestamp 的 IMU， + * 后续更近的图像应覆盖旧描述符，不能让共享图像 publish 返回 FULL。 + */ + static constexpr LibXR::LinuxSharedSubscriberMode image_subscriber_mode = + LibXR::LinuxSharedSubscriberMode::BROADCAST_DROP_OLD; + /** * @brief 从 CameraFrameSync 实例推导 topic 名称。 */ @@ -70,7 +80,7 @@ class CameraFrameSyncSubscriber imu_topic_name_(imu_topic_name), host_topic_domain_name_(host_topic_domain_name), host_topic_domain_(host_topic_domain_name_.c_str()), - image_sub_(image_topic_name_.c_str()), + image_sub_(image_topic_name_.c_str(), image_subscriber_mode), imu_sub_(LibXR::Topic(LibXR::Topic::WaitTopic( imu_topic_name_.c_str(), UINT32_MAX, &host_topic_domain_)), latest_imu_) @@ -89,8 +99,8 @@ class CameraFrameSyncSubscriber * @param timeout_ms 总超时时间，UINT32_MAX 表示无限等待。 * @return OK 表示成功；其他错误来自底层 topic 等待或超时。 * - * 如果先拿到的图像没有对应 IMU，会继续等待；如果 IMU 已经越过该图像时间戳， - * 说明该图像无法配对，当前图像会被释放并等待下一张。 + * 等待顺序是先同步 IMU、后图像。同步 IMU 是 CameraFrameSync 已经完成配对的 + * 事件；这样 RAW_PROBE 启动阶段还没有同步结果时，消费者不会拿住无效图像槽位。 */ LibXR::ErrorCode Wait(SyncedFrame& out, uint32_t timeout_ms) { @@ -98,35 +108,29 @@ class CameraFrameSyncSubscriber while (true) { - ImageData image_data; - const auto wait_ans = - image_sub_.Wait(image_data, RemainingMs(deadline_ms, timeout_ms)); - if (wait_ans != LibXR::ErrorCode::OK) + ImuStampedT imu{}; + const auto imu_ans = WaitForNextImu(deadline_ms, timeout_ms, imu); + if (imu_ans != LibXR::ErrorCode::OK) { - return wait_ans; - } - - const auto* image = image_data.GetData(); - if (image == nullptr) - { - image_data.Reset(); - continue; + return imu_ans; } - const auto imu_ans = - WaitForMatchingImu(static_cast(image->timestamp_us), - deadline_ms, timeout_ms, out.imu); - if (imu_ans == LibXR::ErrorCode::OK) + ImageData image; + const auto image_ans = + WaitForMatchingImage(static_cast(imu.timestamp_us), + deadline_ms, timeout_ms, image); + if (image_ans == LibXR::ErrorCode::OK) { - out.image = std::move(image_data); + out.imu = imu; + out.image = std::move(image); return LibXR::ErrorCode::OK; } - if (imu_ans == LibXR::ErrorCode::EMPTY) + if (image_ans == LibXR::ErrorCode::EMPTY) { - image_data.Reset(); + // 图像已经越过该 IMU timestamp，说明这组数据因丢旧策略被跳过。 continue; } - return imu_ans; + return image_ans; } } @@ -158,38 +162,79 @@ class CameraFrameSyncSubscriber } /** - * @brief 阻塞等待指定图像时间戳对应的同步 IMU。 + * @brief 等待下一条 timestamp 单调前进的同步 IMU。 */ - LibXR::ErrorCode WaitForMatchingImu(uint64_t image_timestamp_us, - uint64_t deadline_ms, - uint32_t timeout_ms, - ImuStampedT& matched_imu) + LibXR::ErrorCode WaitForNextImu(uint64_t deadline_ms, uint32_t timeout_ms, + ImuStampedT& imu) { while (true) { - if (latest_imu_valid_) + const auto wait_ans = + imu_sub_.Wait(RemainingMs(deadline_ms, timeout_ms)); + if (wait_ans != LibXR::ErrorCode::OK) { - const uint64_t imu_timestamp_us = - static_cast(latest_imu_.timestamp_us); - if (imu_timestamp_us == image_timestamp_us) - { - matched_imu = latest_imu_; - latest_imu_valid_ = false; - return LibXR::ErrorCode::OK; - } - if (imu_timestamp_us > image_timestamp_us) + return wait_ans; + } + + const uint64_t imu_timestamp_us = + static_cast(latest_imu_.timestamp_us); + if (last_imu_valid_ && imu_timestamp_us <= last_imu_timestamp_us_) + { + // SyncSubscriber 可能留下旧信号量计数；payload 是最新值，旧计数直接吃掉。 + continue; + } + + last_imu_valid_ = true; + last_imu_timestamp_us_ = imu_timestamp_us; + imu = latest_imu_; + return LibXR::ErrorCode::OK; + } + } + + /** + * @brief 从共享图像流中找到指定 timestamp 的图像。 + * + * 早于目标 IMU 的图像直接释放；晚于目标 IMU 的图像保留在 pending_image_，等待 + * 后续 IMU 追上，避免因为一次错过就丢掉可能可配对的最新图像。 + */ + LibXR::ErrorCode WaitForMatchingImage(uint64_t imu_timestamp_us, + uint64_t deadline_ms, + uint32_t timeout_ms, + ImageData& image) + { + while (true) + { + if (pending_image_.Empty()) + { + const auto wait_ans = image_sub_.Wait( + pending_image_, RemainingMs(deadline_ms, timeout_ms)); + if (wait_ans != LibXR::ErrorCode::OK) { - return LibXR::ErrorCode::EMPTY; + return wait_ans; } } - const auto wait_ans = - imu_sub_.Wait(RemainingMs(deadline_ms, timeout_ms)); - if (wait_ans != LibXR::ErrorCode::OK) + const auto* frame = pending_image_.GetData(); + if (frame == nullptr) { - return wait_ans; + pending_image_.Reset(); + continue; } - latest_imu_valid_ = true; + + const uint64_t image_timestamp_us = + static_cast(frame->timestamp_us); + if (image_timestamp_us < imu_timestamp_us) + { + pending_image_.Reset(); + continue; + } + if (image_timestamp_us == imu_timestamp_us) + { + image = std::move(pending_image_); + return LibXR::ErrorCode::OK; + } + + return LibXR::ErrorCode::EMPTY; } } @@ -201,5 +246,7 @@ class CameraFrameSyncSubscriber typename ImageTopicT::Subscriber image_sub_; ImuStampedT latest_imu_{}; LibXR::Topic::SyncSubscriber imu_sub_; - bool latest_imu_valid_{false}; + bool last_imu_valid_{false}; + uint64_t last_imu_timestamp_us_{0}; + ImageData pending_image_{}; };