ORB SLAM 2 + 构建点云地图复现

引言

上一篇文章我们尝试复现了基本的 ORB SLAM2，其中构建的地图为稀疏的特征点地图. 这篇文章中，我们尝试复现高翔博士关于ORB SLAM2 + 稠密的点云地图的工作（https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map）。

高博的工作是对基本 ORB SLAM2 的扩展，基本思想是为每个关键帧构造相应的点云，然后依据从 ORB SLAM2 中获取的关键帧位置信息将所有的点云拼接起来，形成一个全局点云地图。

高博关于该工作的 github 上的介绍比较简洁，如果读者不熟悉基本的 ORB SLAM2 的复现流程，那么在复现高博的工作时也很可能会遇到一些问题。在这篇文章中，我们将尽可能详细的介绍整个复现流程以及遇到的问题和解决方案。

准备工作

操作系统： Ubuntu 18.04

由于是在 ORB SLAM2 基础上的扩展，原本 ORB SLAM2 需要的那些依赖这里仍然是需要的。
具体的依赖包括：

OpenCV (推荐 3.2 版本)
DBoW2 和 g2o（源文件已经包含在了 github repo 中，随后一起编译即可，这里先不管）
ROS （推荐 melodic）
PCL：由于添加了点云相关的操作，需要安装 PCL 库文件
```
sudo apt install libpcl-dev
```
Eigen3（推荐 3.2 版本）
Pangolin

以上依赖项的安装可以参考我们前一篇文章.

编译

从高博的 github repo 下载源文件

git clone https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map.git

这个 repo 的名字有点长，而且我们只需要其中的子文件夹 ORB_SLAM2_modified，为了方便后续操作，可以将子文件夹 ORB_SLAM2_modified 单独拿出来，放到用户 home（～）路径下。

原本的 ORB SLAM2 中的 Vocabulary 文件夹比较大，高博没有将它放入自己的 repo 中，因此我们还要做些补充工作，把原本 ORB SLAM2 repo 中的 Vocabulary 文件夹和它里面的 ORBvoc.txt.tar.gz 文件拷贝过来，放到 ORB_SLAM2_modified 路径下。

做完以上工作就可以编译了。先把 ~/ORB_SLAM2_modified/build, ~/ORB_SLAM2_modified/Thirdparty/DBoW2/build 和 ~/ORB_SLAM2_modified/Thirdparty/g2o/build 这三个 build 文件夹删掉，然后运行 ORB_SLAM2_modified 中的 build.sh。

如果要以 ROS 的形式运行，还需要对 repo 中的 ROS 文件进行编译。这里也要做一些额外设置。
首先是将 ROS 文件所在路径加入到 ROS_PACKAGE_PATH 环境变量中。
具体操作是在 ~/.bashrc 文件末尾加入以下语句：

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/ORB_SLAM2_modified/Examples/ROS

可以新开一个命令行窗口或者在当前窗口中运行 source ~/.bashrc，检查一下 ROS_PACKAGE_PATH 环境变量是否更新：

echo $ROS_PACKAGE_PATH

应该显示

/opt/ros/melodic/share:/home/<USER_NAME>/ORB_SLAM2_modified/Examples/ROS

第二个设置是修改 ROS 文件夹中的 CMakeLists.txt 文件。高博只修改了 ~/ORB_SLAM2_modified/CMakeLists.txt 文件，加入了 PCL 相关的设置，而没有修改 ROS 文件夹中的 CMakeLists.txt 文件。这样普通编译（运行 build.sh 文件）是没有问题的，但是编译 ROS 文件（运行 build_ros.sh）就会报错。我们需要做的就是参照 ~/ORB_SLAM2_modified/CMakeLists.txt 文件，把 PCL 相关的设置添加到 ~/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt 文件中。
具体要修改的 5 个位置如下：

...
find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
find_package( PCL 1.7 REQUIRED )    ####### 1

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}  ####### 2
)

add_definitions( ${PCL_DEFINITIONS} )   ####### 3
link_directories( ${PCL_LIBRARY_DIRS} ) ####### 4

set(LIBS 
${OpenCV_LIBS} 
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
${PCL_LIBRARIES} ####### 5
)
...

做完以上修改之后，再删掉 ~/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/build 文件夹，就可以运行 build_ros.sh 编译 ROS 文件了。

