【SLAM】在ORB_SLAM2的ROS模式下使用RealSense D435相机

本文介绍了如何在ORB_SLAM2项目中使用RealSense D435相机作为RGB-D输入源，包括ROS下启动D435相机、ORB_SLAM2订阅Topic、ORB_SLAM2读取realsense-viewer录制的rosbag文件等步骤。。

本文首发于❄慕雪的寒舍

1. 前言

先前已经编写了如何用TUM数据集运行ORB_SLAM3以及如何在ROS模式下运行ORB_SLAM3的博客，ORB_SLAM3是基于ORB_SLAM2的，甚至代码仓库中还有遗留的ORB_SLAM2命名空间namespace没有修正，二者不管是用TUM RGB-D数据集直接运行还是在ROS模式下运行的命令都完全一致。所以，在阅读本文之前，先参考上面给出的两篇博客，安装ORB_SLAM2的依赖库和ROS环境。

注意：若想在ROS模式下运行ORB_SLAM2，则一定要安装OpenCV 3.2.0版本，否则会因为系统中存在多个不同版本的OpenCV从而导致动态库链接错误！

RealSense D435在Ubuntu 18.04中realsense驱动安装的步骤也在【SLAM】ubuntu 18.04 安装 RealSense D435 相机驱动（ARM64/AMD64）一文里面介绍过了，继续阅读本文之前，需要先把D435的驱动搞定。

测试使用的操作系统为 Ubuntu 18.04 LTS，平台为AMD64。

2. 运行步骤

2.1. 编译ORB_SLAM2

阅读到这里，就默认你已经根据上面给出的参考博客把相关依赖项、ROS环境和realsense驱动都已经安装完毕了，这里给出ORB_SLAM2的编译步骤，和ORB_SLAM3也是如出一辙的。

bash 复制代码

git clone https://github.com/raulmur/ORB_SLAM2.git

cd ORB_SLAM2
chmod +x build.sh
./build.sh

注意，编译之前需要先修改build.sh脚本，把所有make -j修改成make -j4，避免make编译的时候无止尽地吃掉所有系统资源，这个在ORB_SLAM3的博客中也提到过。

make命令-j选项后面跟着的数字是编译使用的线程数量 ，建议改成linux系统cpu线程数量的一半或者2/3，避免吃光所有系统资源。选项-j后面不跟数字的时候，编译项目时会无止尽地吃掉所有系统内存和CPU，直到被操作系统KILL掉，编译失败（在我的测试环境中是这个现象）。

编译完毕普通版本后，再编译ROS版本，同样需要把build_ros.sh脚本里面的make -j改成make -j4。

bash 复制代码

chmod +x ./build_ros.sh
./build_ros.sh

如果你的依赖项环境一切正常，这两个脚本无需任何额外操作即可编译成功。

2.2. ROS下启动D435相机

参考博客：

使用如下命令安装D435的ROS驱动

bash 复制代码

sudo apt-get install -y ros-melodic-rgbd-launch \
	ros-melodic-realsense2-camera \
	ros-melodic-realsense2-description

安装完毕驱动后，系统中会多出realsense相机的ROS启动文件，可以使用roscd realsense2_camera命令进入apt安装的ros realsense的工作空间，这里就有各种launch文件。

我们需要的是rs_rgbd.launch这个启动文件，以RGB-D模式启动我们的D435相机。

复制代码

king@ubuntu:~/slam$ roscd realsense2_camera
king@ubuntu:/opt/ros/melodic/share/realsense2_camera$ ls
cmake  launch  msg  nodelet_plugins.xml  package.xml  rviz  srv
king@ubuntu:/opt/ros/melodic/share/realsense2_camera$ ls launch/
demo_pointcloud.launch      rs_aligned_depth.launch           rs_from_file.launch         rs_t265.launch
demo_t265.launch            rs_camera.launch                  rs_multiple_devices.launch
includes                    rs_d400_and_t265.launch           rs_rgbd.launch
opensource_tracking.launch  rs_d435_camera_with_model.launch  rs_rtabmap.launch

注意检查一下rs_rgbd.launch启动文件中的下面这两个选项是否为true，如果不是，需要修改为true。

xml 复制代码

  <arg name="enable_sync"         default="true"/>
  <arg name="align_depth"         default="true"/>

这两个参数的前者是让不同传感器数据（Depth, RGB, IMU）实现时间同步，即具有相同的 timestamp；后者会增加若干 rostopic，其中我们比较关心的是 /camera/aligned_depth_to_color/image_raw这个主题，对应D435相机的深度图像数据。

确认启动文件配置无误后，用下面的roslaunch命令就可以启动D435相机了。执行启动命令之前，需要先在另外一个终端执行roscore命令。

bash 复制代码

roslaunch realsense2_camera rs_rgbd.launch

命令执行结果如下

使用rostopic list能看到D435相机发布的topic列表，如下所示。

