Insight3 自适应重定位 - NUSNiuMu/insight-capture-dashboard GitHub Wiki

Insight3 自适应重定位

当前基线：2026-08-05。该 localizer 自 v2.0.0 起随客户发布运行；Insight9 稀疏地图是唯一在线基准，旧 AprilTag 在线对齐已删除。

自 2026-08-27 起，UMI cube marker 作为同一 localizer 内的第二种低频绝对观测源， 与自然地图重定位共用 T_map_odom EKF。几何、配置和验证边界见 UMI Cube Marker 相对定位。

目标

Insight3 的重定位不再把所有修正都固定交给 EKF 缓慢收敛。通过三帧 PnP 共识 得到可靠的绝对位置后，根据新观测相对当前对外 T_map_odom 的差异选择：

  • 正常小幅漂移：继续由 EKF 平滑拉回；
  • 相机大幅漂移、飞走后重新找到地图：立即跳回正确全局位置。

大小修正分流不会降低描述子匹配、PnP-RANSAC 或三帧一致性要求；只有已经通过 原有几何和时间验证的观测才会进入该判断。

数据路径与负载边界

Dashboard 复用已有图像订阅，把 insight3_a / insight3_b 图像各以 2 Hz 中继给 localizer。localizer 不再为两路全帧图像建立额外 DDS reader，避免与 录制、预览和 WebRTC 竞争。SuperGlue 特征匹配通过共享 Unix IPC worker 执行。

Jetson NX 同时发布：

insight3_a_global_camera_center -> right_tcp
insight3_b_global_camera_center -> left_tcp

这是当前 profile 的实测静态外参；其他 profile 必须按机械尺寸独立标定。

判断规则

默认阈值：

  • 平移差异：0.15 m；
  • 旋转差异：10°。

平移或旋转任一达到阈值即进入直跳模式。差异以当前实际发布的 T_map_odom 为参考，而不是上一次 PnP 观测或 EKF 尚未显示出来的内部目标， 因此判断对应用户此刻在 Dashboard 看到的位置偏差。

阈值可通过以下参数调整：

--jump-translation-m 0.15
--jump-rotation-deg 10.0

这两个阈值与三帧共识的 0.20 m / 12° 不是同一概念：后者检查连续三次 PnP 结果是否彼此一致，前者在共识通过后检查“可靠新位置”和“当前显示位置”相差 多远。

三种更新模式

initialize

本次地图会话第一次定位成功时，直接以观测初始化 T_map_odom，并从当前位置 开始全局轨迹。

ekf

新观测小于 0.15 m 且 10° 时，作为误差状态 EKF 的绝对观测。EKF 更新 内部目标，对外校正继续按默认 1 秒时间常数随 100 Hz VIO 渐进收敛，因此没有 单帧跳变，旧 Path 段继续保留。

jump

平移或旋转任一达到阈值时：

  1. EKF 的内部目标和对外校正同时直接重置为新 T_map_odom；
  2. EKF 协方差清零，从找回后的状态重新累计过程不确定度；
  3. 下一帧全局 Pose 直接发布跳回后的正确位置；
  4. 清空旧的可视 Path 段，只保留最新原始 VIO 样本并从新位置重新画轨迹， 避免在飞走位置与找回位置之间绘制跨空间长线。

Insight3 A 和 Insight3 B 各自独立判断、计数和重置，任一路直跳不会清除另一 路相机轨迹。

暂时看不到地图时

一旦某路 Insight3 完成过首次全局定位，T_map_odom 就会保留。相机转向没有 建图的区域、当前帧 PnP 匹配不足时：

  1. 状态切换为 tracking_mode=vio_only，但 localized 继续保持 true；
  2. 继续用最后的 EKF 校正乘以最新设备 VIO，以 30 Hz 发布全局 Pose；
  3. Path 继续随 VIO 增长，Dashboard 模型和轨迹不会因为 PnP 暂时失败而隐藏；
  4. 再次看到已建图区域后恢复 map_matched，由新的三帧 PnP 共识校准累计漂移。

全局 Pose 发布直接使用已解析的双目中心外参，不再依赖 Path 重建器是否已经 缓存该外参。只有从未取得全局定位、New map/VIO 时间戳复位清除了校正，或者 设备 VIO 本身停止发布时，才没有可以继续外推的全局 Pose。

状态诊断

/insight_global/insight3_a/status 和 /insight_global/insight3_b/status 增加：

  • correction_mode：none、initialize、ekf 或 jump；
  • correction_translation_m：新观测相对当前发布校正的平移差；
  • correction_rotation_deg：对应旋转差；
  • hard_relocalizations：本次地图会话累计直跳次数；
  • jump_translation_m、jump_rotation_deg：当前生效阈值。
  • tracking_mode：map_matched、vio_only、verifying 或 unlocalized。

发生直跳时 localizer 同时记录 warning：

insight3_a hard relocalization 1: 0.812 m / 4.20 deg

New map 或设备 VIO 时间戳复位会清空 EKF、共识、Path 和直跳计数。

Insight3 图像 mask 默认值为 0.2，即屏蔽两路图像底部 20%；可从 Dashboard 设置 热更新，不需要重启 localizer，设为 0 可关闭。mask 只改变参与特征匹配的图像区域， 不放宽 PnP 或三帧共识门槛。

验证边界

合成检查覆盖：

  • 0.10 m / 5° 进入 ekf，观测到达当帧对外校正不跳，后续逐步收敛；
  • 0.80 m 进入 jump，内部目标和对外校正立即等于新观测；
  • 单独超过 10° 同样进入 jump；
  • 直跳后协方差清零。
  • 没有新 PnP 观测时，已有校正保持不变。

实机 localizer 已正常加载，两路状态均发布 0.15 m / 10° 生效阈值，2 Hz 图像中继、地图和 SuperPoint 推理链路正常。正式验收仍需现场执行“大幅飞走、 重新对准地图并连续三帧定位”，同时测量找回误差并确认直跳时机符合操作体验。

相关专题：Insight9 建图与回环检测。