

首页  思维导图  详情



ORB-SLAM3跟踪线程视觉初始化流程

2021-12-24 11:33:08   36  举报





AI智能生成

ORB-SLAM3跟踪线程，主要针对视觉初始化和初始化后的关键帧选择流程，简化省略了部分不相关不主要的流程仅作为学习笔记，如有问题敬请留言

学习笔记

模版推荐

作者其他创作

大纲/内容

系统状态mState

NO_IMAGES_YET（默认状态） 初次和Reset后的系统状态

NOT_INITIALIZED（系统[视觉]未初始化状态） 只由NO_IMAGES_YET转变而来

OK（系统初始化完成状态） 单目初始化完成 or跟踪成功bOK==true

RECENTLY_LOST（刚跟踪丢失） 当系统中有IMU，视觉初始化成功mState==OK且跟踪失败bOK==false

LOST（跟踪失败）

跟踪线程track()

判断mState==NO_IMAGES_YET

是

执行mState=NO_INITIALIZED

判断mState==NO_INITIALIZED

是 此时情况为：1.还未获得初始帧 2.已经获得初始帧，但还未初始化成功

执行单目初始化MonocularInitialization()

判断mState!=OK

是

执行return 此时情况为：1.刚获得初始帧，初始化未完成 2.已经获得初始帧，但当前帧不符合条件，重新寻找初始帧 3.当前帧不满足初始帧，重新寻找初始帧

否 此时情况为：1.初始化成功，此时系统开始进行跟踪

bool bOK;//用于判断跟踪状态

判断!mbOnlyTracking//该bool变量表示是否设置为只跟踪，默认值为false

是 非“只跟踪”情况

判断mState==OK//判断是否为初始化成功状态

是 此时情况为：初始化成功

判断(mVelocity.empty() && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2 //判断是否有当前帧速度，imu初始化是否成功或者当前帧是否刚进行重定位（不超过2帧）

是

bOK = TrackReferenceKeyFrame() //此时没有当前帧速度，且刚重定位，所以参考关键帧精度较高，用其进行跟踪

否

bOK = TrackWithMotionModel() //此时有当前帧速度，根据速度模型进行恒速跟踪

判断!bOK //判断恒速跟踪是否成功

是

bOK = TrackReferenceKeyFrame() //恒速跟踪失败，继而用参考关键帧进行跟踪

判断!bOK //判断上述跟踪是否成功

是 跟踪失败，分情况判断，将系统状态设为LOST或者 RECENTLY_LOST

否 此时情况为：初始化失败

对mState状态分情况判断，并进行相关操作

否 定位模式，局部建图已禁用

根据系统状态mState判断是执行单目里程计或是重定位，将单目里程计结果赋值到mbVO

再次判断!mbOnlyTracking

是

判断bOK//判断之前是否跟踪成功

是 跟踪成功

bOK = TrackLocalMap() 将当前帧跟踪局部地图，并将结果反馈到bOK中

判断!bOK 判断跟踪局部地图是否成功

是 跟踪局部地图失败

cout << "Fail to track local map!" << endl

否

判断bOK && !mbVO //判断：跟踪成功且视觉里程计失败

bOK = TrackLocalMap()

判断bOK //判断上述的跟踪是否成功

是

mState = OK //将系统状态设定为初始化成功状态

else判断mState == OK

是 此时情况：初始化成功，且此次跟踪失败

如果系统中有IMU，决定是将mState设置为RECENTLY_LOST还是LOST

判断bOK || mState==RECENTLY_LOST

是 //跟踪成功，或者刚丢失跟踪

设置当前帧位姿和速度 清除VO匹配点 清除temporal MapPoints

bool bNeedKF = NeedNewKeyFrame() //判断是否需要新插入关键帧，将判断结果赋予bNeedKF

判断bNeedKF && (bOK|| (mState==RECENTLY_LOST) //跟踪成功/刚跟踪丢失，且需要插入新关键帧

是

CreateNewKeyFrame() //将当前帧做为新关键帧插入

关键帧是LocalMapping的输入，用来局部建图，进行BA优化，将关键帧插入局部地图中，当IMU未初始化时，用于IMU初始化

单目初始化 MonocularInitialization()

判断!mpInitializer //判断初始帧标志，默认为false

是 //还没有初始帧

判断mCurrentFrame.mvKeys.size()>100 //判断当前帧的关键点是否大于100

是 //满足初始帧要求

mInitialFrame = Frame(mCurrentFrame); mLastFrame = Frame(mCurrentFrame); //将当前帧设为初始帧和前一帧

mpInitializer =  new Initializer(mCurrentFrame,1.0,200); //更新标志位

mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF; //初始化关键帧IMU预积分，并赋给当前帧

return //得到视觉初始化的第一帧（初始帧）后直接返回

否 //已经存在初始帧，此时为第二帧

判断mCurrentFrame.mvKeys.size()<=100||((mSensor == System::IMU_MONOCULAR)&&(mLastFrame.mTimeStamp-mInitialFrame.mTimeStamp>1.0)) //判断当前帧关键点小于100，或者系统中有IMU且前一帧与初始帧的时间差大于1s

是 //不满足初始化第二帧的条件

delete mpInitializer; mpInitializer = static_cast<Initializer*>(NULL); return; //删除初始帧标志位，并直接返回

ORBmatcher matcher(0.9,true); int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100); //构建ORB匹配器，将初始帧和当前帧（第二帧）匹配，将匹配数量赋予nmatches

判断nmatches<100 //初始帧和当前帧匹配点数小于100

是 //不满足初始化条件

判断(mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,mvIniMatches,Rcw,tcw,mvIniP3D......) //利用两帧做重建，计算帧间位姿变换

是 //重建成功

mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F); Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3)); tcw.copyTo(Tcw.rowRange(0,3).col(3)); mCurrentFrame.SetPose(Tcw); //设置初始帧位姿为单位阵，将计算结果作为当前帧位姿

