ORB SLAM2 地图的保存与加载

ORBSLAM中的地图保存与载入原版代码中没有给,需要自己来实现.看了达达大神等一系列的博文,终于有了一个大的改动框架,尤其这篇博文算是集大成者,这里我厚着脸皮进行了转载~

我在此基础上我解决了addVertex：FATAL的错误,这个错误来源于进行G2O设置的时候添加节点时的编号冲突,这个错误看似不会影响运行,实际上是比较致命的.由于这个错误的存在会导致无法添加新的优化节点,造成优化部分信息缺失,在我使用TUM的数据集进行测试的时候,如果不修正这个错误,常常会发生载入地图后第二次的轨迹与第一次差很多的严重错误!!!所以有必要对这个问题进行修改.

具体改动如下:

错误产生原因:载入离线地图之后在开始跟踪时,Frame,KeyFrame,MapPoint的编号与载入的离线地图中的编号冲突,导致新的信息无法添加入优化.

解决办法:记录离线地图的最大编号,在进行载入后,更新Frame,KeyFrame,MapPoint的起始编号(最大编号+1),避免冲突.

1. MapPoint:

Map.cc:

    // Then read MapPoints one after another, and add them into the map
    //读取MapPoint,同时将其添加入map中
    cerr << "Map.cc :: The number of MapPoints:" << nMapPoints << endl;
    for (unsigned int i = 0; i < nMapPoints; i++) {
        MapPoint *mp = LoadMapPoint(f);
        AddMapPoint(mp);
    }

    //更新一下MapPoint那边的id,使得后续插入的MapPoint编号从N+1开始.
    //重点!!!!!!!!!!!!!!!!!!!
    if (mnMaxMPid > 0) {
        MapPoint *temp_MapPoint = new MapPoint(mnMaxMPid,this);  //使用MapPoint的构造函数来解决id更新的问题
    }

    cerr<<"Map.cc :: Max MapPoint ID is: " << mnMaxMPid << ", update MapPoint-mnId to this number" <<endl;

    cerr<<"Map.cc :: MapPoint Load OVER!"<<endl;

在这里,重载了一个MapPoint的构造函数,用这个构造函数更新了id:

MapPoint.cc

    MapPoint::MapPoint(long unsigned int i, Map* pMap):
            mnFirstKFid(0), mnFirstFrame(0), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
            mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1), mnFound(1), mbBad(false),
            mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
    {
        // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
        unique_lock<mutex> lock(mpMap->mMutexPointCreation);
        mnId = i;
        nNextId = i+1;
    }

2. Frame&KeyFrame
在从离线地图中读取了KeyFrame的最后,对Frame以及KeyFrame的编号进行更新:

map.cc

    //更新一下Frame那边的id,把这前N个从离线特征地图读入的KF记作前N个Frame,那么后续插入的Frame编号从N+1开始记录
    if (mnMaxKFid > 0) {
        Frame temp_frame = Frame(mnMaxKFid);  //使用Frame的构造函数来解决id更新的问题
        kf_by_order.front()->updateID(mnMaxKFid);  //更新kf的id
    }
    cerr<<"Map.cc :: Max KeyFrame ID is: " << mnMaxKFid << ", update KF(num+2) and Frame mnId to this number" <<endl;
    cerr<<"Map.cc :: Load IS OVER!"<<endl;

注意,这里的Frame是和MapPoint一样采用了重载构造函数的方法更新id,但是由于KeyFrame需要初始化的成员变量太多,使用构造函数更新id的话比较繁琐,所以这里就新定义了一个成员函数,在这里进行了一下调用:

Frame.cc

    //After Load Map need to Update frame id
    Frame::Frame( long unsigned int i )
    {
        mnId = i;
        nNextId = i+1;
    }

KeyFrame.cc

//After Load Map need to Update Keyframe id
void KeyFrame::updateID(long unsigned int i)
    {
        mnId=i+1;
        nNextId = i+2;   //为了保险,我这边直接设置其下一个id是最大id+2
    }

进行这些修改之后,就可以完美运行了!!!

在这里进行了转载,有需要可以直接访问原作者的博客(Github开源)

---------------------------------------------------------------我是很丑的分界线-----------------------------------------------------------------

ORB SLAM2 真是个神奇的算法。他开源了，但是各种不实用。这大概就是学术和工作的区别吧。人家毕竟花了一整个博士阶段去开发这个算法，我们还是应该心存感激。

