1 下载稠密点云工程
2 修改工程
2.1 增加RGB点云
2.2 增加点云保存功能
3 编译工程
3.1 普通模式
3.2 ROS模式
4 测试工程
4.1 普通模式
4.2 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 ×tamp) { 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播放，最终建图效果与普通模式相同。