CreateInitialMapMonocular() //创建初始化单目地图

创建初始化单目地图 CreateInitialMapMonocular()

KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); //将初始帧和当前帧建为关键帧

mpAtlas->AddKeyFrame(pKFini); mpAtlas->AddKeyFrame(pKFcur); //将初始帧和当前帧添加到mpAtlas地图中

pKFini->AddMapPoint(pMP,i); pKFcur->AddMapPoint(pMP,mvIniMatches[i]); pMP->AddObservation(pKFini,i); pMP->AddObservation(pKFcur,mvIniMatches[i]); pMP->ComputeDistinctiveDescriptors(); pMP->UpdateNormalAndDepth(); mpAtlas->AddMapPoint(pMP); //将地图点和观测值加入到mpAtlas地图中

pKFini->UpdateConnections(); pKFcur->UpdateConnections(); //更新初始帧和当前帧的地图点connection

Optimizer::GlobalBundleAdjustemnt(mpAtlas->GetCurrentMap(),20); //对MpAtlas地图的两帧关键帧进行全局BA //节点：两帧相机位姿、地图点坐标 //边：视觉重投影误差

float medianDepth = pKFini->ComputeSceneMedianDepth(2); float invMedianDepth; if(mSensor == System::IMU_MONOCULAR)    invMedianDepth = 4.0f/medianDepth; // 4.0f else    invMedianDepth = 1.0f/medianDepth; //计算场景深度

cv::Mat Tc2w = pKFcur->GetPose(); Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth; pKFcur->SetPose(Tc2w); //利用深度更新当前帧的相机位姿

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();         }     } //利用深度更新当前地图点的世界坐标

if (mSensor == System::IMU_MONOCULAR)     {         pKFcur->mPrevKF = pKFini;         pKFini->mNextKF = pKFcur;         pKFcur->mpImuPreintegrated = mpImuPreintegratedFromLastKF;         mpImuPreintegratedFromLastKF = new IMU::Preintegrated(pKFcur->mpImuPreintegrated->GetUpdatedBias(),pKFcur->mImuCalib);     } //将两帧关键帧进行链接，并初始化当前关键帧与前一关键帧的IMU预积分

mState=OK; //将系统状态设为OK，视觉初始化完成