【RTAB-Map ROS2视觉SLAM】基于Astra Pro Plus的视觉建图与定位

前言

我们在很早以前就讲过在ROS noetic上配置过Astra S的基础使用教程
- 上手一个RGBD深度相机：从原理到实践--ROS noetic+Astra S（上）：解读深度测距原理和内外参推导
- 上手一个RGBD深度相机：从原理到实践--ROS noetic+Astra S（中）：RGB相机的标定和使用
最近项目中拿到一个Astra Plus Pro，于是就有了本文。
本文将使用Astra Plus Pro深度相机，进行RTAB-Map算法的配置，达到视觉SLAM建图与定位的效果，最终达成以下效果：

1 深度相机Astra Plus Pro配置

1-1 RGBD（RGB + Depth）

Astra Pro Plus 是一款基于 结构光（Structured Light）技术 的 RGB-D 深度相机，由 Orbbec 公司推出，用于获取环境的彩色图像与深度信息。
该设备通过 RGB 相机获取彩色图像，通过红外结构光系统计算深度信息，从而实现每个像素对应真实空间距离的 RGB-D 数据输出。
其主要特点如下：
- 支持 RGB 彩色图像与深度图同步输出
- 深度范围约为 0.6m ~ 8m
- RGB 分辨率最高可达 1920×1080 @ 30fps
- 深度分辨率最高可达 640×480 @ 30fps
- 支持输出 RGB 图像、深度图、红外图及点云数据
- 适用于室内机器人、SLAM建图、3D重建等应用场景

1-2 OrbbecSDK_ROS2安装与配置

首先的当务之急是确认自己的相机版本，不同相机型号对应的sdk版本和兼容性不一样，务必先确认版本！
官网：https://github.com/orbbec/OrbbecSDK_ROS2/tree/main
注意默认拉取仓库时候的分支是v2-main，这里我们使用的是Astra Plus Pro只在v1支持，所以必须进行切换
我们创建一个工作空间，然后拉取仓库

bash 复制代码

mkdir -p ~/orbbsec_ws/src
cd ~/orbbsec_ws/src
git clone https://github.com/orbbec/OrbbecSDK_ROS2.git
git checkout main

安装依赖

bash 复制代码

sudo apt install libgflags-dev nlohmann-json3-dev  \
ros-$ROS_DISTRO-image-transport  ros-${ROS_DISTRO}-image-transport-plugins ros-${ROS_DISTRO}-compressed-image-transport \
ros-$ROS_DISTRO-image-publisher ros-$ROS_DISTRO-camera-info-manager \
ros-$ROS_DISTRO-diagnostic-updater ros-$ROS_DISTRO-diagnostic-msgs ros-$ROS_DISTRO-statistics-msgs \
ros-$ROS_DISTRO-backward-ros libdw-dev

然后我们需要加载udev规则

bash 复制代码

cd  ~/orbbsec_ws/src/OrbbecSDK_ROS2/orbbec_camera/scripts
sudo bash install_udev_rules.sh
sudo udevadm control --reload-rules && sudo udevadm trigger

准备好以后我们就可以直接编译了

bash 复制代码

cd ~/orbbsec_ws
colcon build --event-handlers  console_direct+  --cmake-args  -DCMAKE_BUILD_TYPE=Release

1-3 驱动

根据自己相机的型号，找到对应的启动文件

bash 复制代码

. ./install/setup.bash
ros2 launch orbbec_camera astra_pro_plus.launch.py

启动后，我们可以
打开rviz2进行可视化

bash 复制代码

. ./install/setup.bash
rviz2

1-4 话题说明

1-4-1 RGB 彩色图像相关

/camera/color/image_raw：彩色相机输出的原始RGB图像数据，用于视觉SLAM、目标识别与图像处理。
/camera/color/camera_info：彩色相机内参（焦距、主点、畸变系数），用于图像几何校正与RGB-D对齐。

1-4-2 压缩版本（用于传输优化）

/camera/color/image_raw/compressed：JPEG压缩后的彩色图像
/camera/color/image_raw/compressedDepth：压缩的深度格式（一般用于跨网络传输）
/camera/color/image_raw/theora：视频流编码格式（较少用于SLAM）

1-4-3 深度图像相关

/camera/depth/image_raw：深度相机输出的原始深度图，每个像素表示距离信息（单位通常为米或毫米），用于3D重建与SLAM。
/camera/depth/camera_info：深度相机内参，用于深度图投影到三维空间。

1-4-4 深度压缩/传输版本

/camera/depth/image_raw/compressed：压缩深度图（用于网络传输）
/camera/depth/image_raw/compressedDepth：深度压缩编码格式
/camera/depth/image_raw/theora：视频编码深度流（低频使用）

1-4-5 点云数据（3D信息核心）

/camera/depth/points：由深度图 + 相机内参生成的三维点云数据（PointCloud2），用于：
- 3D环境建图（RTAB-Map）
- 障碍物检测
- FAST-LIO2 / Voxel mapping
- RViz可视化

