【ORBSLAM2源码笔记(3)】Tracking线程跟踪定位流程及Track()源码解析
有道云笔记思维导图整理链接:
Track()源码流程解析: https://note.youdao.com/s/O9lzEplm
Track()源码注释及解析:
在这里一部分引用了计算机视觉life的教程内容,加上自己对代码部分的理解
void Tracking::Track()
{
// mState为tracking的状态,包括 SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
// 如果图像复位过、或者第一次运行,则为NO_IMAGE_YET状态
if(mState == NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState 存储了Tracking最新的状态,用于FrameDrawer中的绘制
mLastProcessedState = mState;
// Get Map Mutex -> Map cannot be changed
// 地图更新时加锁。保证地图不会发生变化
// 疑问:这样子会不会影响地图的实时更新?
// 回答:主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// Step 1:地图初始化
if(mState == NOT_INITIALIZED)
{
if(mSensor == System::STEREO || mSensor == System::RGBD)
//双目RGBD相机的初始化共用一个函数
StereoInitialization();
else
//单目初始化
MonocularInitialization();
//更新帧绘制器中存储的最新状态
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
else
{
// System is initialized. Track Frame.
// bOK为临时变量,用于表示每个函数是否执行成功
bool bOK;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
// mbOnlyTracking == false表示正常SLAM模式(定位+地图更新),mbOnlyTracking等于true表示仅定位模式
// tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode,可以控制是否开启mbOnlyTracking
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
// Step 2:跟踪进入正常SLAM模式,有地图更新
// 是否正常跟踪
if(mState == OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
// Step 2.1 检查并更新上一帧被替换的MapPoints
// 局部建图线程则可能会对原有的地图点进行替换.在这里进行检查
CheckReplacedInLastFrame();
// Step 2.2 运动模型是空的或刚完成重定位,跟踪参考关键帧;否则恒速模型跟踪
// 第一个条件,如果运动模型为空,说明是刚初始化开始,或者已经跟丢了
// 第二个条件,如果当前帧紧紧地跟着在重定位的帧的后面,我们将重定位帧来恢复位姿
// mnLastRelocFrameId 上一次重定位的那一帧
if(mVelocity.empty() || mCurrentFrame.mnId < mnLastRelocFrameId + 2)
{
// 用最近的关键帧来跟踪当前的普通帧
// 通过BoW的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点都对应3D点重投影误差即可得到位姿(BA)
bOK = TrackReferenceKeyFrame();
}
else
{
// 用最近的普通帧来跟踪当前的普通帧
// 根据恒速模型设定当前帧的初始位姿
// 通过投影的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点所对应3D点的投影误差即可得到位姿(BA)
bOK = TrackWithMotionModel();
if(!bOK)
//若根据恒速模型计算初始位姿失败了,只能根据参考关键帧来跟踪
bOK = TrackReferenceKeyFrame();
}
}
else
{
// 如果跟踪状态不成功,那么就只能重定位了
// BOW搜索,EPnP求解位姿
bOK = Relocalization();
}
}
else
{
// Localization Mode: Local Mapping is deactivated
// Step 2:只进行跟踪tracking,局部地图不工作
if(mState == LOST)
{
// Step 2.1 如果跟丢了,只能重定位
bOK = Relocalization();
}
else
{
// 跟踪正常
// mbVO是mbOnlyTracking为true时的才有的一个变量
// mbVO为false表示此帧匹配了很多的MapPoints,跟踪很正常 (注意有点反直觉)
// mbVO为true表明此帧匹配了很少的MapPoints,少于10个,要跪的节奏
if(!mbVO)
{
// In last frame we tracked enough MapPoints in the map
// Step 2.2 如果跟踪正常,运动模型非空,使用恒速模型进行跟踪定位
if(!mVelocity.empty())
{
bOK = TrackWithMotionModel();
}
// Step 2.2 如果跟踪不正常,运动模型为空,使用参考关键帧进行跟踪定位
else
{
bOK = TrackReferenceKeyFrame();
}
}
/// mbVO为true,表明此帧匹配了很少(小于10)的地图点
/// MapPoints < 10
/// 跟踪异常,需要同时进行跟踪和重定位
else
{
// In last frame we tracked mainly "visual odometry" points.
// We compute two camera poses, one from motion model and one doing relocalization.
// If relocalization is sucessfull we choose that solution, otherwise we retain
// the "visual odometry" solution.
bool bOKMM = false; /// MM,“Motion Model”的缩写,表示通过运动模型进行跟踪的结果
bool bOKReloc = false; /// 通过重定位进行跟踪的结果
vector<MapPoint*> vpMPsMM; /// 运动模型中构造的地图点
vector<bool> vbOutMM; /// 追踪运动模型时发现的外点
cv::Mat TcwMM; /// 运动模型得到的位姿
// Step 2.3 当运动模型有效的时候,根据运动模型计算位姿
if(!mVelocity.empty())
{
bOKMM = TrackWithMotionModel();
// 将恒速运动模型跟踪的结果暂存到这几个变量中,因为后面重定位还会改变这几个变量
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.mTcw.clone();
}
// Step 2.4 使用重定位的方法来得到当前帧的位姿
// 只要重定位成功整个跟踪过程正常进行(重定位与运动模型,更相信重定位)
bOKReloc = Relocalization();
// Step 2.5 根据前面的恒速模型、重定位结果来更新状态
// 使用恒速运动模型成功,未使用重定位
if(bOKMM && !bOKReloc)
{
// 使用之前暂存的恒速运动模型的计算结果
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
// 如果当前帧匹配的地图点较少,增加当前可视地图点的观测次数
if(mbVO)
{
// mCurrentFrame.N: 当前帧的特征点个数
// 更新当前帧的每一个特征点被观测到的地图点个数
for(int i =0; i<mCurrentFrame.N; i++)
{
// mvpMapPoints: 每个特征点对应的MapPoint
// mvbOutlier: 每个特征点对应的外点
// 如果当前帧的该特征点观测到对应的MapPoint,并且不存在属于外点的观测
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
{
// IncreaseFound(): 能找到该特征点的地图点个数+1
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
}
}
}
}
// 只要重定位成功整个跟踪过程正常进行(重定位与运动模型,更相信重定位)
else if(bOKReloc)
{
mbVO = false;
}
// 只要其中一个成功,系统就认为仅跟踪tracking初始姿态的过程执行成功
bOK = bOKReloc || bOKMM;
}
}
}
// 将最新的关键帧作为当前帧的参考关键帧
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// If we have an initial estimation of the camera pose and matching. Track the local map.
// Step 3:在跟踪得到当前帧初始姿态后,现在对local map进行跟踪得到更多的匹配,并优化当前位姿
// 前面只是跟踪一帧得到初始位姿,这里搜索局部关键帧、局部地图点,和当前帧进行投影匹配,得到更多匹配的MapPoints后进行位姿优化
if(!mbOnlyTracking)
{
if(bOK)
bOK = TrackLocalMap();
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
// 重定位成功
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
//根据上面的操作来判断是否追踪成功
if(bOK)
mState = OK;
else
mState=LOST;
// Update drawer
// Step 4:更新显示线程中的图像、特征点、地图点等信息
mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
//只有在成功追踪时才考虑生成关键帧的问题
if(bOK)
{
// Update motion model
// Step 5:跟踪成功,位姿非空,更新恒速运动模型
if(!mLastFrame.mTcw.empty())
{
// 更新恒速运动模型 TrackWithMotionModel 中的mVelocity
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc;
}
else
//否则速度为空
mVelocity = cv::Mat();
//更新显示中的位姿
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
// Clean VO matches
// Step 6:清除观测不到的地图点
for(int i=0; i<mCurrentFrame.N; i++)
{
/// mvpMapPoints:每个特征点对应的MapPoint.如果特征点没有对应的地图点,那么将存储一个空指针
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
// 判断该特征点有无对应的地图点
if(pMP)
// Observations(): 返回地图点被观测到的相机数目,单目+1,双目或RGB-D则+2
// < 1表示该地图点不能被观测到
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints
// Step 7:清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints(仅双目和rgbd)
// 步骤6中只是在当前帧中将这些MapPoints剔除,这里从MapPoints数据库中删除
// 临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果,用完以后就扔了,没有添加到地图中
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
// 这里不仅仅是清除mlpTemporalPoints,通过delete pMP还删除了指针指向的MapPoint
delete pMP;
}
// 不能够直接执行这个是因为其中存储的都是指针,前面循环中的操作都是为了避免内存泄露
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
// Step 8:检测并插入关键帧,对于双目或RGB-D会产生新的地图点
if(NeedNewKeyFrame())
CreateNewKeyFrame();
idk ++;
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
// 作者这里说允许在BA中被Huber核函数判断为外点的传入新的关键帧中,让后续的BA来审判他们是不是真正的外点
// 但是估计下一帧位姿的时候我们不想用这些外点,所以删掉
for(int i = 0; i < mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
}
}
// Reset if the camera get lost soon after initialization
// Step 10 如果初始化后不久就跟踪失败,并且relocation也没有搞定,只能重新Reset
if(mState == LOST)
{
// 如果地图中的关键帧信息过少的话,直接重新进行初始化了
// KeyFramesInMap(): 返回地图中的关键帧数目
if(mpMap->KeyFramesInMap() <= 5)
{
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
}
}
// 确保已经设置了参考关键帧
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// 保存上一帧的数据,当前帧变上一帧
mLastFrame = Frame(mCurrentFrame);
}
// Store frame pose information to retrieve the complete camera trajectory afterwards.
// Step 11:记录位姿信息,用于最后保存所有的轨迹
if(!mCurrentFrame.mTcw.empty())
{
// 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1
cv::Mat Tcr = mCurrentFrame.mTcw * mCurrentFrame.mpReferenceKF->GetPoseInverse();
//保存各种状态
mlRelativeFramePoses.push_back(Tcr); /// 所有的参考关键帧的位姿;看上面注释的意思,这里存储的也是相对位姿
mlpReferences.push_back(mpReferenceKF); /// 参考关键帧
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp); /// 关键帧的时间戳
mlbLost.push_back(mState == LOST); /// 是否跟丢的标志
}
else
{
// This can happen if tracking is lost
// 如果跟踪失败,则相对位姿使用上一次值 (.back())
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState == LOST);
}
}