核心课程 / SLAM 与自主导航 / EXP-07
EXP-07 · 第 3 章 · 综合性 · 8 学时

RTAB-Map 3D SLAM

一句话读懂：前 6 个实验，小车眼里世界是"一张平面图"（俯视图，像户型图）。这节课让小车"长出一双立体眼睛"——用 RGB-D 深度相机 + RTAB-Map，构建三维点云/八叉树地图，让机器人理解高度、层次、悬空障碍，为"分楼层导航、抓取定位"打基础。

难度 ⭐⭐⭐ · 8 学时综合性（进阶）📌 无配套源码资源前置：EXP-03

01问题引入：平面图看不见"头顶的横梁"

情境 二维栅格地图是俯视图：一根离地 1.2 m 的横梁、一张悬空的桌面下沿，在俯视图里要么被并进"障碍"、要么根本看不见。仓库取货机器人要"知道第二层货架上有东西"，机械臂抓取要知道"杯子在桌面上 30 cm 处"——这些都需要高度这一维。给小车装上 RGB-D 深度相机（本课程硬件 HP60C），用 RTAB-Map 构建三维点云 / 八叉树地图，世界就从"户型图"升级成了"3D 模型"。

02原理通俗讲解：从"照片"到"立体"的两步跳

白话比喻 普通相机只记"那里有个东西"，深度相机还记"那个东西离我多远"——有了方向（像素 u,v）+ 距离（深度 d），就能拼出立体世界。点云地图是"一堆沙子"（每个颗粒一个三维点，直观但冗余），OctoMap 是"乐高积木"（规则小方块体素，能数得清哪块占据哪块空——可压缩、可增量更新，导航避障常用）。
认路方式
词袋回环（Bag of Words）

RTAB-Map 用图像特征（ORB）聚类成"词汇"，一帧图 = 一组词汇集合；检索"和当前帧最像的历史帧"，相似度过阈值即回环 → g2o 图优化消漂移。就像凭"照片里出现吧台+红椅+吊灯"判断"我回到了这家咖啡馆"，而不是逐像素比照片。

反投影
像素 → 三维点

有内参 (fx, fy, cx, cy) 就能把 (u,v,d) 换算成空间点：X=(u−cx)·d/fx，Y=(v−cy)·d/fy，Z=d。像素放图像中心 (cx,cy) 时应得 X≈0、Y≈0——这是自检口诀。

与前作对比
同是图优化，传感器不同

slam_toolbox：激光雷达 + 扫描匹配回环 + 二维栅格；RTAB-Map：RGB-D 相机 + 图像词袋回环 + 三维点云/OctoMap。两者都是图优化 SLAM，只是"用什么看世界"不同。

三维导航
OctoMap 进代价地图

三维地图替代二维栅格作 costmap 输入，悬空障碍可被正确区分（低矮可跨、横梁必躲）。但 ROS2 里需要投影/转换环节，地面小车通常二维就够——三维更多服务感知/抓取/展示。

选型口诀：二维 = 平面图（便宜够用），三维 = 3D 模型（信息全但贵）。核心判据就一条——任务需不需要"高度"这一维：出现悬空障碍 / 高度分层 / 抓取定位需求时，才该上三维。

03关联知识点

知识点 1
RTAB-Map 框架

Real-Time Appearance-Based Mapping：前端图像特征里程计 → 局部地图 → 词袋回环 → g2o/GTSAM 位姿图优化。

知识点 2
RGB-D 二合一

HP60C 同时输出彩色图（640×480）与深度图（每像素距离 d，单位毫米，25 fps）；反光、超距、强光处深度返回 0（图上全黑）。

知识点 3
点云 vs OctoMap

海量 3D 点直观但冗余不可编辑；体素八叉树可压缩、增量更新、支持占据查询——导航/避障更常用后者。

知识点 4
回环那一"咔"

回环触发时地图会整体抖一下——那是图优化一次性校正所有历史帧的瞬间，属正常现象，不是 bug。

知识点 5
相机内参来源

/camera_info 里带 fx/fy/cx/cy；ros2 topic echo /ascamera_hp60c/camera_publisher/depth0/camera_info 可查。反投影公式必须配内参才成立。

知识点 6
算力现实

RK3576 跑实时三维 SLAM 压力大；三维比二维更怕运动畸变——遥控角速度 ≤0.5 rad/s，转角处多停留。

04实验目标

  • 理解视觉 SLAM 与 RTAB-Map 基本原理（ORB 特征、词袋回环、图优化） —— 原理理解
  • 理解 RGB-D 相机工作原理与反投影公式、点云/八叉树两种地图表示 —— 原理理解
  • 掌握三维建图与三维导航的通用流程（相机 → RTAB-Map → 操控 → 保存） —— 流程掌握
  • 能够完成 RGB-D 数据链路验证（话题频率 / 深度图查看 / 手算反投影） —— 流程掌握
  • 能对比二维与三维 SLAM 的六维差异并做场景选型分析 —— 迁移应用

