Insight9 建图回环检测 - NUSNiuMu/insight-capture-dashboard GitHub Wiki

Insight9 建图回环检测

当前基线：2026-08-05，main@4538763。稀疏 mapper 自 v2.0.0 起随客户发布 运行；dense mapper 与 RViz 仍只用于内部验证。

目标与边界

Insight9 的设备 VIO 继续作为低延迟、连续的 insight9_odom 运动来源；稀疏 mapper 在其上维护关键帧位姿图。当相机返回较早访问区域时，历史三维地标 PnP 提供全局回环边，优化器把闭环误差沿历史关键帧分配，并按优化后的位姿重融合旧地标。

当前实现解决“只修正后续 T_map_odom，但已经写入的地图仍有局部形变”的问题。 它包含 SE(3) 位姿图与关键帧局部三维观测重融合，但尚不是把全部 2D 重投影误差、 相机参数和地标位置联合求解的完整 Bundle Adjustment。

坐标关系

insight9_map
  ├─ 关键帧位姿图（VIO 边 + PnP 全局回环边）
  │    └─ 历史 Path 与重融合后的稀疏地标
  └─ 当前 T_map_odom（图优化目标 + EKF 显示平滑）
       └─ insight9_odom（设备原始 VIO，不写回）
            └─ Insight9 左目 / 双目中心
  • 左目负责 SuperPoint 特征、PnP 和双目三角化。
  • Dashboard Pose、Path 和 TF 的物理锚点是 Insight9 双目中心。
  • T_map_odom 初始为单位阵，因此首次回环也进入正常 EKF 更新，不会把任意 坐标原点作为第一次观测直接跳过去。

回环检测流程

  1. Insight9 按平移 5 cm 或旋转 3° 选取建图关键帧。
  2. 每两个关键帧尝试一次回环。候选地图只包含至少 30 个关键帧以前首次建立的 已确认地标，避免把连续局部跟踪误判成闭环；历史地标再次被观察后仍可继续 用于连续三帧确认。
  3. 复用该关键帧左右目匹配时已经提取的左目 SuperPoint 描述子，不额外执行 一轮 GPU 特征提取。
  4. 当前描述子与历史三维地标描述子做余弦相似度匹配，要求双向最近邻、0.78 ratio test 和至少 0.70 相似度；历史地图少于 80 个特征时不检测。
  5. 至少 20 对 2D–3D 匹配后执行 PnP-RANSAC，并要求：
    • 至少 15 个内点；
    • 内点率至少 55%；
    • 中位重投影误差不超过 2.5 px；
    • 内点至少覆盖 4x4 图像网格中的 5 格。
  6. 候选 T_map_odom 必须连续三次一致，相邻候选差异不得超过 0.20 m / 10°， 才作为一次接受的回环观测。
  7. 接受的当前关键帧全局位姿形成从固定首帧到当前帧的回环边。优化器以相邻关键帧 VIO 相对位姿为连续约束，用 Cauchy 鲁棒核抑制与整张图不一致的错误边。
  8. 优化成功后，所有保留的关键帧双目局部观测在新位姿下重新体素确认并整体发布， 不只更新回环后的新地标。

错误回环比漏检更危险，因此默认阈值偏保守。状态消息会发布描述子匹配数、PnP 内点、拒绝原因、确认进度和累计接受次数，便于用真实数据调参。

轨迹如何修正

这里不使用未来帧，也不是离线前后向平滑；全部是因果更新：

  1. 三帧共识产生当前关键帧的绝对全局位姿，并加入位姿图回环边。
  2. 首个回环立即优化；之后至少间隔 5 秒，10 cm / 5° 以上差异或图达到上限时 立即求解。限流窗口内的 pending 边由后续关键帧自动补做。
  3. 优化结果更新历史关键帧并触发旧地标重融合；Path 按关键帧时间插值分段校正， 因而能显示闭环误差如何沿轨迹分配。
  4. 当前关键帧的图校正作为误差状态 EKF 观测。默认过程噪声为 0.02 m/√s、0.5°/√s， 观测噪声为 0.08 m、2.5°。
  5. 对外发布的最新 Pose/TF 不在观测到达当帧直接替换，而是随后续 VIO 帧按 0.75 秒 时间常数逐步逼近 EKF 目标，因此实时模型和 Pose 没有单帧跳变。

每个会话最多保留 600 个图节点，描述子和分数以 FP16 保存，重融合时恢复 FP32。 达到上限后冻结图结构，继续使用已经求得的分段校正和当前全局修正，防止长会话 内存无界增长。

状态与验收

/insight9_sparse_map/status 的主要回环字段：

  • loop_checked：当前关键帧是否执行了回环检查；
  • loop_historical_features：参与候选匹配的历史地标数；
  • loop_descriptor_matches、loop_inliers、loop_inlier_ratio；
  • loop_rejection：拒绝原因；
  • loop_confirmation_progress / loop_confirmation_required；
  • loop_accepted、loop_closures；
  • loop_ekf_innovation_translation_m / loop_ekf_innovation_rotation_deg。
  • pose_graph_keyframes、pose_graph_edges、pose_graph_loop_edges；
  • pose_graph_pending、pose_graph_optimizations、pose_graph_trigger；
  • pose_graph_initial_cost / pose_graph_final_cost、最大平移/旋转修正；
  • pose_graph_optimization_ms、map_rebuild_ms、pose_graph_observation_mb。

Dashboard 的 Mapping 状态栏显示累计 loops，确认过程中额外显示 loop check x/3。

2026-08-05 的容器内回归中，101 节点 10 cm 平移漂移收敛到 3.85 mm；同时加入 明显错误回环后末端仍在正确位置约 1.9 cm 内；600 节点求解耗时 1.80 秒。 170×150 个观测的地图重融合为 703.1 ms、13.43 MB。在线 ROS/API 首关键帧接线 正常。正式厘米级验收仍需拿相机走小闭环和长闭环，并用独立固定控制点测量地图 形变和 Insight3 重定位误差；这些合成残差不能替代真实精度结论。

Insight3 如何消费该地图并处理大漂移，见 Insight3 自适应重定位。