1-4-6 红外（IR）相机数据

/camera/ir/image_raw：红外图像数据（灰度图），用于结构光深度计算
/camera/ir/camera_info：红外相机内参，用于深度计算与标定

1-4-7 IR压缩版本

/camera/ir/image_raw/compressed
/camera/ir/image_raw/compressedDepth
/camera/ir/image_raw/theora

1-4-8 深度处理与对齐信息

/camera/depth_filter_status：深度滤波状态信息（如降噪、孔洞填补状态）
/camera/depth_to_color：深度图对齐到彩色图的结果
/camera/depth_to_ir：深度图对齐到红外图的结果

2 RTAB-Map

2-1 视觉SLAM简述

视觉SLAM（Visual Simultaneous Localization and Mapping）是一种利用相机作为主要传感器，同时完成**环境建图（Mapping）与自身定位（Localization）**的技术。
其核心流程如下：
- 通过相机获取连续图像序列
- 提取图像特征（如ORB、SIFT等）
- 进行帧间匹配估计相机运动（视觉里程计）
- 构建位姿图（Pose Graph）
- 通过回环检测修正累计误差
- 输出一致的地图与位姿信息

2-2 视觉 SLAM分类

视觉 SLAM 大体分为几类：

特征点法（Feature-based）
直接法（Direct）
半直接法（Semi-direct）

2-2-1 特征点法（最经典 / 工程最常用）

代表算法：ORB-SLAM 系列（最重要）
- ORB-SLAM
- ORB-SLAM2（单目 / 双目 / RGB-D）
- ORB-SLAM3（支持视觉+IMU）
  - 【10天速通Navigation2】(四) ：ORB-SLAM3的ROS2 humble编译和配置
  - 【10天速通Navigation2】(五) ：基于gazebo仿真的复杂地形的ORB-SLAM3配置
特点：
- 提取特征点（ORB / SIFT / FAST）
- 做特征匹配
- PnP求位姿
- 图优化 + 回环检测

2-2-2 直接法（Direct SLAM）

代表算法：
- LSD-SLAM
- DSO（Direct Sparse Odometry）
特点
- 不提特征点
- 直接用像素灰度变化

2-2-3 半直接法（SVO）

代表算法：SVO（Semi-direct Visual Odometry）
特点：少量特征 + 直接法结合

2-3 RTAB-Map

Real-Time Appearance-Based Mapping（实时外观/视觉建图）

RTAB-Map 是一种基于**外观信息（Appearance-Based）与图优化（Graph Optimization）**的SLAM框架，主要用于实时RGB-D SLAM建图与定位。
其核心特点包括：
- 支持 RGB-D、双目、单目及激光雷达等多种传感器
- 基于图结构（Pose Graph）进行建图与优化
- 具备回环检测（Loop Closure）能力，可有效减少累计误差
- 采用基于外观特征的地点识别方法，提高长期定位稳定性
- 同时支持2D栅格地图与3D点云地图生成
值得一说的是：
- RTAB-Map 并不属于视觉SLAM中的单一算法类型（特征点法、直接法或半直接法），而是一种工程化的图优化视觉SLAM系统。
- RTAB-Map 实际上是多个SLAM思想的组合：
  - 前端（Tracking）：
    - 基于特征点匹配（ORB / SURF / SIFT）
    - 或结合 RGB-D 深度约束
  - 后端（Optimization）：
    - 位姿图优化（Pose Graph Optimization）
    - 回环检测（Loop Closure）

2-4 RTAB-Map原理概述

RTAB-Map 的核心原理可以概括为：基于外观特征的图优化SLAM系统，通过"记忆管理 + 回环检测 + 位姿图优化"实现长期稳定建图与定位。

2-4-1 数据输入层

RTAB-Map 接收多种传感器输入，典型为 RGB-D 组合：
- RGB图像（/camera/color/image_raw）
- 深度图像（/camera/depth/image_raw）
- 相机内参（camera_info）
用于提供环境的视觉信息 + 三维结构信息。

2-4-2 前端：视觉里程计（Odometry）

在前端阶段，RTAB-Map 估计相机的相对运动：
- 提取图像特征点（ORB / SURF / SIFT）
- 进行帧间匹配
- 结合深度信息恢复3D点
- 计算相机位姿变化（motion estimation）
输出：

当前帧相对于上一帧的位姿（odometry）

2-4-3 节点生成（Keyframe Selection）

系统不会保存每一帧，而是：
- 选取"关键帧（Keyframes）"
- 每个关键帧包含：
  - 图像信息
  - 深度信息
  - 位姿信息
  - 特征描述子
这些关键帧构成图结构的节点（Node）。

2-4-4 回环检测（Loop Closure）

RTAB-Map 的核心能力之一：
- 通过外观特征（Appearance-Based）
- 检测当前场景是否曾经访问过
- 如果匹配成功，则认为发生"回环"
作用：

