ORB_SLAM3:单目+IMU的LocalMapping线程流程梳理

LocalMapping线程的关键帧都是由Tracking线程传过来的,在该线程会完成IMU的三阶段初始化,处理队列中的关键帧(计算BOW、更新观测、描述子、共视图,插入到地图等),根据地图点的观测情况剔除质量不好的地图点,然后产生新的地图点,使得跟踪更稳,最后会检测当前关键帧的共视关键帧是否冗余,最后将当前关键帧加入到闭环检测队列中

局部地图点:在该线程中为当前关键帧的地图点和当前关键帧共视关键帧的地图点,其实局部地图点应该为当前关键帧能观测到的地图中的地图点(显示为红色的点)。

1、先将mbFinished设为false,进入while(1)循环,然后将mbAcceptKeyFrames设为false意为LocalMapping正处于繁忙状态,Tracking请不要给我发送关键帧打扰我。一直检查mlNewKeyFrames是否为空,结束while(1)循环的条件是:mbFinishRequested为true;

2、当mlNewKeyFrames不为空且mbBadImu为false时,处理队列中的关键帧。

ProcessNewKeyFrame():

1)取出队列中最前面的关键帧,记为当前关键帧(mpCurrentKeyFrame),然后在原来的队列里删掉该关键帧;

2)计算当前关键帧的Bow,其实就是更新当前关键帧的mBowVec和mFeatVec容器,mBowVec用于计算两个关键帧相似度得分,mFeatVec用于加速特征点匹配,二者均是为LocalClosing做准备;

mBowVec:<单词id,weight>如果图像中多个特征点对应同一个单词,weight会叠加。
mFeatVec:<节点id(L=2时),该节点id下面的单词对应的特征点在当前关键帧中的编号(存在容器中)>

3)处理当前关键帧中有效的地图点,如果该地图点不是来自当前帧的观测,为该地图点添加观测(更新mObservations和nObs),更新地图点的平均观测方向、观测距离范围和最佳描述子。如果该地图点是来自当前关键帧的观测则将该地图点放入mlpRecentAddedMapPoints中,等待后续MapPointCulling函数的检验;

std::map<KeyFrame*,std::tuple<int,int> > mObservations;//地图点的成员变量<能观测到该地图点的关键帧指针,该地图点在该关键帧中对应特征点的索引>
std::vector<MapPoint*> mvpMapPoints;//关键帧的地图点,索引为特征点编号,成功生成地图点为该地图点的指针,否则为NULL。

注:nObs表示多少个相机(关键帧)能观测到该地图点,双目和RGB-D每次加2,单目加1。

4)更新关键帧间的连接关系:UpdateConnections() 就是更新mConnectedKeyFrameWeights = KFcounter,mvpOrderedConnectedKeyFrames:当前关键帧的共视关键帧指针按照共视地图点数目从大到小的排序。最后更新生成树的连接,就是找当前关键帧的父关键帧(共视程度最高的那个关键帧),建立双向连接关系,将当前关键帧作为其子关键帧,且所有关键帧只有一个父关键帧(初始化参考关键帧无父关键帧),最后将当前关键帧插入到地图中(mspKeyFrames);

map<KeyFrame *, int> KFcounter;//<关键帧指针,当前关键帧与关键帧共有的地图点数>
vector<pair<int, KeyFrame *>> vPairs;//<当前关键帧与关键帧共有的地图点数,关键帧指针>,
//存储的是共有的地图点数 >= th(15)的关键帧,或者当前关键帧与所有关键帧共有的地图点数都小于th则会把权重最大的加入。

3、MapPointCulling():检测mlpRecentAddedMapPoints中的地图点,根据相机类型设置不同的观测阈值(cnThObs):单目为2,其余为3;

1)召回率=实际观测到该地图点帧数(mnFound)/理论上应该观测到该地图点帧数(mnVisible)< 0.25,就认为该地图点为坏点;

2)当前关键帧id - 最开始创建该地图点那帧id >= 2且地图点的观测(nObs)<= cnThObs,就认为该地图点为坏点;

