visual_localization_v3 定位数据流 (RK3588) Diagram

数据类型链 (真实)

• sensor_msgs/Image → KeyPoint[] + Desc[64D] (XFeat) → MapPoint3D[] (VSMAP medoid) → 2D-3D Correspondence (top-K 450)
• SE(3) T_map_cam (PnP 初值) → Ceres 精化 → T_map_odom (corrector 平滑) → TF map→odom
• odom 全程 T_odom_base: 投影预测 / SE(2) prior / 融合主链路

ROS 接口与 frame

• 订阅: /camera/cam1/image_raw (remap ~image) · ~camera_info · odom_topic (板 /odom, bag /jarvis/odom)
• 发布: localization_pose · _with_covariance · _path · map_cloud (latched) · used_map_points
• TF frame: map → odom (主输出) + map → base_link;无 service

Future / Planned (当前代码不存在)

• GTSAM factor graph: 未实现,全仓库 grep 零命中 — 不画入运行流
• 规划形态: Visual Pose Factor + Odom Factor + AprilTag Factor → GTSAM/ISAM2 → 优化 T_map_odom
• 当前等价物: Ceres SE(2) odom prior (≈odom factor) + N=4 滑窗 between + AprilTag 角点直接注入 PnP (非 factor)