修正累计误差（drift），提升地图全局一致性

2-4-5位姿图优化（Graph Optimization）

检测到回环后：
- 构建位姿图（Pose Graph）
  - 节点 = 关键帧
  - 边 = 相对位姿约束
然后进行全局优化：
- 最小化整体误差
- 修正所有节点位置

2-4-6 地图生成（Mapping）

优化后的结果用于生成地图：

2-5 安装RTAB-Map

bash 复制代码

sudo apt update
sudo apt install ros-humble-rtabmap ros-humble-rtabmap-ros

2-6 启用建图

bash 复制代码

ros2 launch rtabmap_launch rtabmap.launch.py \
    rgb_topic:=/camera/color/image_raw \
    depth_topic:=/camera/depth/image_raw \
    camera_info_topic:=/camera/color/camera_info \
    frame_id:=camera_color_optical_frame \
    approx_sync:=true \
    rgbd_sync:=true \
    visual_odometry:=true \
    use_sim_time:=false \
    publish_tf:=true \
    odom_frame_id:=odom \
    base_frame_id:=base_link \
    map_frame_id:=map \
    rviz:=true \
    delete_db_on_start:=true

参数说明：
- rgb_topic：RGB 图像输入
- depth_topic：深度图输入
- camera_info_topic：相机内参
- frame_id：输入帧（当前观测坐标系）
- approx_sync ：是否使用近似时间同步 （设置为true允许 RGB 和 Depth 时间戳有轻微误差）
- rgbd_sync：是否启用 RGB-D 融合同步（设置为true：RGB + Depth 作为一帧联合处理（3D SLAM标准模式））
- visual_odometry：是否启用视觉里程计（VO）
- use_sim_time：是否使用仿真时间（/clock）
- publish_tf：是否发布 TF 变换树（发布map-odom）
- odom_frame_id：里程计坐标系名称
- base_frame_id：机器人底盘坐标系
- map_frame_id：全局地图坐标系
- rviz：是否自动启动 RViz 可视化
- delete_db_on_start：是否启动时清空数据库

2-7 保存地图

点击左上角的蓝色圈圈进行保存! $\[Pasted image 20260627154143.png$ ]
一共了几个选项：
- Global map optimized（全局优化地图）：包含回环检测，误差最小
- Global map not optimized（全局未优化）：原始轨迹（有漂移）
- Local map optimized（局部优化地图）：只是一段"窗口地图"
- Local map not optimized（局部未优化）：当前帧累积的原始局部点云
rtabmap.db是一个基于 SQLite 的 SLAM数据库，存储"图结构 + 关键帧 + 点云 + 优化信息"的完整工程状态。用来保存整个SLAM过程，并支持之后重新加载、优化和导出地图。
相比与其他地图格式的优点：
- 可回放、可重建（最核心优势）
- 支持后期优化（Re-optimization）
- 保存完整SLAM信息（不是只存地图），额外会存储- 图结构（pose graph、关键帧图像（RGB/Depth）、特征点、回环约束、传感器数据（IMU/odom）等等
- 支持续建图（Incremental Mapping）
地图默认保存在~/.ros/rtabmap.db! $\[Pasted image 20260627153836.png$ ]

bash 复制代码

cd ~/.ros
ls

2-8 定位模式

在建图保存后，我们可以通过database_path:=/home/lzh/.ros/rtabmap.db加载刚刚保存的地图，同时我们localization:=true开启定位模式

bash 复制代码

ros2 launch rtabmap_launch rtabmap.launch.py \
    database_path:=/home/lzh/.ros/rtabmap.db \
    localization:=true \
    rgb_topic:=/camera/color/image_raw \
    depth_topic:=/camera/depth/image_raw \
    camera_info_topic:=/camera/color/camera_info \
    frame_id:=camera_color_optical_frame \
    approx_sync:=true \
    rgbd_sync:=true \
    visual_odometry:=true \
    rviz:=true

当然你也可以不指定database_path直接尝试进行定位

3 SDK 安装

本节为额外内容，OrbbecSDK是官方提供的，不需要ros2直接使用的sdk

3-1 SDK安装

https://github.com/orbbec/OrbbecSDK/releases
同样注意查看对应的sdk版本支持
下载后我们直接解压

bash 复制代码

unzip OrbbecSDK_C_C++_v1.10.35_20260128_fa838f5_linux_x64_release.zip

3-2 基础使用

bash 复制代码

cd OrbbecSDK_v1.10.35/Example/bin
./HelloOrbbec

3-3 深度相机可视化

bash 复制代码

cd OrbbecSDK_v1.10.35/Example/bin
./depth_viewer

总结

本文使用Astra Plus Pro深度相机，进行ROS2的RTAB-Map算法的配置，达到视觉SLAM建图与定位的效果
如有错误，欢迎指出！
感谢你的观看！