05实验环境与准备

资源声明源讲义明确标注：本实验 spec 的 source_code_path 指向 source-codes/exp07/，notes = 无资源（仓库暂缺 RTAB-Map 专用源码）。因此本实验为原理 + 通用流程型：以概念讲解、通用命令、数据链路验证与选型分析为主，不强行要求跑通三维导航、不编造项目专属 launch；若后续补充源码，将按源码更新操作步骤。
类别配置
硬件小车 C1 + HP60C RGB-D 深度相机（驱动在 examples/camera/HP60C/src/ascamera/）
系统Ubuntu 22.04 + ROS 2 Humble
核心库rtabmap_ros（ROS2 版）、depth_image_proc、octomap_server、rviz2
依赖HP60C 驱动（ascamera）能发布 RGB + Depth 话题；Nav2 可用（EXP-03）
主入口通用流程 ros2 run rtabmap_slam rtabmap + 相机驱动 ros2 launch ascamera hp60c.launch.py；本项目为原理演示型
⚠️ 话题名必须记准HP60C 驱动刻意不允许 remap（驱动靠话题名里的 "rgb0/depth0" 子串决定推哪路流），所以真实话题名是 /ascamera_hp60c/camera_publisher/rgb0/image、.../depth0/image_raw、.../depth0/camera_info、.../depth0/points——不是教科书里常见的 /camera/rgb/image_raw。第 08 节的命令都按真实名写。
两条路线有 HP60C 相机：按 08 节通用流程跑数据链路验证与三维建图演示；无相机 / 无资源：完成反投影手算 + 对比表 + 选型判断题（报告写全即可），可观看课程演示视频/截图。深度相机注意有效距离与避开强光直射。

06动画讲解：RGB-D 反投影生成三维点云

深度图 (u,v,d) 近 = 红，远 = 蓝 HP60C 三维点云 (X,Y,Z) X Y Z X=(u−cx)·d/fx Y=(v−cy)·d/fy, Z=d d 先由毫米换成米；中心行 v=cy 时 Y≈0

图 7-1：深度图每个像素 (u,v,d) 携带"方向+距离"，乘相机内参反投影成空间点 (X,Y,Z)；逐帧累积并用相机自身运动拼接，就长出了带颜色的三维点云——再体素化（乐高化）即得 OctoMap。动画中彩色点与左侧深度图像素块一一对应。注意：上式是"相机光学坐标系"约定，Z 朝前、X 朝右、Y 朝下，所以 v 小于 cy（图像偏上）时 Y 为负——这也是深度图坐标系与 RViz 世界系不一致的根源。

07互动讲解：2D 栅格 vs 3D 点云选型台

🎛 给任务选地图形态：二维够吗？该上三维吗？
↑ 点击四个场景：先自己选二维/三维并说明理由，再对照讲义的六维选型表推演。
维度二维 SLAM（slam_toolbox）三维 SLAM（RTAB-Map）
传感器激光雷达RGB-D 深度相机
地图栅格（俯视）点云 / OctoMap（立体）
算力低（RK3576 轻松）高（实时需更强算力）
精度水平精度高含高度信息，垂直精度依赖深度质量
导航Nav2 直接消费 /map需投影/转换后才能消费
适用地面巡检 / 送货分层避障 / 抓取定位 / 展示

08实操步骤：RGB-D 数据链路验证与通用流程

1启动 HP60C 相机驱动，确认四话题（rgb0/image、depth0/image_raw、depth0/camera_info、depth0/points）都有数据
2rqt_image_view 查看深度图（近红远蓝、反光处发黑）；echo camera_info 抄下 fx/fy/cx/cy
3用 numpy 手算一次反投影：把一个像素 (u,v,d) 转成 (X,Y,Z)，并用中心像素自检 X≈0、Y≈0（注意 d 先换算成米）
4启动 RTAB-Map（显式指定话题名）+ RViz PointCloud2 显示，遥控小车低速走闭环，转角停留 1-2 s，观察点云成形与回环对齐
5保存与导出：rtabmap 数据库（.db）+ octomap_server 导出八叉树；无相机条件则按此流程写报告 + 看演示
# ① 启动相机（命名空间 ascamera_hp60c），核对四话题
ros2 launch ascamera hp60c.launch.py
ros2 topic list | grep ascamera_hp60c
#   /ascamera_hp60c/camera_publisher/rgb0/image
#   /ascamera_hp60c/camera_publisher/depth0/image_raw
#   /ascamera_hp60c/camera_publisher/depth0/camera_info
ros2 topic hz /ascamera_hp60c/camera_publisher/depth0/image_raw
ros2 topic echo /ascamera_hp60c/camera_publisher/depth0/camera_info   # 抄 fx fy cx cy

