利用D435i采集数据并运行ORB-SLAM3以及全流程自动化

• 1.数据采集
  • 1.1 配置ROS节点启动文件
  • 1.2 启动相机ROS节点
  • 1.3 动态配置相机设置
  • 1.4 可视化数据
  • 1.5 录制数据
• 2.数据后处理
  • 2.1 导出影像数据
  • 2.2 导出IMU数据
  • 2.3 生成时间戳文件
• 3.运行SLAM
• 4.流程自动化

在之前的很多篇博客中，我们已经介绍了Intel Realsense D435i传感器的基本情况以及它提供的ROS接口，进一步介绍了利用其进行相机标定(Kalibr)、相机标定(ROS)、录制ROS Bag包、实时运行SLAM等内容。在这篇博客中，我们以利用D435i进行数据采集、数据后处理和跑SLAM为主线，对之前的内容进行整合，形成一个便于使用的脚本。博客主要内容分为以下几个部分：

• 数据采集
• 数据后处理
• 运行SLAM
• 流程自动化

1.数据采集

正如之前这篇博客中提到的，数据采集阶段主要可以分为以下几个步骤：

• Step1: 配置ROS节点启动文件(可选)
• Step2: 启动相机ROS节点
• Step3: 动态配置相机设置(可选)
• Step4: 可视化数据
• Step5: 录制数据

下面简单对每个步骤进行介绍。

1.1 配置ROS节点启动文件

D435i提供的ROS节点有对应的配置文件rs_camera.launch，里面有很多可以自定义的参数，可以根据需求进行设置。如果没有任何特殊的需求，则无需修改，默认配置就可以。比如D435i默认是关闭加速度计和陀螺仪的，如果需要打开，需要配置enable_accel和enable_gyro两个属性。关于启动文件里各参数的含义以及如何修改，可以参考这篇博客。完成之后就可以进入下一步了。

1.2 启动相机ROS节点

毫无疑问，要用ROS对D435i数据进行录制，至少要先启动个ROS节点向外发布数据。435i官方提供了ROS接口，所以我们直接安装使用即可。如果还没安装好，可以参考这篇博客进行配置。安装好以后，在终端输入：

roslaunch realsense2_camera rs_camera.launch

即可启动ROS节点。

1.3 动态配置相机设置

这是可选步骤，适合于对于传感器有特殊需求的场景。我们可以用rqt_reconfigure工具来配置一些传感器的“动态设置”，也就是在前面的启动文件中无法修改的参数。在终端中输入如下内容即可打开可视化界面进行配置。

rosrun rqt_reconfigure rqt_reconfigure

比如在这篇博客中提到的由于红外发射器而导致的红外散斑问题，可以通过动态配置红外发射器状态(On/Off/Auto)来解决。

1.4 可视化数据

既然要录制数据，可视化是必不可少的步骤之一，否则无法得知录制的数据质量是非常严重的问题。我们曾经就出现过录制了很久的数据，但没注意D435i的数据线“入境”的情况，导致整个数据作废，只能重录的惨剧。可视化可以通过rqt_image_view工具实现，在终端中输入如下内容即可。

# RGB相机可视化
rqt_image_view /camera/color/image_raw
# 左红外相机可视化
rqt_image_view /camera/infra1/image_rect_raw
# 右红外相机可视化
rqt_image_view /camera/infra2/image_rect_raw

可视化效果如这篇博客所示。

1.5 录制数据

数据录制采用ROS Bag工具，在终端中输入rosbag record+要录制的Topic名称即可。比如这里我们录制RGB相机、双目红外、IMU数据，对应如下内容。

rosbag record /camera/color/image_raw /camera/imu /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw

录制的界面如这篇博客所示。如果要结束录制，按Ctrl+C终止即可。

至此，数据采集的工作就完成了，你也应该会得到一个以时间命名(默认)的.bag文件。下一步就是对数据进行一些后处理。

2.数据后处理

这里所谓的后处理主要是指将ROS Bag文件解析成普通的图像文件，使得SLAM系统(如ORB-SLAM3)以非ROS模式运行。而如果以ROS模式运行，其实不需要做什么特别的后处理，唯一可能需要注意的是一些运行参数的配置，如录制的Topic名称代码中写的名称不同问题。以ORB-SLAM3为例，对于这个问题有两种解决办法。一种是修改代码，将识别的Topic名称改成我们录制的名称，可以参考这篇博客，另一种是利用ROS的topic_tools进行实时转发，转发成代码中写的名称，可以参考这篇博客。

对于数据后处理，主要包括以下几个步骤：

• Step1: 导出影像数据
• Step2: 导出IMU数据
• Step3: 生成时间戳文件

没有特殊说明，导出数据均遵循EuRoC数据集中的标准。

2.1 导出影像数据

顾名思义就是将ROS Bag文件中的影像流导出成序列影像文件，以时间戳为文件名，以便程序调用。具体而言，我们可以利用ROS提供的Python接口来访问Bag文件，然后逐帧解析图片数据并保存。为了更加方便，之前就已经编写好了parseBag.py脚本用于处理，可以参见Github项目，用法见项目的ReadMe文档。更多实现细节和介绍参考这篇博客。

2.2 导出IMU数据

和影像文件类似的，IMU数据也需要导出，导出的IMU数据格式如下(作为对比，我们也列出了TUM和KITTI的格式)：

• EuRoC IMU Format: timestamp(ns)、gyro_x(rad/s)、gyro_y(rad/s)、gyro_z(rad/s)、accel_x(m/s^2)、accel_y(m/s^2)、accel_z(m/s^2) EuRoC数据集侧重双目、IMU，无深度数据。

