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