Ubuntu20.04下ORB-SLAM2稠密建图+octomap生成八叉树地图（上）
前言
最近用orb-slam2跑了一下自己的数据集，由于是稀疏点云建图，效果并不是很理想，看到别人建的稠密点云地图效果还可以，就想着看看效果，参考了网上很多大佬的博客，也踩了不少坑，为避免遗忘，索性记录下来。
所用版本
这里用的是高博修改的版本ORBSLAM2_with_pointcloud_map，系统为Ubuntu20.04，Ubuntu18.04也大差不差，网上也有很多资料。
准备工作
虽然是高博修改的版本，但是orb-slam2所需要的一些依赖项还是要安装的，已经成功编译过orb-slam2可以跳过这一步直接进入正文稠密建图。
安装依赖项
sudo apt-get update
sudo apt-get install git gcc g++ vim make cmake
安装Pangolin
我这里安装Pangolin 0.6,下载链接。下载完是一个压缩包，记得解压一下。
（1）安装依赖项
sudo apt-get install libglew-dev libboost-dev libboost-thread-dev libboost-filesystem-dev
sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev libpng-dev
（2）编译
进入解压后的Pangolin文件
cd Pangolin
mkdir build && cd build
cmake -DCPP11_NO_BOOST=1 ..
make
sudo make install
（3）安装成功后执行下面，会出现下面窗口就是成功了
cd ../examples/HelloPangolin
mkdir build && cd build
cmake ..
make
./HelloPangolin
安装OpenCV3
我安装的是OpenCV-3.4.15，很顺利没什么问题。网上也有很多人的用的其他版本，如果之前安装过可以输入下面命令查询。
pkg-config --modversion opencv
这是opencv官网地址，找到对应的版本，点击点击 Sources 进行下载，得到 opencv-3.4.15.zip 压缩文件。
打开 opencv-3.4.15.zip所在文件夹，执行下面命令解压后放到合适的地方
unzip opencv-3.4.15.zip
安装依赖项
sudo add-apt-repository "deb http://security.ubuntu.com/ubuntu xenial-security main"
sudo apt update
sudo apt-get install build-essential libgtk2.0-dev libavcodec-dev
sudo apt-get install libavformat-dev libjpeg.dev libtiff5.dev
sudo apt-get install libswscale-dev libjasper-dev
sudo apt-get install libcanberra-gtk-module libcanberra-gtk3-module
#或者
sudo apt-get install libcanberra-gtk*
配置并编译
cd opencv-3.4.15
mkdir build && cd build
cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..
#电脑性能差可去掉-j4，性能很好可增加数字（线程）
make -j4
sudo make install
查询OpenCV版本、库以及头文件目录的三个命令来确保上面的OpenCV安装步骤都正常
pkg-config --modversion opencv
pkg-config --cflags opencv
pkg-config --libs opencv
安装Eigen3
直接安装：
sudo apt-get install libeigen3-dev
建议源码安装
cd eigen3
mkdir build && cd build
cmake ..
make
sudo make install
安装后头文件在：
/usr/local/include/eigen3/
复制头文件到/usr/local/include：
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include
安装orb-slam2
下载zip文件后解压，执行
注意：出现了对应的错误则修改，如果没有出现则不修改。每次改动后将 ORB_SLAM2 文件夹下面的 build 文件夹删掉，还有 ORB_SLAM2/Thirdparty/DBoW2 && g2o 文件夹下面的 build 文件夹删掉，之后再执行上面的指令重新编译看看是否有错
cd ORB_SLAM2
chmod +x build.sh
./build.sh
编译时如果有错误1
error: ’usleep’ was not declared in this scope
根据终端报错显示的文件位置去修改，找到对应的头文件加上
#include
编译时如果有错误2
error: static assertion failed: std::map must have the same value_type as its allocator
把ORB-SLAM2源码目录中include/LoopClosing.h文件中的
typedef map,
Eigen::aligned_allocator > > KeyFrameAndPose;
修改为：
typedef map,
Eigen::aligned_allocator > > KeyFrameAndPose;
到这里应该就可以成功编译了，然后进入正题。
稠密建图
如果之前成功编译过orb-slam2，下面的工作会顺利很多。
1.新建工作空间
#orb_ws是工作空间名，可以自行替换
mkdir -p orb_ws/src
cd orb_ws
catkin_make设置环境变量，在orb_ws目录下打开一个终端
echo "source ~/orb_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc可以在主目录下ctrl+h看到隐藏文件.bashrc中最下面多了工作空间的环境变量
2.ORBSLAM2_with_pointcloud_map源码
cd orb_ws/src/
git clone https://gitcode.com/gaoxiang12/ORBSLAM2_with_pointcloud_map.git删除ORB_SLAM2_modified/Thirdparty/DBoW2/build、ORB_SLAM2_modified/Thirdparty/g2o/build以及ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/build这3个 build 文件夹。
将 之前下载好的ORB_SLAM2下的Vocabulary子文件夹复制粘贴到ORB_SLAM2_modified 文件夹下。
在ORB_SLAM2_modified文件下的CMakeLists.txt中取消编译器的一些设置。
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
在 Thirdparty/g2o/CMakeLists.txt 中删除 -march=native
# Compiler specific options for gcc
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native")
SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native")
3.编译
cd ORB_SLAM2_modified
chmod +x build.sh
./build.sh编译时可能会出现的错误1：
#报错
/usr/include/pcl-1.10/pcl/pcl_config.h:7:4: error: #error PCL requires C++14 or above
7 | #error PCL requires C++14 or above表明正在使用的点云库 (PCL) 需要至少 C++14 的 C++ 标准版本才能编译，可能使用较旧的 C++ 标准，从而导致此问题。
修改/ORB_SLAM2_modified/CMakeLists.txt文件-std=c++11换成-std=c++14
编译时可能会出现的错误2：
/usr/include/c++/9/bits/stl_map.h:122:71: error: static assertion failed: std::map must have the same value_type as its allocator解决办法： 打开include文件下的LoopClosing.h
typedef map,
Eigen::aligned_allocator > > KeyFrameAndPose; 替换为：
typedef map,
Eigen::aligned_allocator > > KeyFrameAndPose; 编译时可能会出现的错误3：
home/cgm/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/Monocular/mono_tum.cc:81:22: error: ‘std::chrono::monotonic_clock’ has not been declared
81 | std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();根据报错的文件位置来修改：
将代码中所有使用 `std::chrono::monotonic_clock` 的地方替换为 `std::chrono::steady_clock`。
不出意外，到这里就编译成功辣！
然后继续编译ros节点，执行：
./build_ros.sh可能会出现下面报错
报错
[rosbuild] rospack found package "ORB_SLAM2" at "/home/fast-drone/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2", but the current directory is原因：之前编译过orb-slam2，所以在编译高博的这版ROS_PACKAGE_PATH会冲突，按下面步骤修改一下就好了。
解决办法：
将ORB_SLAM2_modified/Examples/ROS/的ORB_SLAM2改名为ORB_SLAM2_1，如果之前没有编译过orb-slam2不用做这一步。
然后在~/.bashrc最后面设置路径（注意这个path是我的路径，读者需要自己修改）：
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/fast-drone/orb_ws/src/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/ROS
source ~/.bashrc然后进入/opt/ros/noetic/目录下会有一个文件setup.bash，在当前目录下打开一个终端执行：
sudo vim setup.bash在setup.bash最下面添加
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/fast-drone/orb_ws/src/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/ROS
然后source setup.bash刷新一下，再开一个终端执行：
echo $ROS_PACKAGE_PATH查看终端显示是否为自己刚才修改的路径
然后继续编译ros节点
cd ORB_SLAM2_modified
chmod +x build_ros.sh
./build_ros.sh编译成功后下面进行数据集的工作
4.数据集
由于之前git clone高博的这版文件中并没有tum数据集，所以还需要用到原版orb-slam2中的官网数据集，当然有自己的数据集也可以用。
在主目录下新建一个darren_file文件，将orb-slam2的data文件中的rgbd_dataset_freiburg1_xyz复制过来。
我的路径是：
/home/fast-drone/darren_file/rgbd_dataset_freiburg1_xyz
再将ORB_SLAM2_modified/Examples/RGB-D 里所有文件复制到/home/fast-drone/ORB_SLAM2/data/rgbd_dataset_freiburg1_xyz 重名文件直接替换。（这里根据自己的路径修改）
在rgbd_dataset_freiburg1_xyz里开一个终端输入：
python associate.py rgb.txt depth.txt > association.txt成功的话会生成一个association.txt
可能的报错：
如果没有生成或者是空白文件，看看是不是这个报错
File "associate.py", line 119, in matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) 解决办法：python2和python3语法不一样，需要将associate.py中
first_keys = first_list.keys()
second_keys = second_list.keys()换成:
first_keys = list(first_list.keys())
second_keys = list(second_list.keys())然后再重新执行python associate.py rgb.txt depth.txt > association.txt
问题就解决辣！
下面继续跑数据集
进入ORB_SLAM2_modified输入
执行（里面的路径修改成自己的）：
./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml /home/fast-drone/darren_file/rgbd_dataset_freiburg1_xyz /home/fast-drone/darren_file/rgbd_dataset_freiburg1_xyz/association.txt运行成功会出现黑白的点云图，而且还不能保存地图，继续修改。
彩色点云地图
在ORB_SLAM2_modified/include/Tracking.h添加
// Current Frame
Frame mCurrentFrame;
cv::Mat mImRGB; //添加这行
cv::Mat mImGray;
cv::Mat mImDepth;在ORB_SLAM2_modified/src/Tracking.cc处修改两处
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp)
{
mImRGB = imRGB;//添加这行
mImGray = imRGB;
mImDepth = imD;
// insert Key Frame into point cloud viewer
//mpPointCloudMapping->insertKeyFrame( pKF, this->mImGray, this->mImDepth );
mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth ); //修改地方
保存彩色点云地图
修改ORB_SLAM2_modified/src/pointcloudmapping.cc，在其中调用 PCL 库的pcl::io::savePCDFileBinary函数就可以保存点云地图了，修改如下：
加入头文件
#include
在 void PointCloudMapping::viewer() 函数中（大概在最后面的那部分代码）加入保存地图的命令，最后样式如下：
...
for ( size_t i=lastKeyframeSize; i重新编译
进入ORB_SLAM2_modified目录下重新编译（把build文件删除了）
cd ORB_SLAM2_modified
chmod +x build.sh
./build.sh
#编译ros节点
./build_ros.shORB_SLAM2_modified目录下重开一个终端运行
#路径改成自己的
./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml /home/fast-drone/darren_file/rgbd_dataset_freiburg1_xyz /home/fast-drone/darren_file/rgbd_dataset_freiburg1_xyz/association.txt此时可以看到下面的彩色点云图
终端ctrl+c中断后，在ORB_SLAM2_modified目录下可以看到一个vslam.pcd文件
在该目录下开一个终端，查看保存的点云地图：
#如果没有安装pcl，执行下面
sudo apt-get install pcl-tools
#查看点云
pcl_viewer vslam.pcd得到保存好的彩色点云地图，后面就可以用 octomap生成八叉树地图进行路径规划了。
先休息下，剩下的八叉树地图后面再写吧 。
