引言
上一篇文章我们尝试复现了基本的 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 + 点云建图.
网友评论