3)若从建立该地图点开始,已经过了3个关键帧而没有被剔除,则认为是质量高的地图点。

4、用当前关键帧与相邻关键帧通过三角化产生新的地图点,使得跟踪更稳。CreateNewMapPoints():找当前关键帧共视程度最高的30个关键帧,若是IMU模式则会加入更多的关键帧,这些关键帧存入vpNeighKFs中。若此时mlNewKeyFrames为空则通过三角化产生新的地图点,若mlNewKeyFrames不为空则return,先去处理新的关键帧。新生成的地图点存入mspMapPoints(存储地图中所有地图点)中,最后将新产生的点放入检测队列mlpRecentAddedMapPoints中。

5、将mbAbortBA设为false,如果已经处理完队列中的最后的一个关键帧,则检查并融合当前关键帧与相邻关键帧(两级相邻)中重复的地图点,在这过程中不断检查mbAbortBA是否为true,为true则停止该过程;SearchInNeighbors():

1)与当前关键帧共视程度最高的30个关键帧,称为一级相邻关键帧。一级相邻关键帧的共视程度最高的20个关键帧作为二级相邻关键帧,IMU模式下加了一些prevKF,这些关键帧都存入vpTargetKFs中;

2)将当前关键帧的地图点与vpTargetKFs中关键帧的地图点进行正向融合(将当前关键帧中的地图点融合到vpTargetKF中的关键帧中)和反向融合;

3)融合的策略为:

a.若地图点反投影对应位置上不存在地图点,则直接添加观测;

b.若地图点反投影对应位置上存在地图点,则将两个地图点合并到其中观测较多的那个地图点上。

4)更新当前关键帧地图点的描述子、深度、观测主方向等属性,最后更新关键帧间的连接关系。

6、已经处理完队列中的最后的一个关键帧,并且闭环检测没有请求停止LocalMapping,同时当前地图中关键帧数目大于2个且非IMU模式或者当前关键帧所在的地图未完成IMU第一次初始化,进行局部BA优化。一级共视关键帧:当前关键帧的共视关键帧位姿(会被优化),局部地图点能观测到的关键帧但不属于一级共视关键帧会加入优化图但位姿不会被优化,局部地图点添加为顶点,边为局部地图点的重投影,最后更新了一级共视关键帧位姿和局部地图点坐标;

注:第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时,能够及时执行/停止BA。

Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(), num_FixedKF_BA, num_OptKF_BA, num_MPs_BA, num_edges_BA, bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2());

7、检测并剔除当前关键帧相邻的关键帧中冗余的关键帧,冗余关键帧判断标准:该关键帧的90%的地图点可以被其它关键帧观测到,但最新加入的关键帧不会被剔除;

KeyFrameCulling():

1)更新当前关键帧的mvpOrderedConnectedKeyFrames,取出当前关键帧的共视关键帧(mvpOrderedConnectedKeyFrames)存入vpLocalKeyFrames中,当前关键帧前面有多余22个关键帧时,last_ID记录地图中倒数第22个关键帧的id,count=21;

2)遍历所有的共视关键帧(vpLocalKeyFrames),提取每个共视关键帧的地图点,更新了nRedundantObservations和nMPs,最后判断该关键帧是否冗余,如果冗余且非IMU模式则删除该关键帧;

3)冗余且IMU模式下,不删除该关键帧的条件:

a.地图中的关键帧少于21个;

b.关键帧与当前关键帧id差1(最新加入的关键帧);

c.不满足下面的这两个条件:

if((bInitImu && (pKF->mnId < last_ID) && t < 3.) || (t < 0.5))
else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() && ((pKF->GetImuPosition() - pKF->mPrevKF->GetImuPosition()).norm() < 0.02) && (t < 3))

4)pKF->SetBadFlag():

a.初始关键帧不能删除,mbNotErase(其在LoopClosing中设置)表示不应该删除,于是把mbToBeErased置为true,假装已经删除,其实没有删除。

b.删除当前关键帧的共视的关键帧、地图点;

c.更新生成树。

8、将当前关键帧加入到闭环检测队列中。

  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值