• TUM IMU Format: TUM数据集侧重RGBD影像，所以没有完整IMU数据，但有加速度计数据，格式为timestamp(sec)、accel_x(m/s)、accel_y(m/s)、accel_z(m/s)

• KITTI IMU Format：KITTI数据集侧重双目，无IMU数据。

为进一步方便比较，也列出EuRoC groundtruth的格式(TUM和KITTI在这篇博客中已经总结了，这里再列一下)：

• EuRoC Groundtruth Format: timestamp(ns)、position_x(m)、position_y(m)、position_z(m)、quaternion_w、quaternion_x、quaternion_y、quaternion_z、velocity_x(m/s)、velocity_y(m/s)、velocity_z(m/s)、rotation_vel_x(rad/s)、rotation_vel_y(rad/s)、rotation_vel_z(rad/s)、accel_x(m/s^2)、accel_y(m/s^2)、accel_z(m/s^2)

• TUM Groundtruth Format: timestamp(sec)、pos_x(m)、pos_y(m)、pos_z(m)、quat_x、quat_y、quat_z、quat_w。简单来说就是时间戳+平移+四元数。

• KITTI Groundtruth Format: R(0,0)、R(0,1)、R(0,2)、t(0)、R(1,0)、R(1,1)、R(1,2)、t(1)、R(2,0)、R(2,1)、R(2,2)、t(2)，R为旋转矩阵，t为平移向量(单位为m)，它们来自于变换矩阵T。没有直接写时间戳，每一行对应一张影像，每张影像有各自的时间戳，存放于times.txt文件中，时间单位为秒。

可以看到，EuRoC提供的真值内容是最丰富的(位置+姿态+线速度+角速度+线加速度)，TUM和KITTI都只提供了基本的位置+姿态。

回到我们的问题上来，和影像类似的，我们也写好了从ROS Bag中提取IMU数据的脚本convertIMU2csv.py，可以参见Github项目，用法见项目的ReadMe文档。

2.3 生成时间戳文件

经过上面两步操作，我们就可以得到解析出来的影像和IMU数据了。最后还需要生成一个时间戳索引文件，便于程序调用和读取。类似的，我们编写了genTimeStamps.py脚本，用于生成时间戳文件，可以参见Github项目，用法见项目的ReadMe文档。这里我们也简单总结一下EuRoC、TUM和KITTI的时间戳文件格式，如下：

• EuRoC Timestamp Format: timestamps.txt，直接就是时间戳数据(如1403637130538319104)，单位为纳秒(ns)。影像文件名即为其时间戳，格式为timestamp.png。

• TUM Timestamp Format: 无直接时间戳文件，每种数据类型有各自的时间戳和文件名对应关系。以RGB数据为例，有rgb.txt，第一行为名称(如# color images)，说明数据类型，第二行为ROS Bag名称(如# file: 'rgbd_dataset_freiburg2_desk.bag')，第三行为数据格式(如# timestamp filename)，之后为具体内容(如1311868164.363181 rgb/1311868164.363181.png)。包含时间戳(单位为秒)+对应文件路径。影像名即为时间戳，格式为timestamp.png。

• KITTI Timestamp Format: times.txt，直接就是时间戳数据，每个文件都从0开始递增，如1.037359e-01，单位为秒(s)。影像文件名非时间戳，而是一个6位的整数表示序号，格式为xxxxxx.png，如000003.png，与时间戳文件一一对应。

至此，我们就基本完成了采集数据的后处理部分。

3.运行SLAM

通过上面的处理，我们就可以得到用于SLAM的数据序列了。还是以ORB-SLAM3为例，介绍运行双目+IMU的模式(非ROS，ROS模式参考这篇博客和这篇博客)。对于双目+IMU我们需要传入如下参数：

• ORB字典文件路径
• 配置文件路径
• 序列影像文件夹路径
• 时间戳文件路径

依照自己的实际情况，依次填好对应部分即可。对于字典文件，用ORB-SLAM3提供的默认就好。对于配置文件，由于是自己采集的数据，所以需要根据自己的实际情况标定相机，然后参考EuRoC.yaml进行修改。相机标定相关内容可以参考这篇博客和这篇博客。最后，对于影像文件夹和时间戳文件路径，需要稍微注意一下。在ORB-SLAM3中，是把影像路径文件夹和影像类型“写死”了，如下所示。 如果你的情况和代码中不一样，那就需要修改一下：或者是改代码，或者是改你的文件路径和类型。

一些准备完成以后，就可以在终端输入命令，直接运行程序了，比如如下。

./stereo_inertial_pcl ../../Vocabulary/ORBvoc.txt /root/Datasets/PCL/Calibration/PCL-infra-stereo-inertial.yaml /root/Datasets/2021-11-15/2021-11-15-22-24-31 /root/Datasets/2021-11-15/2021-11-15-22-24-31/camera-infra1-image_rect_raw/timestamps.txt

至此从数据采集、数据后处理到SLAM运行都已经介绍完了。

4.流程自动化

最后可以看到，第一部分(数据采集)和第二部分(数据后处理)其实有很多步骤，输入很多的命令，有时候可能一不小心就输错了，或者忘记了，十分不方便。我曾经也干过输错了Topic名称，然后导致本以为录好了，结束之后才发现根本没录上的情况。所以应该实现自动化。

对于第一部分，编写了d435i_stage1_data_collection.py脚本用于自动化。对于第二部分，编写了d435i_stage2_postprocessing.py脚本用于自动化。涵盖了上面提到的所有步骤，使用起来十分方便。脚本详见Github项目，用法见项目的ReadMe文档。欢迎Star或Fork，如果在使用中有任何问题，也欢迎提Issue。