【ORBSLAM2源码笔记（3）】Tracking线程跟踪定位流程及Track()源码解析

编程入门行业动态更新时间:2024-10-04 19:25:58

【ORBSLAM2<a href=https://www.elefans.com/category/jswz/34/1770099.html style= 源码笔记（3）】Tracking线程跟踪定位流程及Track()源码解析"/>

【ORBSLAM2源码笔记（3）】Tracking线程跟踪定位流程及Track()源码解析

有道云笔记思维导图整理链接：

Track()源码流程解析:

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，可以控制是否开启mbOnlyTrackingif(!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(): 能找到该特征点的地图点个数+1mCurrentFrame.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;elsemState=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 中的mVelocitycv::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还删除了指针指向的MapPointdelete 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也没有搞定，只能重新Resetif(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^-1cv::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);}}