UMI Cube Marker 相对定位

目标

两只 Insight3 夹爪相机都固定安装 UMI cube。Insight9 看到 cube 后，利用已知的 cube→Insight3 双目中心外参，为各自 VIO 提供低频 T_map_odom 绝对观测，纠正长时间 相对运动的累计漂移。它运行在 insight3-global-localizer 内部，不是独立后台服务。

几何与编号

Cube 外边长为 70 mm，四面 marker 黑框边长为 60 mm。Cube 原点位于几何中心，方向按 Insight3 光学坐标约定：+X 向右、+Y 向下、+Z 向前。

面	Insight3 A	Insight3 B	固定坐标
顶面	8	2	Y=-35 mm
后面	9	3	Z=-35 mm
左面	10	4	X=-35 mm
右面	11	5	X=+35 mm

Jetson NX 当前使用的 cube→双目中心平移为：

Insight3 A: [0, -58, 90] mm
Insight3 B: [0, -53, 93] mm
rotation: identity

左右值由同一块 4x4 AprilGrid、同一角点尺度分别测量。统一模型后左右 Y/Z 差异约 4.4/3.7 mm，与单段约 4–5 mm 的重复性相当；早期约 10 mm 的差异包含左右分别拟合 标定板角点尺度产生的系统误差。

坐标链

Insight9 使用压缩彩色图 /insight9_a/camera/color/image_rect_raw/compressed 检测 marker， 联合可见面得到 T_RGB_cube。在图像时间戳处组合：

T_map_camera(marker)
  = T_map_head_center
  * T_head_center_RGB
  * T_RGB_cube
  * T_cube_camera_center

T_map_odom(marker)
  = T_map_camera(marker)
  * inverse(T_odom_camera_center(VIO))

最终对外 Pose 仍以 Insight3 VIO 提供高频运动：

T_map_camera_center
  = T_map_odom(EKF)
  * T_odom_imu(VIO)
  * T_imu_camera_center

共识与 EKF

Cube marker 与自然地图重定位各有独立候选窗口，但最终更新同一个六自由度 T_map_odom 误差状态 EKF。

• 检测频率：10 Hz；
• 共识：5 帧窗口内至少 3 帧；候选间不超过 30 mm / 3 度；
• PnP：内点比例至少 0.75，重投影中位不超过 2.5 px；
• Cube marker 观测噪声：10 mm / 1 度；
• 小修正：进入 EKF，发布值按 1 秒时间常数平滑；
• 达到 150 mm 或 10 度：已确认观测触发 hard relocalization；
• 实际修正强制要求至少两个非共面标记面，每面至少三个 RANSAC 内点；即使配置为 min_markers=1，单面也不能修正。只看到单面时继续沿用 VIO，等待多面或自然特征定位。
• 离线 shadow 模式仍允许 min_markers=1，用于分析单面歧义。

头部位姿时间戳回退或 New Map 时，同时重置图像时间戳门槛、候选窗口和待处理队列。 每次重置递增会话编号，检测过程中发生重置时，旧结果不能写回共识、诊断状态或定位。

状态位于 /insight_global/insight3_a/status 与 insight3_b/status 的 cube_marker 字段， 包含 marker_ids、角点/内点数、重投影误差、observations、updates、 correction_mode 和 hard_relocalizations。

运行配置

生产配置位于 config/devices/jetson-nx/runtime.json：

"enabled": true,
"apply_corrections": true

没有该配置段时功能默认关闭；apply_corrections 缺省时默认只做 shadow。修改 profile 后 需执行 ./scripts/select_device.sh jetson-nx，再重建 insight3-global-localizer。

离线检查命令：

python3 tools/diagnostics/replay_cube_marker_shadow.py \
  --config config/devices/jetson-nx/runtime.json \
  --camera insight3_a /path/to/bag

该工具固定为只读 shadow，不受生产配置中 apply_corrections=true 影响。

验证边界与已知风险

修复前的 Take 54 回放结果：

相机	有效 PnP	重投影中位	局部相对误差中位
Insight3 A	132	0.87 px	60.0 mm / 5.31 度
Insight3 B	148	0.81 px	30.1 mm / 2.59 度

Insight3 A 在 ID 9、10、9+11 等部分视角出现过平面 PnP 错误分支，Z 方向错误可达约 0.7 m；多帧共识能过滤短暂异常，但持续错误解仍可能通过。因此当前实机必须监控 hard_relocalizations 和换面连续性。像素重投影只能说明投影自洽，不能作为机械外参或 全局定位绝对精度的独立真值。

2026-09-08 修复后，实际修正拒绝单面和共面内点。修复前可复现的约 0.54 px 角点扰动、 三帧稳定错误解不再进入修正；正确的双面合成观测仍能恢复位姿。相关测试共 130 项通过， 包括时间从 100 秒回退到 1 秒后的处理恢复，以及在途检测遇到时间回退/New Map 后的 成功、未检测到和异常三种结果隔离。回归脚本位于 ~/workspaces/insight_capture_tests/cube_marker_safety/test_regression.py。 验证时没有运行中的 Docker 容器，未进行在线 ROS 或真实相机部署验证；这些合成测试 不能代替现场精度与覆盖率评估。

对应主仓库提交：初始功能 ea2828d，安全修复 b8b53bc。