实验室里用的相机是ZED的双目相机，所以为了这个相机，我做了自己的双目相机数据包和接口，发现实时SLAM速度在i7处理器的条件下也就只能跑5~6Hz左右，这还不考虑做闭环的时间。显然，对于一个正在快速运动的机器人来说，实时的SLAM并不实用。为了能够真正地实时定位，我能想到合理的解决方案是，事先建好地图，然后保存地图（关键点、关键帧及各种所需连接）。等我们需要定位时，直接载入做纯定位即可。

当然，做起来永远没有那么简单。读算法就需要很久，找到问题又要花很久。网上有很多教程，但可惜的是大家都没能把自己的算法彻底开源。所以我尝试提供一个可以直接从github抓下来用的带有保存和加载地图功能的版本，供学习使用。（主要是没人交流，痛苦啊，干脆开一个仓库，求大神帮忙给个pull request啊！）

喜大普奔，现在终于流畅的实现重载功能了（2018/05/02）。一开始是双目ZED相机的功能已经调试好。后续更新我会写在下一段里面。

感谢前辈大神们的博文：

https://blog.csdn.net/felaim/article/details/79667635

https://blog.csdn.net/felaim/article/details/79671160

https://blog.csdn.net/u012177641/article/details/78802315

http://www.cnblogs.com/mafuqiang/p/6972841.html

我的代码同步更新在这里, 反正是献丑了，旨在拯救同胞于苦难。要是觉得有用，帮我github加颗星吧：https://github.com/BoomFan/ORB_SLAM2

这是直接从ORB SLAM2原版当中fork出来的。既然开源了，改动的地方大家也能看见，我就不多说了。代码持续更新中。。。

（2018/04/17 更新）现在暂时能够载入地图了但重定位时还是有问题。感觉是新读进来的一帧并没有和之前的最后一帧产生任何关系。我调试好了再回来写具体是怎么回事吧。

（2018/05/01更新）现在载入地图后的重定位调试成功，但是插入新的关键帧出了些小问题。

（2018/05/02 更新）现在插入新的关键帧的问题解决了，这个版本终于可以用了。我现在只调试好了Example/Stereo文件夹里的stereo_zed.cc文件。至于stereo_kitti，和stereo_euroc，甚至是单目相机的部分，我暂时先放一会。

（2018/05/03 更新）stereo_kitti.cc 现在也可以使用了。理论上stereo_euroc.cc应该也能用了，我只是懒得下载他们的dataset罢了，所以没有亲自测试。除非有人报Issues, 不然我应该也不会去管啦。接下来会尝试完成单目部分的调试。

（2018/05/05 更新）现在mono_kitti.cc 也可以使用了，至少重复播完全相同的dataset是通过了我的测试的。所以理论上单目相机的程序应该都能正常使用啦。此处debug时其实买下了一个隐患，但是我先用一个愚蠢的办法强行避开了。如果有麻烦，以后再说吧。接下来我会把时间放在机器人身上了，算法本身的事就先放一放。

具体的运行命令大概是：

cd /XXXXXX/ORB_SLAM2

./Examples/Stereo/stereo_zed Vocabulary/ORBvoc.txt Examples/Stereo/zed0000012643.yaml /XXXXX/ROSbag/XXXXX

反正是这么个格式，具体的大家可以自己改。我后面把bug调试完了再想办法写个好用的接口出来。

给一个我调试成功的KITTI datasaet的双目命令作为样例吧，我自己的代码，Ubuntu 16.04的系统，按照原作者的要求安装的Library, 是可以跑通的：

cd ~/XXXX/ORB_SLAM2

./Examples/Stereo/stereo_kitti Vocabulary/ORBvoc.txt Examples/Stereo/KITTI04-12.yaml /XXXX/KITTI/dataset/sequences/08

或者单目：

/Examples/Monocular/mono_kitti Vocabulary/ORBvoc.txt Examples/Monocular/KITTI04-12.yaml /XXXX/KITTI/dataset/08

########################### Debug 过程 ###############################

2018.04.17 update

现阶段大致的猜测是，这样的: ORB SLAM当中的关键帧和mappoint都不是完全不变化的。这意味着，有的时候某些特定index的mappoint或者KeyFrame会被删掉。而我们在保存的时候，是按照剩下的C++的指针来保存的，重新载入地图之后，可能所有的指针都出现了问题。唉。。。感觉并不像一个很容易解决的问题啊。。。加油加油。。。

2018.05.01 update

