visual_localization_v3 单帧定位时序 (RK3588)

visual_localization_v3 单帧定位时序 (RK3588) A sequence diagram generated by Archify. sensor_msgs/Image Extract (灰度 512×640) KeyPoint[] + Desc[64D] Query (ref KF + 共视图 n=10) MapPoint3D[] + medoid 投影窗口匹配 (odom 预测) 2D-3D 对应 (top450) solvePnPRansac (EPnP·KB切平面) T_map_cam + inliers SE(2) prior + 滑窗 BA (N=4) T_w_body (优化) T_map_odom = T_w_body·T_odom_body⁻¹ 小校正平滑 / 中校正确认 / 大校正严格闸门 (localization_node.cpp:1381) TF map→odom + localization_pose 特征不足 → 跳帧 匹配 <10 → widen ×3 重搜 仍不足 → 不进 PnP inliers < min / PnP 失败 → 丢帧 连续 3 帧失败 → CosPlace 全局重定位 0.5m / 5° 触发 → 切换 ref KF 重建 AprilTag 角点注入 (负 id · 休眠) 帧到达 特征与匹配 求解与融合 异常分支 (secondary) Camera · cam1/image_raw · Sequence participant Camera cam1/image_raw Loc Node · v3 单节点 · Sequence participant Loc Node v3 单节点 XFeat · RKNN NPU · Sequence participant XFeat RKNN NPU Local Map · VSMAP·共视图 · Sequence participant Local Map VSMAP·共视图 Matcher · 投影窗口 · Sequence participant Matcher 投影窗口 PnP · RANSAC PnP · Sequence participant PnP RANSAC PnP Optimizer · Ceres BA · Sequence participant Optimizer Ceres BA TF · map→odom · Sequence participant TF map→odom Nav · 下游消费 · Sequence participant Nav 下游消费 Legend request return async trace default message

主成功路径 (源码级)

  • • imageCallback → XFeat RKNN → buildLocalMap → searchByProjectionMulti → solvePnPRansac (localization.cpp:2997) → Ceres → TF
  • • 数据类型: Image → KeyPoint[]+Desc[64D] → MapPoint3D[] → 2D-3D → T_map_cam → T_w_body
  • • 稳态全链 24fps (NPU 特征 ~11ms)

融合公式 (按源码)

  • • candidate = T_w_body · T_odom_body⁻¹ (localization_node.cpp:1381) — body 系, 非cam系假设公式
  • • MapOdomCorrector: 小校正平滑 / 中校正确认 / 大校正严格闸门
  • • 本节点发 map→odom, 里程计发 odom→base_link; 无 GTSAM (grep 零命中)

异常分支 (真实阈值)

  • • 匹配门 min_matches_for_pnp_=10 → widen×3 重搜; 质量闸门 min_final_inliers_ 等五项联合
  • • 连续 3 帧失败 (max_consecutive_failures_) → CosPlace 全局重定位 + 地理过滤 (字面量 20/0.30/5/8.0)
  • • 局部地图 0.5m/5° 切换 ref KF;AprilTag 角点注入 (休眠)