ORB-SLAM2之地图保存、加载和重定位
前言
ORB-SLAM2的开源代码没有开发保存和加载地图的功能，重定位也只是在线的。
很多情况下，我们需要保存地图，并加载地图进行重定位。
很多人在这方面进行了探索，并开源了代码，这篇博客就是根据一版开源代码修改实现的。
参考资料：
[1] https://blog.csdn.net/jjjwwwjjjwww/article/details/51248539
[2] https://blog.csdn.net/u012177641/article/details/78802315
[3] https://www.cnblogs.com/mafuqiang/p/6972342.html
[4] https://blog.csdn.net/qq_34254510/article/details/79969046
安装修改版ORB-SLAM2
安装方法同ORB-SLAM2。
mkdir ~/map-orbslam
cd map-orbslam
git clone https://github.com/MathewDenny/ORB_SLAM2
cd ORB_SLAM2
./build.sh
打开.bashrc，在文件末尾添加export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/map-orbslam/ORB_SLAM2/Examples/ROS，然后source ~/.bashrc。
注意：注释掉其他的ROS_PACKAGE_PATH，否则下面的编译会冲突。
cd Examples/ROS/ORB_SLAM2
mkdir build
cd build
cmake .. -DROS_BUILD_TYPE=Release
make -j
由于作者只修改了ROS下的单目，所以需要一个单目USB摄像头实时运行ORB_SLAM2。
如何在ROS下使用USB摄像头请参考：https://blog.csdn.net/learning_tortosie/article/details/79879575。
mkdir -p ~/usb_cam/src
cd ~/usb_cam/src
git clone https://github.com/bosch-ros-pkg/usb_cam.git usb_cam
cd ..
catkin_make
source devel/setup.bash
roslaunch usb_cam usb_cam-test.launch
注意：
（1）ORB-SLAM2默认订阅的话题为/camera/image_raw，而usb_cam节点发布的话题为/usb_cam/image_raw，因此需要在ros_mono.cc中修改订阅的话题。
(2）打开ros_mono.cc，将以下代码取消注释，因为要实现加载地图，然后重新编译。
if (bReuseMap)
SLAM.LoadMap("Slam_Map.bin");
运行
作者并没有写README说明使用方法，所以完全是我根据ros_mono.cc摸索出来的。
1.启动摄像头
roslaunch usb_cam usb_cam-test.launch
2.启动ORB_SLAM2，建图并保存地图
rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.bin Examples/Monocular/***.yaml 0
移动摄像头进行定位与建图，完成后按ctrl+c退出，但是并没有保存地图，也没有退出程序。
修改
打开ros_mono.cc，更改代码顺序为：
// Save map
SLAM.SaveMap("Slam_latest_Map.bin");
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
// Stop all threads
SLAM.Shutdown();
重新编译后运行，按ctrl+c即可保存地图和关键帧轨迹。
3.加载地图并进行重定位
rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.bin Examples/Monocular/***.yaml true
移动摄像头，在终端窗口会输出位置和四元数表示的姿态。
然后，编写一个节点，订阅话题ORB_SLAM/pose，便可得到位姿。
ros::Publisher PosPub = nodeHandler.advertise("ORB_SLAM/pose", 5);
至此，完成了地图的保存、加载和重定位。
总结
到这里，ORB-SLAM还是只能提供定位功能，由于稀疏地图的原因，无法实现真正意义上的导航，最多能实现寻迹导航。
所谓的寻迹，必须是建图时走过的轨迹，然后把它记录下来，并保存地图。
在轨迹上设置属性点，如起点、终点等。
加载地图，得到在地图中的位姿，实时计算到起点的距离和旋转角，到达起点后进行寻迹直到到达终点，最好有避障功能。
接下来，我会扒一扒代码，扩展到双目和数据集。