唉，真所谓求人不如求己。然而求己很耽误时间啊。真的是不得已硬着头皮啃这块骨头。感觉时间很容易就耽误了，因为代码无论怎么读，不自己动手逐行debug是没有办法真正记住里面究竟发生了什么的。

上次的问题终于被我解决了一部分。加载地图时，如果地图很大，用float Distances[N][N]这种格式来保存二维数组是会出现内存错误的，所以我用vector替换了一下。然后，上次之所以读进来不能定位，是因为现有的网上能找到的代码当中，LoadMap时只重载了Mappoints，KeyFrame和Covisibilitygraph的边。但是并没有将KeyFrame放到KeyFrameDatabase里面去。也就是说，当我们选择载入已有的地图时，即System::System（）函数里，在mpMap->Load(strPathMap, mySystemSetting, mpKeyFrameDatabase)的时候是需要把mpKeyFrameDatabase这个指针也传进去的。所以相关的一套代码全都要修改。

真是心累啊。于是我又在Tracking::Track()函数里增加了一些判断条件：

if(mState==NOT_INITIALIZED && mpMap->GetMaxKFid() == 0)//这就是完全新建地图

else if(mState==NOT_INITIALIZED && mpMap->GetMaxKFid() > 0)// 这意味着我们重载了地图

else // 最后才是正常的跟踪线程

大概是下面这个思维导图里的意思（整个程序确实太庞大，我还没写明白，暂时先不公开这张思维导图的全图啦）：

然后我又添加了StereoInitializationWithMap()函数，用以区分StereoInitialization()函数。MonocularInitializationWithMap()我暂时还没写，先留白，后面把双目调试明白了再回来写这个单目的。

另外，由于我不小心稍稍修改了一下savemap的二进制保存顺序，所以可能只能读取我这个版本自己存的二进制文件，不能用之前几位大神的二进制文件啦！

可以看到：地图已经能重载啦！这是一个我用5000多张图片制作的大地图。

如下图所示KeyFrame 有1847帧，Mappoints有71900个

手动输入几张新的图片之后：

可以看到，左边红色的点和绿色的当前帧已经成功定位啦！

可惜的是，如下图命令行所示，多输入几张还是会有内存问题，可能是新的闭环被检测出来，哪里又出了错误吧：

代码仍然是同步到了我的github里面，当前版本毕竟还不完善，我还在debug，所以有些cout就没有抹掉，导致程序看起来还特别乱，等我后面调试好了再回来删吧。

现阶段是重载了之后仍然继续在原来的基础上建立地图，等把所有这些调好了之后我再调纯定位的部分。

2018.05.02 update

今天又耐着性子找了半天，发现是在载入地图的关键帧时，mGrid这个变量没有求，导致后面ORBmatcher::Fuse需要GetFeatureInArea的时候后内存出错。所以我在KeyFrame::KeyFrame里面把mGrid这一段给加上了。成果如图，终于能从左边的起点流畅运行到上方的绿色帧啦：

当然，我貌似还遇到过addVertex：FATAL的错误出现。但貌似并不影响程序的正常工作，我姑且称之为warning,先不去管它，至少程序还没停。等后面找到原因了我再回来改吧。

2018.05.03 update

修复了一些载入地图之后的bug，比如前几帧质量都不好的情况下，我们需要重新定位，不能直接走入正常模式。更新完成后，我们会忽略掉最开始质量不高的几帧，从特征点足够多的那一帧开始。

2018.05.05 update

在我debug的时候其实有一件很奇怪的事情发生。重载地图后，程序运行了几秒钟就报Segmentation fault,仔细检查发现是KeyFrame::SetBadFlag()函数里面，正当中的这一行

(vpConnected[i]->mnId == (*spcit)->mnId)  

会出现错误。

原因是vpConnected[i]会等于0。

很奇怪，一KF的指针，怎么会指向0而不指向一个地址呢？深挖一下，原因来自KeyFrame::AddConnection函数和KeyFrame::UpdateBestCovisibles()函数，他们里面的mConnectedKeyFrameWeights可能本身就不对。也就是说，可能KeyFrame::UpdateConnections()这里就已经出了错。

向来应该是一件很难说清楚原因的事，也许改动的地方实在是太多，所以我就干脆在KeyFrame::SetBadFlag()里面使了点小伎俩，加了个if ... continue强行忽略了这个错误。唉，也许是立了个flag?毕竟这是我认为最小幅度的改动了，应该不会影响到其他功能。