ORB SLAM2 地图的保存与加载 (我决定将代码开源)

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?毕竟这是我认为最小幅度的改动了,应该不会影响到其他功能。

阅读更多

没有更多推荐了,返回首页