bash 复制代码

[root:/]# rostopic list
/camera/align_to_color/parameter_descriptions
/camera/align_to_color/parameter_updates
/camera/aligned_depth_to_color/camera_info
/camera/aligned_depth_to_color/image_raw
/camera/aligned_depth_to_color/image_raw/compressed
/camera/aligned_depth_to_color/image_raw/compressed/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/compressed/parameter_updates
/camera/aligned_depth_to_color/image_raw/compressedDepth
/camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_updates
/camera/aligned_depth_to_color/image_raw/theora
/camera/aligned_depth_to_color/image_raw/theora/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/theora/parameter_updates
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions
/camera/color/image_raw/compressed/parameter_updates
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/compressedDepth/parameter_descriptions
/camera/color/image_raw/compressedDepth/parameter_updates
/camera/color/image_raw/theora
/camera/color/image_raw/theora/parameter_descriptions
/camera/color/image_raw/theora/parameter_updates
/camera/color/image_rect_color
/camera/color/image_rect_color/compressed
/camera/color/image_rect_color/compressed/parameter_descriptions
/camera/color/image_rect_color/compressed/parameter_updates
/camera/color/image_rect_color/compressedDepth
/camera/color/image_rect_color/compressedDepth/parameter_descriptions
/camera/color/image_rect_color/compressedDepth/parameter_updates
/camera/color/image_rect_color/theora
/camera/color/image_rect_color/theora/parameter_descriptions
/camera/color/image_rect_color/theora/parameter_updates
/camera/color/metadata
/camera/color_rectify_color/parameter_descriptions
/camera/color_rectify_color/parameter_updates
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/depth/image_rect_raw/compressed
/camera/depth/image_rect_raw/compressed/parameter_descriptions
/camera/depth/image_rect_raw/compressed/parameter_updates
/camera/depth/image_rect_raw/compressedDepth
/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/camera/depth/image_rect_raw/theora
/camera/depth/image_rect_raw/theora/parameter_descriptions
/camera/depth/image_rect_raw/theora/parameter_updates
/camera/depth/metadata
/camera/depth_registered/points
/camera/extrinsics/depth_to_color
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/diagnostics
/rosout
/rosout_agg
/tf
/tf_static
[root:/]#

其中我们需要的是/camera/color/image_raw 和 /camera/aligned_depth_to_color/image_raw 这两个topic，分别对应 RGB 图像和深度图像数据流。

2.3. ORB_SLAM2在ROS下订阅D435发布的topic

接下来需要创建一个相机内参文件（类似Examples/RGB-D/TUM1.yaml），填写D435相机的内外参数。内外参数最好的获取方式是通过ROS的camera_calibration工具，需要打印一个棋盘格标定板对D435相机进行标定。

本文不介绍如何标定D435相机，直接使用D435相机发布的相机信息的/camera/color/camera_info主题，通过如下命令获取D435相机的内外参数，

bash 复制代码

rostopic echo /camera/color/camera_info

该命令的输出结果如下所示

复制代码

---
header: 
  seq: 8477
  stamp: 
    secs: 1740896373
    nsecs: 113253355
  frame_id: "camera_color_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [605.8230590820312, 0.0, 323.6572570800781, 0.0, 604.4893798828125, 242.0369110107422, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [605.8230590820312, 0.0, 323.6572570800781, 0.0, 0.0, 604.4893798828125, 242.0369110107422, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

这个输出中：

K 是 相机内参矩阵 （fx, fy, cx, cy）
D 是 畸变系数 （ORB_SLAM2 只用前 4 个 k1, k2, p1, p2）
width 和 height 是相机拍摄的图像分辨率；

Camera.bf的计算公式如下，其中baseline是D435两颗摄像头之间的间距，官方的参数是50mm，将其和fx相乘就能得到bf。

b f = b a s e l i n e × f x = 0.05 × 605.8230590820312 ≈ 30.29 bf=baseline×fx=0.05×605.8230590820312≈30.29 bf=baseline×fx=0.05×605.8230590820312≈30.29

收集了这些数据后，参考ORB_SLAM2代码仓库中的Examples/ROS/ORB_SLAM2/Asus.yaml文件，把上述命令的结果中的参数写到文件Examples/RGB-D/RealSenseD435.yaml中。

最终我依照上述命令结果制作了如下yaml文件，每一个参数都取值都用注释标注出来了，没有中文注释的部分保持不变，不用修改。

yaml 复制代码

%YAML:1.0
#--------------------------------------------------------------------------------------------
# 通过`rostopic echo /camera/color/camera_info`获取并编写
#--------------------------------------------------------------------------------------------

# 相机内参 Camera Parameters
Camera.fx: 605.8230590820312  # K[0]
Camera.fy: 604.4893798828125  # K[4]
Camera.cx: 323.6572570800781  # K[2]
Camera.cy: 242.0369110107422  # K[5]

