从零开始Ubuntu16.04+ORBSLAM2+ROS实验实录(五)：稠密点云建图

前文：
从零开始Ubuntu16.04+ORBSLAM2+ROS实验实录(一)：安装与配置
从零开始Ubuntu16.04+ORBSLAM2+ROS实验实录(二)：相机测试与标定
从零开始Ubuntu16.04+ORBSLAM2+ROS实验实录(三)：使用USB相机运行ORBSLAM
从零开始Ubuntu16.04+ORBSLAM2+ROS实验实录(四)：ORBSLAM评估工具EVO的使用

特别感谢：https://www.jianshu.com/p/5e7b8358893f提供的指导

1 下载稠密点云工程

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

将原 ORB SLAM2 中的 Vocabulary及其内ORBvoc.txt.tar.gz拷贝到pointCloud的ORB_SLAM2_modified 文件夹下，删除ORB_SLAM2_modified/Thirdparty/DBoW2/build 和 ORB_SLAM2_modified/Thirdparty/g2o/build

2 修改工程

2.1 增加RGB点云

若不进行修改，生成的稠密点云只有灰度，修改步骤如下：
1.在Tracking.h中声明RGB矩阵

Frame mCurrentFrame;
cv::Mat mImRGB;	//new declared
cv::Mat mImGray;

2.在Tracking.cc中定义RGB矩阵

cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp)
{
    
    
mImRGB = imRGB;		// new
mImGray = imRGB;
......

3.在Tracking.cc中定义RGB点云映射

mpPointCloudMapping->insertKeyFrame( pKF, this->mImGray, this->mImDepth );//change the mImGray to mImRGB as next row
mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth );// new

2.2 增加点云保存功能

1.修改pcl

sudo gedit ORB_SLAM2_modified/src/pointcloudmapping.cc

加入头文件

#include <pcl/io/pcd_io.h>

在void PointCloudMapping::viewer() 函数中加入保存地图的命令：

...
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);   // new
...

2.查看pcd点云文件

sudo apt-get install pcl-tools
pcl_viewer vslam.pcd

3 编译工程

3.1 普通模式

cd ORB_SLAM2_modified
./build.sh

3.2 ROS模式

1.配置ROS路径

sudo vi ~/.bashrc
export ROS_PACKAGE_PATH=${
    
    ROS_PACKAGE_PATH}:~/Project/SLAM/ORBSLAM2/ORB_SLAM2/pointCloud/ORB_SLAM2_modified/Examples/ROS
source ~/.bashrc

2.修改ROS CMakeLists.txt
即参照 ORB_SLAM2_modified/CMakeLists.txt 文件，把 PCL 相关的设置添加到 ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt 文件中。

具体而言，如下：

...
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
)
...

4 测试工程

4.1 普通模式

首先对 rgbd_dataset_freiburg1_xyz 中的 RGB 文件和 Depth 文件进行匹配：

cd ~/Project/SLAM/ORBSLAM2/ORB_SLAM2/pointCloud/ORB_SLAM2_modified/dataSet/rgbd_dataset_freiburg1_xyz
python associate.py rgb.txt depth.txt  > association.txt

然后即可运行

cd /ORB_SLAM2_modified
./bin/rgbd_tum ./Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1.yaml ./dataSet/rgbd_dataset_freiburg1_xyz  ./dataSet/rgbd_dataset_freiburg1_xyz/association.txt

结果如下：

4.2 ROS模式

运行ROS前，要更改相机参数文件如下

// 终端A
roscore
// 终端B
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1_ROS.yaml 
// 终端C
rosbag play --pause rgbd_dataset_freiburg1_xyz
.bag /camera/rgb/image_color:=/camera/rgb/image_raw /camera/depth/image:=/camera/depth_registered/image_raw

其中TUM1_ROS.yaml即为上述改过相机参数的标签文件；按空格键可以启动bag播放，最终建图效果与普通模式相同。