首页  思维导图  详情

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

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





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

是
//不满足初始化条件

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

判断(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，视觉初始化完成