ORB-SLAM3跟踪线程视觉初始化流程
2021-12-24 11:33:08 25 举报
AI智能生成
登录查看完整内容
ORB-SLAM3跟踪线程,主要针对视觉初始化和初始化后的关键帧选择流程,简化省略了部分不相关不主要的流程 仅作为学习笔记,如有问题敬请留言
作者其他创作
大纲/内容
NO_IMAGES_YET(默认状态)初次和Reset后的系统状态
NOT_INITIALIZED(系统[视觉]未初始化状态)只由NO_IMAGES_YET转变而来
OK(系统初始化完成状态)单目初始化完成or跟踪成功bOK==true
RECENTLY_LOST(刚跟踪丢失)当系统中有IMU,视觉初始化成功mState==OK且跟踪失败bOK==false
LOST(跟踪失败)
系统状态mState
执行mState=NO_INITIALIZED
是
判断mState==NO_IMAGES_YET
执行单目初始化MonocularInitialization()
执行return此时情况为:1.刚获得初始帧,初始化未完成2.已经获得初始帧,但当前帧不符合条件,重新寻找初始帧3.当前帧不满足初始帧,重新寻找初始帧
判断mState!=OK
是此时情况为:1.还未获得初始帧2.已经获得初始帧,但还未初始化成功
bool bOK;//用于判断跟踪状态
bOK = TrackReferenceKeyFrame()//此时没有当前帧速度,且刚重定位,所以参考关键帧精度较高,用其进行跟踪
bOK = TrackWithMotionModel()//此时有当前帧速度,根据速度模型进行恒速跟踪
bOK = TrackReferenceKeyFrame()//恒速跟踪失败,继而用参考关键帧进行跟踪
判断!bOK//判断恒速跟踪是否成功
否
判断(mVelocity.empty() && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2//判断是否有当前帧速度,imu初始化是否成功或者当前帧是否刚进行重定位(不超过2帧)
是跟踪失败,分情况判断,将系统状态设为LOST或者 RECENTLY_LOST
判断!bOK//判断上述跟踪是否成功
是此时情况为:初始化成功
对mState状态分情况判断,并进行相关操作
否此时情况为:初始化失败
判断mState==OK//判断是否为初始化成功状态
是非“只跟踪”情况
根据系统状态mState判断是执行单目里程计或是重定位,将单目里程计结果赋值到mbVO
否定位模式,局部建图已禁用
判断!mbOnlyTracking//该bool变量表示是否设置为只跟踪,默认值为false
bOK = TrackLocalMap()将当前帧跟踪局部地图,并将结果反馈到bOK中
是跟踪成功
cout << \"Fail to track local map!\" << endl
是跟踪局部地图失败
判断!bOK判断跟踪局部地图是否成功
判断bOK//判断之前是否跟踪成功
bOK = TrackLocalMap()
判断bOK && !mbVO//判断:跟踪成功且视觉里程计失败
再次判断!mbOnlyTracking
mState = OK//将系统状态设定为初始化成功状态
如果系统中有IMU,决定是将mState设置为RECENTLY_LOST还是LOST
是此时情况:初始化成功,且此次跟踪失败
else判断mState == OK
判断bOK//判断上述的跟踪是否成功
设置当前帧位姿和速度清除VO匹配点清除temporal MapPoints
bool bNeedKF = NeedNewKeyFrame()//判断是否需要新插入关键帧,将判断结果赋予bNeedKF
CreateNewKeyFrame()//将当前帧做为新关键帧插入
关键帧是LocalMapping的输入,用来局部建图,进行BA优化,将关键帧插入局部地图中,当IMU未初始化时,用于IMU初始化
判断bNeedKF && (bOK|| (mState==RECENTLY_LOST)//跟踪成功/刚跟踪丢失,且需要插入新关键帧
是//跟踪成功,或者刚丢失跟踪
判断bOK || mState==RECENTLY_LOST
否此时情况为:1.初始化成功,此时系统开始进行跟踪
判断mState==NO_INITIALIZED
跟踪线程track()
mInitialFrame = Frame(mCurrentFrame);mLastFrame = Frame(mCurrentFrame);//将当前帧设为初始帧和前一帧
return//得到视觉初始化的第一帧(初始帧)后直接返回
是//满足初始帧要求
判断mCurrentFrame.mvKeys.size()>100//判断当前帧的关键点是否大于100
是//还没有初始帧
delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);return;//删除初始帧标志位,并直接返回
是//不满足初始化第二帧的条件
判断mCurrentFrame.mvKeys.size()<=100||((mSensor == System::IMU_MONOCULAR)&&(mLastFrame.mTimeStamp-mInitialFrame.mTimeStamp>1.0))//判断当前帧关键点小于100,或者系统中有IMU且前一帧与初始帧的时间差大于1s
是//不满足初始化条件
判断nmatches<100//初始帧和当前帧匹配点数小于100
CreateInitialMapMonocular()//创建初始化单目地图
是//重建成功
判断(mpCamera->font color=\"#E65100\
否//已经存在初始帧,此时为第二帧
判断!mpInitializer//判断初始帧标志,默认为false
单目初始化MonocularInitialization()
KeyFrame* font color=\"#388E3C\
mpAtlas->AddKeyFrame(pKFini);mpAtlas->AddKeyFrame(pKFcur);//将初始帧和当前帧添加到mpAtlas地图中
pKFini->UpdateConnections();pKFcur->UpdateConnections();//更新初始帧和当前帧的地图点connection
float medianDepth = pKFini->ComputeSceneMedianDepth(2);float invMedianDepth;if(mSensor == System::IMU_MONOCULAR) invMedianDepth = 4.0f/medianDepth; // 4.0felse invMedianDepth = 1.0f/medianDepth;//计算场景深度
vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches(); for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++) { if(vpAllMapPoints[iMP]) { MapPoint* pMP = vpAllMapPoints[iMP]; pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth); pMP->UpdateNormalAndDepth(); } }//利用深度更新当前地图点的世界坐标
mState=OK;//将系统状态设为OK,视觉初始化完成
创建初始化单目地图CreateInitialMapMonocular()
Track
收藏
收藏
0 条评论
回复 删除
下一页