ORB-SLAM单目地图保存、加载和定位

老大说：我要可以定位的地图，你把ORB-SLAM的地图保存下来吧！

我：老大，ORB-SLAM单目情况下是产生不了稠密的的地图的，保存下来也没啥用。

老大：我看这地图还挺稠密的，你先把保存下来把。

我：好吧，老大.....

废话：这一块的工作前前后后大概花了3周的时间，每周大概4天左右耗费在这上面，这段时间当中maplab开源出来了，空闲的时候也尝试着编译通过了，感觉很开心。

在这个地方我一定要感谢 @达达MFZ，地图的加载和保存的工作主要就是基于他的代码上进行修改的，可以说没有大神的博客详细介绍，我是搞不出来的，这里附上三篇文章的链接，其中下面评论中出现的错误和别人的修改也是很有用的。

https://www.cnblogs.com/mafuqiang/p/6972342.html

一、地图的保存

出现在上面对应的代码和所在位置我就不贴了，保存的代码主要是在Map.h和Map.cc敲，根据代码里的保存格式，应该是保存成二进制文件，对应的代码敲进对应的位置后，我在system.cc中定义了一个save函数去调用map中的save函数，然后在ros_mono.cc中调用了system中的save函数，可能我这样的做法有点太绕了，或许可以直接在ros_mono中调用map函数：

在system.h和system.cc中分别添加声明和定义：

  void SaveMap(const string &filename);

 void System::SaveMap(const string &filename)
 {
    mpMap->Save(filename);
 }

然后在ros_mono.cc中添加

 char IsSaveMap;
     cout << "Do you want to save the map?(Y/N)" << endl;
     cin >> IsSaveMap;
     if(IsSaveMap == 'Y' || IsSaveMap == 'y')
         SLAM.SaveMap("MapPointandKeyFrame.bin");

IsSaveMap是一个flag，让你确定是否保存地图的，当然这里输入Y或者y就是保存，输入其他的字符都是不保存啦，写得不是很规范。这样MapPoint 和KeyFrame都可以保存在了home目录下的MapPointandKeyFrame.bin文件中了。

二、地图的加载

地图的加载有点麻烦呢，写得就很麻烦了，调用也很让我不爽，充分认识到了自己是多菜的菜鸟。

写得部分我就不在这里赘述了，请看http://www.cnblogs.com/mafuqiang/p/6972841.html这篇文章和对应的下一篇，其中需要按照评论在MapPoint.h和MapPoint.cc中分别添加

 KeyFrame* SetReferenceKeyFrame(KeyFrame* RFKF);

 KeyFrame* MapPoint::SetReferenceKeyFrame(KeyFrame* RFKF)
 {
 return mpRefKF = RFKF;
 }

并在keyframe.h中加上initkeyframe头文件。

然后开始加载地图点和关键帧，同样我还是在system.h中声明了

 void LoadMap(const string &filename,SystemSetting* mySystemSetting);

并在对应的system.cc中添加了定义

 void System::LoadMap(const string &filename,SystemSetting* mySystemSetting)
 {
    mpMap->Load(filename, mySystemSetting);
 }

哦，对了，如果这样读进来好像没法做定位只能显示出地图，需要在Map.cc中Load函数读入每一个关键帧后添加到KeyFrameDatabase中去，但这样需要在Map.h中加入KeyFrameDatabase的头文件，这样就可以读入地图后进行relocalisation了。

然后，我读入是直接在system.cc中读入的，

 cout << "Do you want to load the map?(Y/N)" << endl;
     cin >> IsLoadMap;
     SystemSetting *mySystemSetting = new SystemSetting(mpVocabulary);
     mySystemSetting->LoadSystemSetting("/home/liuzhen/catkin_ws/src/ORB_SLAM2-master/Examples/ROS/ORB_SLAM2/Asus.yaml");
     if(IsLoadMap == 'Y' || IsLoadMap == 'y'){
         mpMap->Load("/home/liuzhen/catkin_ws/src/ORB_SLAM2-master/Examples/ROS/ORB_SLAM2/MapPointandKeyFrame.bin",mySystemSetting);
 }

这样应该就可以在运行ros_mono时加载地图了，我的地图加载出来是这样的：



三、定位     定位的话，我主要是在tracking.cc里面修改的，我也想不到什么其他更好的方案，主要修改是在track中，目前来说是可以重定位然后track，但是不是就segmented dumped，这个就很痛苦了，不过这个说不好，所以目前就做到这了。

四、附
其实在地图的加载和保存的过程中，poine做出来一版，我在自己电脑上编译跑通了，但因为我是Ubuntu16.04版本，那个版本的ORBSLAM还不支持opencv3,这个就很尴尬了，我尝试改了ROS里的cvbridge，希望能够让opencv指向我自己安装的opencv2.4.13,但是没有成功，不知道有没有其他大神知道这个怎么搞，还希望能指导！！