运行

这里我们使用的是 TUM 数据集中比较简单的 fr1/xyz 以及相应的 rosbag (https://vision.in.tum.de/data/datasets/rgbd-dataset/download)，为了方便管理，下载之后都放在新建的~/ORB_SLAM2_modified/datasets 目录中。

普通模式

在使用数据之前，需要对 rgbd_dataset_freiburg1_xyz 中的 RGB 文件和 Depth 文件进行匹配。TUM 数据官网提供了匹配的程序 associate.py，命令为：

python associate.py rgb.txt depth.txt  > association.txt

然后，在 ~/ORB_SLAM2_modified 文件夹下运行如下格式的命令：

./bin/rgbd_tum Vocabulary/ORBvoc.bin path_to_settings path_to_sequence path_to_association

针对我们的情况，具体命令为：

./bin/rgbd_tum Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1.yaml datasets/rgbd_dataset_freiburg1_xyz  datasets/rgbd_dataset_freiburg1_xyz/associations.txt

结果如下：

rgbd-tum.png

上述显示中，点云地图是黑白的，视觉效果不太好，在高博的 github issue 中有建立彩色点云地图的方法https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map/issues/10#issuecomment-309663764，按照步骤修改 ~/ORB_SLAM2_modified/include/Tracking.h 和 ~/ORB_SLAM2_modified/src/Tracking.cc，重新编译系统

cd ~/ORB_SLAM2_modified/build

make -j8

然后再次运行前述 SLAM 命令，可以得到以下的彩色点云地图

color.png

ROS 模式

在运行 ROS 之前，我们还需要对参数做些修改。在 Examples/RGB-D/TUM1.yaml 中保存了相机的各种参数，其中有一个参数名为 DepthMapFactor。TUM 数据官网中提到 “factor = 5000 # for the 16-bit PNG files，OR: factor = 1 # for the 32-bit float images in the ROS bag files”。因此，在运行 ROS 之前需要将这个参数改为 DepthMapFactor: 1.0.

在具体操作时，我们复制了 TUM1.yaml 文件，命名为 TUM1_ROS.yaml，修改其中的参数 DepthMapFactor: 1.0。修改之后，运行以下命令：

roscore

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1_ROS.yaml

rosbag play datasets/rgbd_dataset_freiburg1_xyz.bag /camera/rgb/image_color:=/camera/rgb/image_raw /camera/depth/image:=/camera/depth_registered/image_raw

可以得到与上述普通模式相同的建图效果。

保存地图

高博的程序只能实时查看点云地图，不能保存。我们可以稍微修改一下文件 ~/ORB_SLAM2_modified/src/pointcloudmapping.cc，在其中调用 PCL 库的 pcl::io::savePCDFileBinary 函数就可以保存点云地图了。

具体修改如下：

加入头文件

#include <pcl/io/pcd_io.h>

在 void PointCloudMapping::viewer() 函数中（ 123 行附近）加入保存地图的命令，最后样式如下：

...
for ( size_t i=lastKeyframeSize; i<N ; i++ )
{
    PointCloud::Ptr p = generatePointCloud( keyframes[i], colorImgs[i], depthImgs[i] );
    *globalMap += *p;
}
pcl::io::savePCDFileBinary("vslam.pcd", *globalMap);   // 只需要加入这一句
...

修改之后重新编译程序

cd ~/ORB_SLAM2_modified/build

make -j8

然后再运行前述的各个 SLAM 命令就可以在 ~/ORB_SLAM2_modified 路径下产生一个名为 vslam.pcd 的点云文件。

为了查看这个 pcd 文件，我们需要安装相应的工具

sudo apt-get install pcl-tools

然后通过 pcl_viewer 查看

pcl_viewer vslam.pcd

pcl_map.png

总结

本文详细记录了 ORB SLAM 2 + 点云建图的复现过程，以及其中可能存在的一些问题和解决方案。

在接下来的工作中，我们将尝试用深度相机 Intel RealSense D435i 实现 ORB SLAM 2 + 点云建图.

ORB SLAM 2 + 构建点云地图 复现

引言

准备工作

编译

运行

普通模式

ROS 模式

保存地图

总结

ORB SLAM 2 + 构建点云地图复现