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 <unistd.h>

编译时如果有错误2

error: static assertion failed: std::map must have the same value_type as its allocator

把ORB-SLAM2源码目录中include/LoopClosing.h文件中的

typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;

修改为：

typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<KeyFrame *const, g2o::Sim3> > > 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<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;

替换为：

typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<KeyFrame *const, g2o::Sim3> > > 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 <module>     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 &timestamp)
{
    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 <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);   // 只需要加入这一句
...

 重新编译

进入ORB_SLAM2_modified目录下重新编译（把build文件删除了）

cd ORB_SLAM2_modified
chmod +x build.sh
./build.sh
#编译ros节点
./build_ros.sh

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

此时可以看到下面的彩色点云图

终端ctrl+c中断后，在ORB_SLAM2_modified目录下可以看到一个vslam.pcd文件

 

 在该目录下开一个终端，查看保存的点云地图：

#如果没有安装pcl，执行下面
sudo apt-get install pcl-tools
#查看点云
pcl_viewer vslam.pcd

得到保存好的彩色点云地图，后面就可以用 octomap生成八叉树地图进行路径规划了。

先休息下，剩下的八叉树地图后面再写吧 。