RTAB-Map RGB-D 视觉里程计验证

验证目标

验证 Jetson Orin Nano + Intel RealSense D435i 上运行的 RTAB-Map 是否产生有效的视觉里程计，为后续 PX4 室内无 GPS 导航实验奠定基础。

系统配置： Jetson Orin Nano · Ubuntu 22.04 · ROS2 Humble · Intel RealSense D435i · RTAB-Map · PX4 1.17 · MAVROS

1. 验证 RTAB-Map 里程计发布频率

ros2 topic hz /rtabmap/odom

average rate: 5.72 Hz
average rate: 5.57 Hz

解读：
RTAB-Map 以约 5.7 Hz 发布位姿估计。
周期 T = 1/f ≈ 0.175 秒 → 每 175 毫秒 生成一个新位姿。

讨论：
建图与 SLAM 应用：5-10 Hz = 可接受
激进自主飞行：20-50 Hz = 更佳
当前结果确认稳定的 RGB-D 里程计，但尚未达到高帧率 VIO。

2. 验证位姿输出

ros2 topic echo /rtabmap/odom --once

position:
x: -0.0299
y: -0.0103
z: -0.0390

orientation:
x: -0.0040
y: 0.0071
z: 0.0060
w: 0.99995

covariance:
- 5.35e-06

位置 (Position)

x: -0.0299 → -2.99 cm (X 轴位移)
y: -0.0103 → -1.03 cm
z: -0.0390 → -3.90 cm
相机接近起始位置，数值合理。

方向 (Orientation - 四元数)

四元数 w = 0.99995
小幅度旋转时 w ≈ 1 是正常现象。
健康的四元数通常满足 w 接近 1。
跟踪失败时，w 可能变为 0 或无效。
当前结果：有效方向估计

协方差 (Covariance)

数值：5.35 × 10⁻⁶ (非常小)
协方差表示不确定性：小 → 高置信度，大 → 低置信度
典型失败估计：9999
当前结果：置信度很高

3. 验证 TF 树

ros2 run tf2_ros tf2_echo odom camera_color_optical_frame

最初：Invalid frame ID "odom" (正常，TF 尚未发布)
数秒后有效变换出现：
Translation: [-0.005, 0.017, -0.003]
Rotation: [0.005, 0.004, -0.010, 1.000]

TF 树验证通过

4. 理解平移 (Translation)

平移 [x, y, z] 表示相机移动。
示例：[0.060, 0.009, 0.161] → X = 6.0 cm, Y = 0.9 cm, Z = 16.1 cm

测试期间，数值随相机移动平滑变化 → 位置跟踪有效

5. 理解旋转 (Rotation)

四元数 [x, y, z, w] 由 TF 转换为 Roll/Pitch/Yaw。
示例：RPY: [1.96°, 60.78°, 25.20°]
测试期间，旋转数值随相机倾斜连续变化 → 方向跟踪有效

6. 物理验证

相机移动测试：前进 · 后退 · 左 · 右 · 上 · 下 · 旋转

位置连续变化
旋转连续变化
无突变跳跃
无变换冻结
无跟踪丢失

RTAB-Map 估计的是真实相机运动，而非静态值。

7. SLAM 数学原理

T = [ R t; 0 1 ]

R = 旋转矩阵 · t = 平移向量

Frame(k) → 特征提取 → 特征匹配 → 外点剔除 → 位姿估计 → 变换 T

RTAB-Map 随时间累积变换以估计完整运动轨迹。

8. 最终评估

D435i RGB 相机工作
D435i 深度相机工作
RTAB-Map 运行中
位姿估计有效
协方差健康
TF 树有效
视觉里程计验证通过
跟踪稳定

实测性能： 里程计频率 ≈ 5.7 Hz

当前成熟度等级：
安装阶段完成
视觉里程计阶段完成
TF 验证阶段完成
PX4 集成 → 下一步

下一个里程碑：
RTAB-Map Odometry → MAVROS Vision Pose → PX4 EKF2 → 室内无 GPS 飞行

结论

系统已超越纯软件安装阶段，正在产生经验证有效的 RGB-D 视觉里程计，适用于未来的 PX4 外部视觉集成实验。

Related Drobotics guides