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

环境： 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
1
2
3
4
5
6
7
8
9

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

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

在尝试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
1
2

(这里我找了很多方法依次尝试，总结出解决方法)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
1

(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
1
2
3
4
5

(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; }
1
2
3
4
5
6

(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
1
2

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

export LD_LIBRARY_PATH=~/ORB_SLAM3/lib:$LD_LIBRARY_PATH
1

刷新.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
1
2

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
1
2
3
4
5
6
7
8
9