# ② 查看深度图（近红远蓝，反光/超距处发黑）
ros2 run rqt_image_view rqt_image_view /ascamera_hp60c/camera_publisher/depth0/image_raw

# ③ 启动 RTAB-Map（通用命令：话题名不匹配就显式指定）
ros2 run rtabmap_slam rtabmap --ros-args -p subscribe_rgbd:=true \
  -p rgb_topic:=/ascamera_hp60c/camera_publisher/rgb0/image \
  -p depth_topic:=/ascamera_hp60c/camera_publisher/depth0/image_raw \
  -p approx_sync:=true

# ④ 遥控低速走闭环（角速度 ≤ 0.5 rad/s，转角停留 1-2s）观察 RViz 点云
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# ⑤ 保存数据库 / 导出八叉树
ros2 service call /rtabmap/save std_srvs/srv/Empty
ros2 launch octomap_server octomap_mapping.launch.xml   # 通用
# 反投影手验算（numpy）：中心像素应得到 X≈0, Y≈0, Z=d
import numpy as np
fx, fy, cx, cy = 380.0, 380.0, 320.0, 240.0   # ← 以实测 camera_info 为准
u, v, d = 320.0, 240.0, 1.500                 # 深度换算成「米」再算
X = (u - cx) * d / fx
Y = (v - cy) * d / fy
Z = d
print(X, Y, Z)   # 期望 ≈ 0, 0, 1.5
单位陷阱HP60C 的 depth 图默认是 millimeter 编码（16UC1，单位毫米），而 RTAB-Map 与 Nav2 都用「米」。手算时先把 d 除以 1000，否则算出来的 Z 会大三个数量级——这是本实验最容易错的一步。
无资源时的验收标准仓库暂缺 RTAB-Map 专用 launch/源码——有相机跑通 ①-⑤ 并记录回环现象；无相机则交付：反投影验算截图 + 二维/三维对比表 + 四场景选型结论 + 完整通用流程文字描述（等同本页 08 节）。

09重点与易错点

重点① 词袋回环 vs 扫描匹配回环的本质区别；② RGB-D 反投影公式、内参与毫米→米的单位；③ 点云与 OctoMap 的取舍（沙子 vs 乐高）；④ 六维选型表——"什么信号提示你该上三维了"。
坑现象解决
深度图全黑无深度数据超出有效距离或反光/强光直射；调整距离与环境
话题名照抄教科书RTAB-Map 收不到数据真实名是 /ascamera_hp60c/camera_publisher/rgb0/image 与 depth0/image_raw；用 -p rgb_topic:=… / -p depth_topic:=… 显式指定
给相机做 remap话题改名后相机不再出流驱动靠话题名里的 "rgb0/depth0" 子串判断推哪路流，remap 会丢掉这个线索——不要 remap
深度单位当米用点云/点算出来大 1000 倍depth 图是 16UC1 毫米编码，计算前先 /1000
反投影公式记反点云左右/上下镜像中心像素自检：(cx,cy) → X≈0、Y≈0、Z=d
三维地图"花"点云散乱重影走太快导致运动畸变；更慢速（≤0.5 rad/s），转角停留 1-2 s
回环时地图乱一下点云整体抖动正常！图优化一次性校正所有帧，记录即可
算力不足卡顿RViz 卡、掉帧降分辨率/关闭额外显示；实时性优先场景改用二维
以为三维导航直接替换Nav2 吃不到 OctoMap需投影/转换成 Nav2 costmap 可消费的格式，工程有额外环节
以为词袋 = 深度学习概念混淆词袋是经典特征聚类（ORB + KMeans），不是神经网络
跟风上三维成本失控三维算力与工程成本翻倍；地面任务优先二维，确有高度需求再上
  • 本实验为原理+演示型，不强行要求跑通三维导航，重点在理解与选型分析
  • RGB-D 强光/远距/反光处失效——户外或大场景优先激光雷达
  • 三维地图文件大，保存后检查磁盘空间
  • 记住"话题名 + 毫米单位"这两个本机特有的坑，比背公式更容易拿分

10总结与思考：什么时候该上三维

本课收获打通了"深度图像素 → 反投影 → 三维点云 → 体素 OctoMap"的表示链；理解了 RTAB-Map 以词袋回环替代扫描匹配的图优化路线；掌握 RGB-D 数据链路验证与三维建图通用流程；并能用六维对比表回答选型问题。也如实认识了工程边界：仓库暂无 RTAB-Map 专用源码，本课为原理+流程型。
思考 1RTAB-Map 用"图像词袋"做回环检测，slam_toolbox 用"扫描匹配"——两者本质区别是什么？各在什么传感器配置下更可靠（提示：走廊重复纹理 vs 空旷大厅）？
思考 2写出 RGB-D 反投影公式并解释每个符号；点云与 OctoMap 各自优缺点是什么、导航为何更常用 OctoMap？请给出至少两条"该上三维了"的信号，并解释把三维导航落到 Nav2 上的工程难点。