1.函数作用
ORB-SLAM2的三大线程之一---跟踪线程，负责估计运动信息、跟踪局部地图。
追踪线程的主要工作原理就是我们从数据集中读入一帧帧，刚开始的时候跟踪线程没有进行初始化（没有初始化不知道世界坐标系的原点和相机的位姿），我们初始化跟踪线程，初始成功之后把符合要求的第一帧的相机坐标作为世界坐标系的原点，在之后传进来的每帧，用三种跟踪方式计算相机的坐标，并生成地图点。
总体来说，ORB-SLAM2跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。
流程图如下：
2.函数流程
Step 1：地图初始化
Step 2：跟踪进入正常SLAM模式，有地图更新
Step 3：在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
Step 4：更新显示线程中的图像、特征点、地图点等信息
Step 5：跟踪成功，更新恒速运动模型
Step 6：清除观测不到的地图点
Step 7：清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
Step 8：检测并插入关键帧，对于双目或RGB-D会产生新的地图点
Step 9 删除那些在bundle adjustment中检测为outlier的地图点
Step 10 如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Reset
Step 11：记录位姿信息，用于最后保存所有的轨迹
3.code
void Tracking::Track() { // 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点重投影误差即可得到位姿 bOK = TrackReferenceKeyFrame(); } else { // 用最近的普通帧来跟踪当前的普通帧 // 根据恒速模型设定当前帧的初始位姿 // 通过投影的方式在参考帧中找当前帧特征点的匹配点 // 优化每个特征点所对应3D点的投影误差即可得到位姿 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) { // Step 2.2 如果跟踪正常，使用恒速模型 或 参考关键帧跟踪 // In last frame we tracked enough MapPoints in the map if(!mVelocity.empty()) { bOK = TrackWithMotionModel(); // ? 为了和前面模式统一，这个地方是不是应该加上 // if(!bOK) // bOK = TrackReferenceKeyFrame(); } else { // 如果恒速模型不被满足,那么就只能够通过参考关键帧来定位 bOK = TrackReferenceKeyFrame(); } } 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. // mbVO为true，表明此帧匹配了很少（小于10）的地图点，要跪的节奏，既做跟踪又做重定位 //MM=Motion Model,通过运动模型进行跟踪的结果 bool bOKMM = false; //通过重定位方法来跟踪的结果 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; //? 疑似bug！这段代码是不是重复增加了观测次数？后面 TrackLocalMap 函数中会有这些操作 // 如果当前帧匹配的3D点很少，增加当前可视地图点的被观测次数 if(mbVO) { // 更新当前帧的地图点被观测次数 for(int i =0; i<mCurrentFrame.N; i++) { //如果这个特征点形成了地图点,并且也不是外点的时候 if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]) { //增加能观测到该地图点的帧数 mCurrentFrame.mvpMapPoints[i]->IncreaseFound(); } } } } else if(bOKReloc) { // 只要重定位成功整个跟踪过程正常进行（重定位与跟踪，更相信重定位） mbVO = false; } //有一个成功我们就认为执行成功了 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后进行Pose优化 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; // 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 = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc 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++) { MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; if(pMP) 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; delete pMP; } // 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint // 不能够直接执行这个是因为其中存储的都是指针,之前的操作都是为了避免内存泄露 mlpTemporalPoints.clear(); // Check if we need to insert a new keyframe // Step 8：检测并插入关键帧，对于双目或RGB-D会产生新的地图点 if(NeedNewKeyFrame()) CreateNewKeyFrame(); // 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来审判他们是不是真正的外点 // 但是估计下一帧位姿的时候我们不想用这些外点，所以删掉 // Step 9 删除那些在bundle adjustment中检测为outlier的地图点 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) { //如果地图中的关键帧信息过少的话,直接重新进行初始化了 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 // 如果跟踪失败，则相对位姿使用上一次值 mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); mlpReferences.push_back(mlpReferences.back()); mlFrameTimes.push_back(mlFrameTimes.back()); mlbLost.push_back(mState==LOST); } }// Tracking
4.函数解析
当一帧传入跟踪线程时，我们先判断跟踪线程的状态：
跟踪状态有如下几种：
///跟踪状态类型 enum eTrackingState{ SYSTEM_NOT_READY=-1, //系统没有准备好的状态,一般就是在启动后加载配置文件和词典文件时候的状态 NO_IMAGES_YET=0, //当前无图像 NOT_INITIALIZED=1, //有图像但是没有完成初始化 OK=2, //正常时候的工作状态 LOST=3 //系统已经跟丢了的状态 }; ///跟踪状态 eTrackingState mState; ///上一帧的跟踪状态.这个变量在绘制当前帧的时候会被使用到 eTrackingState mLastProcessedState;如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态。
如果我们的SLAM系统没有初始化，则进入SLAM系统初始化函数Tracking::MonocularInitialization并更新帧绘制器中存储的最新状态。
如果地图初始化成功的话，我们先检查上一帧中的地图点是否需要被替换。
void Tracking::CheckReplacedInLastFrame() { for(int i =0; i<mLastFrame.N; i++) { MapPoint* pMP = mLastFrame.mvpMapPoints[i]; //如果这个地图点存在 if(pMP) { // 获取其是否被替换,以及替换后的点 // 这也是程序不直接删除这个地图点删除的原因 MapPoint* pRep = pMP->GetReplaced(); if(pRep) { //然后替换一下 mLastFrame.mvpMapPoints[i] = pRep; } } } }
运动模型是空的或刚完成重定位，跟踪参考关键帧；
如果运动模型为空，说明是刚初始化开始，或者已经跟丢了；如果当前帧紧紧地跟着在重定位的帧的后面，我们将重定位帧来恢复位姿。
ORB-SLAM2 ---- Tracking::TrackReferenceKeyFrame函数解析
https://blog.csdn.net/qq_41694024/article/details/126467663 否则用恒速模型追踪，用最近的普通帧来跟踪当前的普通帧。
ORB-SLAM2 ---- Tracking::TrackWithMotionModel函数解析
ORB-SLAM2 ---- Tracking::TrackReferenceKeyFrame函数解析
ORB-SLAM2 ---- Tracking::Relocalization函数解析
若得到了当前帧的位姿，将最新的关键帧作为当前帧的参考关键帧。
在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿。
ORB-SLAM2 --- Tracking::TrackLocalMap函数
更新显示线程中的图像、特征点、地图点等信息。
成功追踪时才考虑生成关键帧的问题：
跟踪成功，更新恒速运动模型。
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 = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc mVelocity = mCurrentFrame.mTcw*LastTwc; } else //否则速度为空 mVelocity = cv::Mat();cv::Mat mTcw; ///< 相机姿态 世界坐标系到相机坐标坐标系的变换矩阵,是我们常规理解中的相机位姿/** * @brief Get the Rotation Inverse object * mRwc存储的是从当前相机坐标系到世界坐标系所进行的旋转，而我们一般用的旋转则说的是从世界坐标系到当前相机坐标系的旋转 * @return 返回从当前帧坐标系到世界坐标系的旋转 */ inline cv::Mat GetRotationInverse() { return mRwc.clone(); } // Returns the camera center. /** * @brief 返回位于当前帧位姿时,相机的中心 * * @return cv::Mat 相机中心在世界坐标系下的3D点坐标 */ inline cv::Mat GetCameraCenter() { return mOw.clone(); }如果上一帧的相机位姿不为空，更新速度mVelocity，意义是表示上一帧到当前帧的变换。
清除观测不到的地图点。
for(int i=0; i<mCurrentFrame.N; i++) { MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; if(pMP) if(pMP->Observations()<1) { mCurrentFrame.mvbOutlier[i] = false; mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL); } }清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd），上一步是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除。临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中。
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++) { MapPoint* pMP = *lit; delete pMP; }检测并插入关键帧，对于双目或RGB-D会产生新的地图点。
ORB-SLAM2 --- Tracking::NeedNewKeyFrame函数
删除那些在bundle adjustment中检测为outlier的地图点。
for(int i=0; i<mCurrentFrame.N;i++) { // 这里第一个条件还要执行判断是因为, 前面的操作中可能删除了其中的地图点 if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i]) mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL); }如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Reset。如果地图中的关键帧信息过少的话,直接重新进行初始化了。
if(mState==LOST) { //如果地图中的关键帧信息过少的话,直接重新进行初始化了 if(mpMap->KeyFramesInMap()<=5) { cout << "Track lost soon after initialisation, reseting..." << endl; mpSystem->Reset(); return; } }确保已经设置了参考关键帧，保存上一帧的数据，当前帧变上一帧。
if(!mCurrentFrame.mpReferenceKF) mCurrentFrame.mpReferenceKF = mpReferenceKF; // 保存上一帧的数据,当前帧变上一帧 mLastFrame = Frame(mCurrentFrame);最后记录位姿信息，用于最后保存所有的轨迹。
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 // 如果跟踪失败，则相对位姿使用上一次值 mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); mlpReferences.push_back(mlpReferences.back()); mlFrameTimes.push_back(mlFrameTimes.back()); mlbLost.push_back(mState==LOST); }
https://mp.csdn.net/mp_blog/creation/editor/127816993
3464
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