# 畸变参数 (D)
Camera.k1: 0.0  # D[0]
Camera.k2: 0.0  # D[1]
Camera.p1: 0.0  # D[2]
Camera.p2: 0.0  # D[3]
Camera.k3: 0.0  # D[4]

# 图像分辨率
Camera.width:640
Camera.height:480

# Camera frames per second 相机帧数
Camera.fps:30.0

# IR projector baseline times fx (aprox.)
# bf = baseline × fx = 0.05 × 605.8230590820312 ≈ 30.29
# baseline是d435两个摄像头的基线距离，为50mm
Camera.bf:30.29

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB:1

# Close/Far threshold. Baseline times.
ThDepth:40.0

# Deptmap values factor，将深度像素值转化为实际距离，原来单位是mm，转化成m
DepthMapFactor:1000.0

# 下面这部分都不需要修改，直接从Asus.yaml复制过来
#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

2.4. 运行ORB_SLAM2

相机参数文件准备好之后，就可以启动ORB_SLAM2了，先执行export命令设置一下ROS的环境变量

bash 复制代码

export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$PWD/Examples/ROS/ORB_SLAM2

启动命令如下，这里指定了我们刚刚自己制作的yaml文件，然后指定了两个topic的绑定

bash 复制代码

rosrun ORB_SLAM2 RGBD \
    Vocabulary/ORBvoc.txt \
    Examples/RGB-D/RealSenseD435.yaml \
    /camera/rgb/image_raw:=/camera/color/image_raw \
    /camera/depth_registered/image_raw:=/camera/aligned_depth_to_color/image_raw

命令中:=左侧为订阅的topic，右侧为输入的topic，ORB_SLAM2订阅的topic可以在Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc代码里面找到

cpp 复制代码

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 1);

一切正常的话，应该已经可以在ORB_SLAM2的GUI中看到D435相机拍摄到的画面了。

缓慢移动D435相机，可以在GUI中观察SLAM的追踪和建图结果。注意必须缓慢移动相机，过快移动相机会导致ORB_SLAM2直接丢跟踪（tracking lost）。

至此，在ORB_SLAM2中通过ROS使用D435相机的全步骤结束。

2.5. 可能遇到的问题

roslaunch启动D435相机的时候可能会直接报错，如下所示

复制代码

king@ubuntu:/opt/ros/melodic/share/realsense2_camera$ roslaunch realsense2_camera rs_rgbd.launch
... logging to /home/king/.ros/log/292703ba-f72c-11ef-a6ca-000c29839929/roslaunch-ubuntu-11850.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

Resource not found: rgbd_launch
ROS path [0]=/opt/ros/melodic/share/ros
ROS path [1]=/opt/ros/melodic/share
The traceback for the exception was written to the log file

这个问题是因为缺少安装一个ros的包，安装了之后就OK了，可以正常执行roslaunch命令了。

bash 复制代码

sudo apt-get install -y ros-melodic-rgbd-launch

前文的D435 ROS驱动安装命令中已经包含了这个软件包了。

3. ORB_SLAM2读取realsense-viewer录制的rosbag文件

D435相机的realsense-viewer软件是可以直接提前录制视频成rosbag格式的.bag文件的，这样能方便我们用同一个数据集测试SLAM系统，并以此改进SLAM算法。本质上和TUM数据集提供的rosbag格式文件没有什么区别。

在realsense-viewer中同时开启深度相机和RGB相机，点击record录制视频后，默认会存放在~/Documents文件夹下，找到录制的bag文件，使用rostopic -b 文件名的方式查看录制的bag文件中的topic列表，需要找到下面这两个主题，分别对应深度数据和彩色数据

复制代码

/device_0/sensor_1/Color_0/image/data
/device_0/sensor_0/Depth_0/image/data

修改ORB_SLAM2的启动命令如下，主要是订阅的主题不同。

bash 复制代码

rosrun ORB_SLAM2 RGBD \
    Vocabulary/ORBvoc.txt \
    Examples/RGB-D/RealSenseD435.yaml \
    /camera/rgb/image_raw:=/device_0/sensor_1/Color_0/image/data \
    /camera/depth_registered/image_raw:=/device_0/sensor_0/Depth_0/image/data

启动之后，使用rosplay发布bag文件中的topic即可，注意修改命令中的exmaple.bag为你录制的bag文件的路径

bash 复制代码

rosbag play exmaple.bag \
    --topics \
    /device_0/sensor_1/Color_0/image/data \
    /device_0/sensor_0/Depth_0/image/data

如图所示，ORB_SLAM2同样可以读取D435提前录制的视频。

4. The end

关于ORB_SLAM2和SLAM的专题博客到这里就基本结束啦，能记录的点都已经写成博客了，后续如果有其他的再继续更新吧。主要是希望能帮到其他SLAM初学者学会咋运行ORB_SLAM2。

为了毕设临时学了这么多东西，挺累人的说实话。