38ORB-SLAM2源码详解

ORB-SLAM代码详细解读信息科学与工程学院人工智能与机器人研究所1吴博@泡泡机器人[e-mail]2016.8.29代码主要结构22016/8/30变量命名规则：“p”表示指针数据类型n表示int类型“b”表示bool类型s表示set类型“v”表示vector数据类型'l'表示list数据类型“m”表示类成员变量Tracking.cppLocalMapping.cppLoopClosing.cppViewer.cpp3System入口:输入图像GrabImageStero(imRectLeft,imRectRight)GrabImageRGBD(imRectLeft,imRectRight)GrabImageMonocular(im)转为灰度图Stero:mImGray,imGrayRightRGBD:mImGray,imDepthMono:mImGray构造FrameStero:Frame(mImGray,imGrayRight,mpORBextractorLeft,mpORBextractorRight)RGBD:Frame(mImGray,imDepth,mpORBextractorLeft)Mono(未初始化):Frame(mImGray,mpIniORBextractor)Mono(已初始化):Frame(mImGray,mpORBextractorLeft)Track数据流进入Tracking线程注：mpIniORBextractor相比mpORBextractorLeft提取的特征点多一倍2016/8/304Tracking线程:初始化SteroInitialization()相机位姿跟踪KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB)注：mbOnlyTracking默认为false，用户可通过运行界面选择仅跟踪定位模式2016/8/30局部地图跟踪是否生成关键帧MonocularInitialization()mbOnlyTracking(false)mbOnlyTracking(true):同时跟踪与定位，不插入关键帧，局部地图不工作位姿跟踪TrackWithMotionModel()TrackReferenceKeyFrame()Relocalization()UpdateLocalMap()SearchLocalPoints()：获得局部地图与当前帧的匹配PoseOptimization():最小化投影误差优化位姿UpdateLocalKeyFrames()UpdateLocalPoints()Frame生成关键帧很长时间没有插入关键帧、局部地图空闲、跟踪快要跪跟踪地图MapPoints的比例比较少对于双目或RGBD摄像头构造一些MapPoints,为MapPoints添加属性5LocalMapping线程:检查队列2016/8/30mlNewKeyFrames和当前关键帧相连的关键帧及MapPoints做局部BA优化ProcessNewKeyFrame()CheckNewKeyFrames()处理新关键帧剔除MapPoints生成MapPointsMapPoints融合LocalBA更新MapPoints与KeyFrame的关联UpdateConnections()剔除地图中新添加的但质量不好的MapPoints：a.IncreaseFound/IncreaseVisible25%b.观测到该点的关键帧太少运动过程中和共视程度比较高的关键帧通过三角化恢复出一些MapPoints检查当前关键帧与相邻帧（两级相邻）重复的MapPoints关键帧剔除其90%以上的MapPoints能被其它共视关键帧（至少3个）观测到的关键帧6LocalClosing线程(闭环检测):队列中取一帧2016/8/30mlploopKeyFrameQueue判断距离上一次闭环检测是否超过10帧计算当前帧与相连关键帧的Bow最低得分检测得到闭环候选帧mpcurrentKFmpcurrentKFminscore检测候选帧连续性……SpreviousGroup……Result:1、for(i,vpcandidateKFs)3、for(*sit,spcandidateKFs)mvpEnoughConsistentcandidatesvcurrentconsistentGroup2、for(iG,mvConsistentGroup)找出与当前帧有公共单词的关键帧，但不包括与当前帧相连的关键帧(pKF,minscore)统计候选帧中与pKF具有共同单词最多的单词数lKFsharingwordsmaxcommonwords得到阈值mincommons=0.8*maxcommonwordsmaxcommonwordsmincommonsminscore筛选共有单词大于mincommons且Bow得分大于minscore的关键帧lscoreAndMatch将存在相连的分为一组，计算组最高得分bestAccScore，同时得到每组中得分最高的关键帧lsAccScoreAndMatchbestAccScore得到阈值minScoreToRetain=0.75*bestAccScorevpLoopCandidateslsAccScoreAndMatchminScoreToRetain1、三个阈值都是计算获得，鲁邦性好minscoremincommonsminscoreToRetain2、通过分组可以将单独得分很高的无匹配关键帧剔除vpLoopCandidates连续性检测示意图：213410如图：1、2、3、4、10都是闭环候选帧。节点1：与2、3相连，1与2、3分为一组节点2：与1、3相连，2与1、3分为一组节点3：与1、2、4相连，3与1、2、4分为一组节点4：与3相连，4与3分为一组节点10：10自己单独一组分组示意图：7LocalClosing线程(Sim3计算):SearchByBowmpCurrentKF处构造Sim3求解器，对匹配成功的关键帧进行Sim3求解for(pKF,mvpEnoughConsistentCandidates)vvpMapPointMatchesSearchBySim3得到更多匹配Scm--(R,t,s)mpCurrentKF,pKFvpMapPointMatchesOptimizeSim3(mpCurrentKF,pKF)Scm--gscmvpMapPointMatches将MatchedKF共视帧取出vpMapPointMatches--mvpCurrentMatchedPointspKF--MatchedKF将MatchedKF共视帧取出(包括MatchedKF)vpLoopConnectedKFsSearchByProjection得到更多匹配点mpCurrentKFmvpLoopMapPointsgscm--mg2oscw--mscwmvpCurrentMatchedPoints7LocalClosing线程(Sim3计算):,,,X轴：Y轴：=‖‖⁄=(,−,)−[(,−,)⋅]其中：=‖‖⁄其中：=,−,12Z轴：=×3三对匹配3D，分别对左右三个3D点建立坐标系：如果左边坐标系有一个向量,那么：=||右坐标系同理，且令：=||4可以得到向量沿着坐标轴的值左乘可以变换到右坐标系,故可推导出旋转：=5=计算平移量：质心：=1,=1,5,,,原点移到质心：,=,−,=,−6,=0,=0优化函数：‖‖=‖,−[(,)+‖=‖,−(,‖−2⋅,−(,)]+‖‖‖,−(,‖与优化量无关8min‖‖=0,=(,)−=−+(79==−(8LocalClosing线程(Sim3计算):计算尺度：由于=0‖‖=‖,−(,‖‖‖=‖,‖−2,⋅(,)+‖(,‖‖‖=‖,‖−2,⋅(,)+‖,‖令：‖‖=−2+=−⁄+(−)⁄=(,⋅(,)‖,‖根据对称性：=()+=1⁄=−1=可是：=1⁄≠(,⋅(,)‖,‖12=1,−,10如果公式10变为：111−2+=−1+2−则公式11变为：=‖,‖‖,‖⁄139=()+LocalClosing线程(Sim3计算):如果对于大于三组匹配点：令：==,,=++−−−−−−++−+−+−+−++−−+特征值分解N矩阵：N最小特征值对应的特征向量就是待求四元数=cos2⁄+sin2⁄四元数转欧拉角：10LocalClosing线程(correctLoop):暂停LocalMapping防止插入新的关键帧更新当前帧的共视相连关系mpCurrentKF得到与当前帧相连关键帧mpCurrentKFmvpCurrentConnectedKFs(包括当前关键帧)mpCurrentKF的mg2oScw根据传播得到与当前帧相连关键帧闭环后的Sim3调整当前相连关键帧的MapPointscorrectedSim3NocorrectedSim3将Sim3转为SE3，并调整关键帧位姿更新关键帧连接关系用pLooMapPoints替换pCurrentMapPoints将这些关键帧的MapPoints与闭环MapPoints融合更新关键帧连接关系得到由闭环形成的连接关系OptimizeEssentialGraph新建一个线程进行全局BA优化mvpCurrentMatchedPoints--pLoopMapPointmpCurrentKF的MapPoint--pCurrentMapPointcorrectedSim3mvpLoopMapPointsmvpCurrentConnectedKFsLoopConnectionsmpMapLoopConnectionsmpMatchedKFmpCurrentKFcorrectedSim3NocorrectedSim3共视关键帧：有共同MapPoints观测的关键帧CovisibilityGraph：共视关键帧及它们的连接关系构成的图EssentialGraph（包含3部分）：a.扩展树连接关系b.闭环连接关系c.共视关系非常好的连接关系112134512Fr