ROS2 基于ORB_SLAM3的单目相机2D格栅实时地图

环境： Ubuntu20.04 ROS-Foxy
SLAM3版本： https://github.com/electech6/ORB_SLAM3_detailed_comments
参考带有2D格栅地图SLAM(ROS1)版本：
https://github.com/abhineet123/ORB_SLAM2
https://github.com/MrPicklesGG/ORB_SLAM3_Grid_Mapping

一、环境搭建

因涉及PCL，先搭建环境
根据Ubuntu20.04选择VTK8.2.0、PCL1.10.1版本
参考1 参考2

1.VTK编译安装

(1)库依赖

#X11
sudo apt-get install libx11-dev libxext-dev libxtst-dev libxrender-dev libxmu-dev libxmuu-dev
#OpenGL
sudo apt-get install build-essential libgl1-mesa-dev libglu1-mesa-dev
#cmake && cmake-gui
sudo apt-get install cmake cmake-gui
sudo apt install -y libxt-dev
sudo apt install -y cmake-qt-gui
sudo apt install build-essential cmake

如果你安装没有错误请跳过此部分
（在安装cmake-gui cmake-qt-gui报错
根据错误安装

sudo apt-get install cmake-data = 3.16.3-1ubuntu1

在尝试update和更换下载源都无法解决该问题，决定下载源码包安装，网址：
http://archive.ubuntu.com/ubuntu/pool/main/c/cmake/cmake_3.16.3-1ubuntu1_amd64.deb

chmod +x cmake_3.16.3-1ubuntu1_amd64.deb
sudo dpkg -i cmake_3.16.3-1ubuntu1_amd64.deb

(这里我找了很多方法依次尝试，总结出解决方法)http://archive.ubuntu.com/ubuntu/pool/main/c/cmake/cmake_3.16.3-1ubuntu1.20.04.1_amd64.deb下载，在终端安装

sudo dpkg -i cmake_3.16.3-1ubuntu1.20.04.1_amd64.deb

(2)编译安装
再根据https://blog.csdn.net/weixin_54470372/article/details/127522413?spm=1001.2014.3001.5506安装cmake-gui
剩下的按照https://blog.csdn.net/ljn1046016768/article/details/131867002?spm=1001.2014.3001.5506从VTK编译和安装的第三点开始到第四点结束，中间如果在点击configure后，勾选“VTK-Group-Qt”，点击“Configure”时报错，参考https://blog.csdn.net/danxibaoxxx/article/details/104251311?spm=1001.2014.3001.5506

2.PCL编译安装

(1)下载
https://github.com/PointCloudLibrary/pcl
我选择的版本是1.10.1

(2)编译安装
在解压后的包中进入终端

mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=None ..
make (如果速度太慢用 make -j8 ;默认使用单核make，-j8启用8核)
sudo make install

(3)测试
参考：

https://blog.csdn.net/ljn1046016768/article/details/131867002?spm=1001.2014.3001.5506

二、ROS2 ORB_SLAM3格栅实时地图

下载ros1版本的带格栅实时地图的ORB_SLAM3模型:

https://github.com/MrPicklesGG/ORB_SLAM3_Grid_Mapping

下载ORB_SLAM3及ros2版本相机启动程序：

https://github.com/electech6/ORB_SLAM3_detailed_comments
https://github.com/zang09/ORB_SLAM3_ROS2

都进行解压，ORB_SLAM3_detailed_comments重新命名为ORB_SLAM3，将ORB_SLAM3_ROS2包放进ORB_SLAM3中

1. 2D格栅地图

(1) 在ORB_SLAM3/ROS2_ORB_SLAM3/src/orbslam3_ros2/src中创建monomap文件夹，并新建ros2_mono_pub.cpp和ros2_mono_sub.cpp两个文件
(2) 打开ORB_SLAM3_Grid_Mapping/Examples/ROS/ORB_SLAM3/src中的ros_mono_pub.cc和ros_mono_sub.cc
(3) 将ROS1的2D格栅单目实时地图ros_mono_pub.cc和ros_mono_sub.cc
移植成ROS2的ros2_mono_pub.cpp和ros2_mono_sub.cpp。
这里ROS1移植到ROS2没什么好说的，网上一大堆语法详细说明，
主要注意一点原来ros_mono_pub.cc中的void publish函数需要修改写到ros2_mono_pub.cpp的相机图像回调所在的类中，且将位姿Tcw作为函数的输入参数，并修改相关代码。

(4) 参考ORB_SLAM3_Grid_Mapping在ORB_SLAM3的

1. Frame.h、Frame.cc、Tracking.cc相应位置添加is_keyframe相关代码
2. Tracking.cc、Tracking.h、LoopClosing.h、LoopClosing.cc相应位置添加loop_detected相关代码
3. System.h相应位置添加

    Atlas* getMap() {
        return mpAtlas;
    }
    Tracking* getTracker(){ return mpTracker; }
    LocalMapping* getLocalMapping(){ return mpLocalMapper; }
    LoopClosing* getLoopClosing(){ return mpLoopCloser; }

(5) 参考ORB_SLAM3_Grid_Mapping/Examples/ROS/ORB_SLAM3/CMakeLists.txt修改ORB_SLAM3/ROS2_ORB_SLAM3/src/orbslam3_ros2/CMakeLists.txt，主要修改部分如下
添加PCL：
添加2D格栅地图运行节点：

2.编译运行

(1) 编译不带ROS2的ORB_SLAM3
在ORB_SLAM3文件夹中进入终端：

chmod +x build.sh
./build.sh

回到主目录下，按Ctrl+H，修改.bashrc文件，将下面内容添加到文件最后（其中lib路径根据你自己的路径来填写）

export LD_LIBRARY_PATH=~/ORB_SLAM3/lib:$LD_LIBRARY_PATH

刷新.bashrc文件，终端：source ~/.bashrc

(2) 编译带2D格栅的ORB_SLAM3_ROS2包
修改FindORB_SLAM3.cmake
ROS2_ORB_SLAM3/src/orbslam3_ros2/CMakeModules/FindORB_SLAM3.cmake
修改第8行，改为之前非ROS的ORB_SLAM3的安装路径
在ROS2_ORB_SLAM3中进入终端：colcon build

(3) 因为要启动多个终端，懒得一个个创建，所以写了个mono_map.sh文件放在ORB_SLAM3文件夹中
在该文件所在的位置进入终端：

chmod +x mono_map.sh
./mono_map.sh

2D格栅图的显示有点瑕疵，改改就行，问题不大。

最后注意：
在运行ros2 run orbslam3 Monosub 30 5 2 -2 2 -2 0.55 1 5时，要根据自己需求修改相关数值，具体这些数值是什么意思在https://github.com/abhineet123/ORB_SLAM2的问答里面作者有解答，这里直接做个表格搬运过来方便查看。

ros2 run ORB_SLAM3 Monosub 5 3 29 -25 48 -12 0.55 0.50 1 5
scale_factor:5
resize_factor:3
cloud_max:29 ,48
cloud_min:-25 ,-12
free_thresh:0.55
occupied_thresh:0.5
use_local_counters:1
visit_thresh:5

scale_factor	since it is monocular, information about scale is not provided orbslam. This is the variable that you can adjust wrt to the scale of the real points! In other words, it is theactual scale you convert from orbslam units to meter.
resize_factor	is just a resizing of the final 2d grid map image! (it won’t affect the 2d mapping process. you can use it when the map is so big for your screen to be shown)
cloud_max, cloud_min	was the min and max height of the 3d point cloud! This is to find out the size of the grid map !
free_thresh, occupied_thresh	This method is based on gmapping idea. for each cell we have the ration of how many times a cell has been seen as an occupied cell over how many times it has been seen. a cell is considered free cell, if that ratio is below the free_threshold and it is considered occupied if the ratio is more than occupied_thresh and if it is between it will be considered as an unknown(not descided yet).
use_local_counters	There is an issue in projecting 3D points into 2d plane which was discussed in section 3.2.2 of report. it is better to use this approach.
visit_thresh	since the points are noisy in orbslam and some of them are being removed in latter frames we used this theshold not to account for cells that has low visiting times. In order to decide if a cell is occupied or not no matter what is the ratio, it needs to be seen as many as ‘visit_thesh’ to take into consideration.
grid_min, grid_max	will be calculated usingmax point cloud values in your 3d point clouds! It basically is a measure of how big is your map (by using scale factor you can actually change it into meter or you can change the resolution of the grid map)
grid_size	grid size is just a grid size of the 2d map at the end.