ORB-SLAM2双目开源框架（2） Tracking解析

Tracking类

Tracking类主要运行Track()函数，完成的主要内容有：双目初始化，跟踪参考关键帧或者跟踪上一帧，重定位，更新局部地图，跟踪局部地图，判断并创建关键帧。

双目初始化

双目初始化是指构建第一帧和初始的地图点信息。后续的位姿估计和地图扩展都是基于双目初始化的结果。该部分由 void Tracking:: StereoInitialization() 实现，通过当前帧包含特征点的数量判断是否初始化成功，而后设置初始位姿，构建新关键帧和地图点，并建立关键帧和地图点之间的关联。

void Tracking::StereoInitialization()
{// 初始化要求当前帧的特征点超过500if(mCurrentFrame.N>500){// Set Frame pose to the origin// 设定初始位姿为单位旋转，0平移//设置第一帧位姿mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));// Create KeyFrame// 将当前帧构造为初始关键帧// mCurrentFrame的数据类型为Frame// KeyFrame包含Frame、地图3D点、以及BoW// KeyFrame里有一个mpMap，Tracking里有一个mpMap，而KeyFrame里的mpMap都指向Tracking里的这个mpMap// KeyFrame里有一个mpKeyFrameDB，Tracking里有一个mpKeyFrameDB，而KeyFrame里的mpMap都指向Tracking里的这个mpKeyFrameDB// 提问: 为什么要指向Tracking中的相应的变量呢? -- 因为Tracking是主线程，是它创建和加载的这些模块KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);// Insert KeyFrame in the map// KeyFrame中包含了地图、反过来地图中也包含了KeyFrame，相互包含// 在地图中添加该初始关键帧mpMap->AddKeyFrame(pKFini);// Create MapPoints and asscoiate to KeyFrame，创建地图点，建立地图点和关键帧之间的关系// 为每个特征点构造MapPointfor(int i=0; i<mCurrentFrame.N;i++){//只有具有正深度的点才会被构造地图点float z = mCurrentFrame.mvDepth[i];if(z>0){// 通过反投影得到该特征点的世界坐标系下3D坐标cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);// 将3D点构造为MapPointMapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);// 为该MapPoint添加属性：// a.观测到该MapPoint的关键帧// b.该MapPoint的描述子// c.该MapPoint的平均观测方向和深度范围// a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到pNewMP->AddObservation(pKFini,i);// b.从众多观测到该MapPoint的特征点中挑选区分度最高的描述子             pNewMP->ComputeDistinctiveDescriptors();// c.更新该MapPoint平均观测方向以及观测距离的范围pNewMP->UpdateNormalAndDepth();// 在地图中添加该MapPointmpMap->AddMapPoint(pNewMP);// 表示该KeyFrame的哪个特征点可以观测到哪个3D点pKFini->AddMapPoint(pNewMP,i);// 将该MapPoint添加到当前帧的mvpMapPoints中// 为当前Frame的特征点与MapPoint之间建立索引mCurrentFrame.mvpMapPoints[i]=pNewMP;}}

跟踪参考关键帧
由void TrackReferenceKeyFrame()实现，主要完成的内容有：计算当前帧的词袋，使用词袋模型与参考关键帧进行特征匹配；得到参与优化的地图点集合vpMapPointMatches,设置当前帧初始的位姿估计为上一帧的位姿估计，然后进行位姿估计。位姿估计完成后，对不合格地图点进行剔除操作。当跟踪的有效地图点个数大于10时，认为跟踪参考关键帧成功，即得到有效的当前帧位姿估计。

PnP(Perspective-n-Point)是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时,如何估计相机所在的位姿。

通俗的讲，PnP问题就是在已知世界坐标系下N个空间点的真实坐标以及这些空间点在图像上的投影，如何计算相机所在的位姿。

bool Tracking::TrackReferenceKeyFrame()
{// Compute Bag of Words vector// Step 1：将当前帧的描述子转化为BoW向量mCurrentFrame.ComputeBoW();// We perform first an ORB matching with the reference keyframe// If enough matches are found we setup a PnP solverORBmatcher matcher(0.7,true);vector<MapPoint*> vpMapPointMatches;// Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配int nmatches = matcher.SearchByBoW(mpReferenceKF,          //参考关键帧mCurrentFrame,          //当前帧vpMapPointMatches);     //存储匹配关系// 匹配数目小于15，认为跟踪失败if(nmatches<15)return false;// Step 3:将上一帧的位姿态作为当前帧位姿的初始值mCurrentFrame.mvpMapPoints = vpMapPointMatches;mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些// Step 4:通过优化3D-2D的重投影误差来获得位姿Optimizer::PoseOptimization(&mCurrentFrame);// Discard outliers// Step 5：剔除优化后的匹配点中的外点//之所以在优化之后才剔除外点，是因为在优化的过程中就有了对这些外点的标记int nmatchesMap = 0;for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){//如果对应到的某个特征点是外点if(mCurrentFrame.mvbOutlier[i]){//清除它在当前帧中存在过的痕迹MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);mCurrentFrame.mvbOutlier[i]=false;pMP->mbTrackInView = false;pMP->mnLastFrameSeen = mCurrentFrame.mnId;nmatches--;}else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)//匹配的内点计数++nmatchesMap++;}}// 跟踪成功的数目超过10才认为跟踪成功，否则跟踪失败return nmatchesMap>=10;
}

其中，位姿估计 int Optimizer::PoseOptimization(Frame *pFrame)具体实现如下：

/** @brief Pose Only Optimization* * 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) \n* 只优化Frame的Tcw，不优化MapPoints的坐标* * 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw* 2. Edge:*     - g2o::EdgeSE3ProjectXYZOnlyPose()，BaseUnaryEdge*         + Vertex：待优化当前帧的Tcw*         + measurement：MapPoint在当前帧中的二维位置(u,v)*         + InfoMatrix: invSigma2(与特征点所在的尺度有关)*     - g2o::EdgeStereoSE3ProjectXYZOnlyPose()，BaseUnaryEdge*         + Vertex：待优化当前帧的Tcw*         + measurement：MapPoint在当前帧中的二维位置(ul,v,ur)*         + InfoMatrix: invSigma2(与特征点所在的尺度有关)** @param   pFrame Frame* @return  inliers数量*/
int Optimizer::PoseOptimization(Frame *pFrame)
{// 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位// Step 1：构造g2o优化器, BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维g2o::SparseOptimizer optimizer;g2o::BlockSolver_6_3::LinearSolverType * linearSolver;linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);//设置非线性优化求解器LMg2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);optimizer.setAlgorithm(solver);// 输入的帧中,有效的,参与优化过程的2D-3D点对int nInitialCorrespondences=0;// Set Frame vertex//设置当前帧位姿节点，为非固定节点，作为待优化变量并加入优化器// Step 2：添加顶点：待优化当前帧的Tcwg2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));// 设置idvSE3->setId(0);    // 要优化的变量，所以不能固定vSE3->setFixed(false);optimizer.addVertex(vSE3);// Set MapPoint verticesconst int N = pFrame->N;// for Monocularvector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;vector<size_t> vnIndexEdgeMono;vpEdgesMono.reserve(N);vnIndexEdgeMono.reserve(N);// for Stereovector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;vector<size_t> vnIndexEdgeStereo;vpEdgesStereo.reserve(N);vnIndexEdgeStereo.reserve(N);// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991const float deltaMono = sqrt(5.991);  // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815   const float deltaStereo = sqrt(7.815);     // Step 3：添加一元边{// 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误unique_lock<mutex> lock(MapPoint::mGlobalMutex);// 遍历当前地图中的所有地图点for(int i=0; i<N; i++){MapPoint* pMP = pFrame->mvpMapPoints[i];// 如果这个地图点还存在没有被剔除掉if(pMP){// Monocular observation// 单目情况，单目类型观测：地图点只有在左图像有对应特征点if(pFrame->mvuRight[i]<0){nInitialCorrespondences++;pFrame->mvbOutlier[i] = false;// 对这个地图点的观测Eigen::Matrix<double,2,1> obs;const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];//观测为左图像中的特征点坐标obs << kpUn.pt.x, kpUn.pt.y;// 新建单目的边，一元边，误差为观测特征点坐标减去投影点的坐标//设置观测边约束g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();// 设置边的顶点e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));e->setMeasurement(obs);// 这个点的可信程度和特征点所在的图层有关//设置该边的信息矩阵，与该特征点所在的金字塔层数有关const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);// 在这里使用了鲁棒核函数g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaMono);    // 前面提到过的卡方阈值// 设置相机内参和地图点3D坐标，用于计算重投影误差e->fx = pFrame->fx;e->fy = pFrame->fy;e->cx = pFrame->cx;e->cy = pFrame->cy;// 地图点的空间位置,作为迭代的初始值cv::Mat Xw = pMP->GetWorldPos();e->Xw[0] = Xw.at<float>(0);e->Xw[1] = Xw.at<float>(1);e->Xw[2] = Xw.at<float>(2);//将该边加入优化器optimizer.addEdge(e);vpEdgesMono.push_back(e);vnIndexEdgeMono.push_back(i);}else  // Stereo observation 双目{nInitialCorrespondences++;pFrame->mvbOutlier[i] = false;//SET EDGE//设置双目类型边// 观测多了一项右目的坐标Eigen::Matrix<double,3,1> obs;// 这里和单目不同const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];const float &kp_ur = pFrame->mvuRight[i];obs << kpUn.pt.x, kpUn.pt.y, kp_ur;// 这里和单目不同// 新建边，一元边，误差为观测特征点坐标减去投影点的坐标g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();// 这里和单目不同e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));e->setMeasurement(obs);// 置信程度主要是看左目特征点所在的图层//设置边的信息矩阵和鲁棒代价函数，设置相机参数（fx,fy,cx,cy）地图点3D坐标(e->Xw),用于计算重投影误差const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;e->setInformation(Info);g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaStereo);e->fx = pFrame->fx;e->fy = pFrame->fy;e->cx = pFrame->cx;e->cy = pFrame->cy;e->bf = pFrame->mbf;cv::Mat Xw = pMP->GetWorldPos();e->Xw[0] = Xw.at<float>(0);e->Xw[1] = Xw.at<float>(1);e->Xw[2] = Xw.at<float>(2);optimizer.addEdge(e);vpEdgesStereo.push_back(e);vnIndexEdgeStereo.push_back(i);} // 根据单目和双目不同的相机输入执行不同的操作过程}}} // 离开临界区// 如果没有足够的匹配点,那么就只好放弃了if(nInitialCorrespondences<3)return 0;// We perform 4 optimizations, after each optimization we classify observation as inlier/outlier// At the next optimization, outliers are not included, but at the end they can be classified as inliers again.// Step 4：开始优化，总共优化四次，每次优化迭代10次,每次优化后，将观测分为outlier和inlier，outlier不参与下次优化// 由于每次优化后是对所有的观测进行outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）const float chi2Mono[4]={5.991,5.991,5.991,5.991};          // 单目const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};       // 双目const int its[4]={10,10,10,10};// 四次迭代，每次迭代的次数// bad 的地图点个数int nBad=0;// 一共进行四次优化for(size_t it=0; it<4; it++){vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));// 其实就是初始化优化器,这里的参数0就算是不填写,默认也是0,也就是只对level为0的边进行优化optimizer.initializeOptimization(0);// 开始优化，优化10次optimizer.optimize(its[it]);nBad=0;// 优化结束,开始遍历参与优化的每一条误差边(单目)for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++){g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];const size_t idx = vnIndexEdgeMono[i];// 如果这条误差边是来自于outlierif(pFrame->mvbOutlier[idx]){e->computeError(); }// 就是error*\Omega*error,表征了这个点的误差大小(考虑置信度以后)const float chi2 = e->chi2();if(chi2>chi2Mono[it]){                pFrame->mvbOutlier[idx]=true;e->setLevel(1);                 // 设置为outlier , level 1 对应为外点,上面的过程中我们设置其为不优化nBad++;}else{pFrame->mvbOutlier[idx]=false;e->setLevel(0);                 // 设置为inlier, level 0 对应为内点,上面的过程中我们就是要优化这些关系}if(it==2)e->setRobustKernel(0); // 除了前两次优化需要RobustKernel以外, 其余的优化都不需要 -- 因为重投影的误差已经有明显的下降了} // 对单目误差边的处理// 同样的原理遍历双目的误差边for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++){g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];const size_t idx = vnIndexEdgeStereo[i];if(pFrame->mvbOutlier[idx]){e->computeError();}const float chi2 = e->chi2();if(chi2>chi2Stereo[it]){pFrame->mvbOutlier[idx]=true;e->setLevel(1);nBad++;}else{                e->setLevel(0);pFrame->mvbOutlier[idx]=false;}if(it==2)e->setRobustKernel(0);} // 对双目误差边的处理if(optimizer.edges().size()<10)break;} // 一共要进行四次优化// Recover optimized pose and return number of inliers// Step 5 得到优化后的当前帧的位姿g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();cv::Mat pose = Converter::toCvMat(SE3quat_recov);pFrame->SetPose(pose);// 并且返回内点数目return nInitialCorrespondences-nBad;
}

跟踪上一帧
当得到运动模型后，采用跟踪上一帧的方式进行当前帧的位姿估计。该过程由bool Tracking::TrackWithMotionModel()实现，主要完成的工作有：根据运动模型，设置当前帧的姿态估计；根据地图点的重投影坐标，与上一帧进行特征匹配，得到参数与优化的地图点集合vpMapPointMatches。后续的优化和地图点剔除工作与跟踪参考关键帧相同。

如果跟踪上一帧失败，转而进行跟踪参考关键帧。

                    // 用最近的普通帧来跟踪当前的普通帧// 根据恒速模型设定当前帧的初始位姿// 通过投影的方式在参考帧中找当前帧特征点的匹配点// 优化每个特征点所对应3D点的投影误差即可得到位姿bOK = TrackWithMotionModel();if(!bOK)//根据恒速模型失败了，只能根据参考关键帧来跟踪bOK = TrackReferenceKeyFrame();}

/*** @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪* Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点* Step 2：根据上一帧特征点对应地图点进行投影匹配* Step 3：优化当前帧位姿* Step 4：剔除地图点中外点* @return 如果匹配数大于10，认为跟踪成功，返回true*/
bool Tracking::TrackWithMotionModel()
{// 最小距离 < 0.9*次小距离 匹配成功，检查旋转ORBmatcher matcher(0.9,true);// Update last frame pose according to its reference keyframe// Create "visual odometry" points// Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点UpdateLastFrame();// Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿。mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);// 清空当前帧的地图点fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));// Project points seen in previous frame// 设置特征匹配过程中的搜索半径int th;if(mSensor!=System::STEREO)th=15;//单目elseth=7;//双目// Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);// If few matches, uses a wider window search// 如果匹配点太少，则扩大搜索半径再来一次if(nmatches<20){fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th}// 如果还是不能够获得足够的匹配点,那么就认为跟踪失败if(nmatches<20)return false;// Optimize frame pose with all matches// Step 4：利用3D-2D投影关系，优化当前帧位姿Optimizer::PoseOptimization(&mCurrentFrame);// Discard outliers// Step 5：剔除地图点中外点int nmatchesMap = 0;for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){if(mCurrentFrame.mvbOutlier[i]){// 如果优化后判断某个地图点是外点，清除它的所有关系MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);mCurrentFrame.mvbOutlier[i]=false;pMP->mbTrackInView = false;pMP->mnLastFrameSeen = mCurrentFrame.mnId;nmatches--;}else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)// 累加成功匹配到的地图点数目nmatchesMap++;}}    if(mbOnlyTracking){// 纯定位模式下：如果成功追踪的地图点非常少,那么这里的mbVO标志就会置位mbVO = nmatchesMap<10;return nmatches>20;}// Step 6：匹配超过10个点就认为跟踪成功return nmatchesMap>=10;
}

重定位
当跟踪上一帧和跟踪参考关键帧均失败时，ORB-SLAM2进行相机重定位。重定位功能由bool Tracking ::Relocalization()实现。主要步骤有：对当前帧提取词袋，根据该词袋信息，从词袋库中选取候选关键帧集合。与跟踪参考关键帧类似，当前帧与候选关键帧通过词袋进行特征匹配和位姿估计，当满足约束的地图点数量达到阈值时，则认为重定位成功。

内外点之分最简单的说法就是是否符合当前位姿的判断：如果根据当前位姿，之前帧二维特征点所恢复出的地图点重投影到当前帧与实际的二维特征点匹配不上了，那么认为这个是质量差的点是outlier，抛弃掉，如果能匹配上，那就是inlier，保留。

/*** @details 重定位过程* @return true * @return false * * Step 1：计算当前帧特征点的词袋向量* Step 2：找到与当前帧相似的候选关键帧* Step 3：通过BoW进行匹配* Step 4：通过EPnP算法估计姿态* Step 5：通过PoseOptimization对姿态进行优化求解* Step 6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解*/
bool Tracking::Relocalization()
{// Compute Bag of Words Vector// Step 1：计算当前帧特征点的词袋向量mCurrentFrame.ComputeBoW();// Relocalization is performed when tracking is lost// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation// Step 2：用词袋找到与当前帧相似的候选关键帧vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);// 如果没有候选关键帧，则退出if(vpCandidateKFs.empty())return false;const int nKFs = vpCandidateKFs.size();// We perform first an ORB matching with each candidate// If enough matches are found we setup a PnP solverORBmatcher matcher(0.75,true);//每个关键帧的解算器vector<PnPsolver*> vpPnPsolvers;vpPnPsolvers.resize(nKFs);//每个关键帧和当前帧中特征点的匹配关系vector<vector<MapPoint*> > vvpMapPointMatches;vvpMapPointMatches.resize(nKFs);//放弃某个关键帧的标记vector<bool> vbDiscarded;vbDiscarded.resize(nKFs);//有效的候选关键帧数目int nCandidates=0;// Step 3：遍历所有的候选关键帧，通过词袋进行快速匹配，用匹配结果初始化PnP Solverfor(int i=0; i<nKFs; i++){KeyFrame* pKF = vpCandidateKFs[i];if(pKF->isBad())vbDiscarded[i] = true;else{// 当前帧和候选关键帧用BoW进行快速匹配，匹配结果记录在vvpMapPointMatches，nmatches表示匹配的数目int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);// 如果和当前帧的匹配数小于15,那么只能放弃这个关键帧if(nmatches<15){vbDiscarded[i] = true;continue;}else{// 如果匹配数目够用，用匹配结果初始化EPnPsolver// 为什么用EPnP? 因为计算复杂度低，精度高PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);pSolver->SetRansacParameters(0.99,   //用于计算RANSAC迭代次数理论值的概率10,     //最小内点数, 但是要注意在程序中实际上是min(给定最小内点数,最小集,内点数理论值),不一定使用这个300,    //最大迭代次数4,      //最小集(求解这个问题在一次采样中所需要采样的最少的点的个数,对于Sim3是3,EPnP是4),参与到最小内点数的确定过程中0.5,    //这个是表示(最小内点数/样本总数);实际上的RANSAC正常退出的时候所需要的最小内点数其实是根据这个量来计算得到的5.991); // 自由度为2的卡方检验的阈值,程序中还会根据特征点所在的图层对这个阈值进行缩放vpPnPsolvers[i] = pSolver;nCandidates++;}}}// Alternatively perform some iterations of P4P RANSAC// Until we found a camera pose supported by enough inliers// 这里的 P4P RANSAC是Epnp，每次迭代需要4个点// 是否已经找到相匹配的关键帧的标志bool bMatch = false;ORBmatcher matcher2(0.9,true);// Step 4: 通过一系列操作,直到找到能够匹配上的关键帧// 为什么搞这么复杂？答：是担心误闭环while(nCandidates>0 && !bMatch){//遍历当前所有的候选关键帧for(int i=0; i<nKFs; i++){// 忽略放弃的if(vbDiscarded[i])continue;//内点标记vector<bool> vbInliers;     //内点数int nInliers;// 表示RANSAC已经没有更多的迭代次数可用 -- 也就是说数据不够好，RANSAC也已经尽力了。。。bool bNoMore;// Step 4.1：通过EPnP算法估计姿态，迭代5次PnPsolver* pSolver = vpPnPsolvers[i];cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);// If Ransac reachs max. iterations discard keyframe// bNoMore 为true 表示已经超过了RANSAC最大迭代次数，就放弃当前关键帧if(bNoMore){vbDiscarded[i]=true;nCandidates--;}// If a Camera Pose is computed, optimizeif(!Tcw.empty()){//  Step 4.2：如果EPnP 计算出了位姿，对内点进行BA优化Tcw.copyTo(mCurrentFrame.mTcw);// EPnP 里RANSAC后的内点的集合set<MapPoint*> sFound;const int np = vbInliers.size();//遍历所有内点for(int j=0; j<np; j++){if(vbInliers[j]){mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];sFound.insert(vvpMapPointMatches[i][j]);}elsemCurrentFrame.mvpMapPoints[j]=NULL;}// 只优化位姿,不优化地图点的坐标，返回的是内点的数量int nGood = Optimizer::PoseOptimization(&mCurrentFrame);// 如果优化之后的内点数目不多，跳过了当前候选关键帧,但是却没有放弃当前帧的重定位if(nGood<10)continue;// 删除外点对应的地图点for(int io =0; io<mCurrentFrame.N; io++)if(mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);// If few inliers, search by projection in a coarse window and optimize again// Step 4.3：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解// 前面的匹配关系是用词袋匹配过程得到的if(nGood<50){// 通过投影的方式将关键帧中未匹配的地图点投影到当前帧中, 生成新的匹配int nadditional = matcher2.SearchByProjection(mCurrentFrame,          //当前帧vpCandidateKFs[i],      //关键帧sFound,                 //已经找到的地图点集合，不会用于PNP10,                     //窗口阈值，会乘以金字塔尺度100);                   //匹配的ORB描述子距离应该小于这个阈值// 如果通过投影过程新增了比较多的匹配特征点对if(nadditional+nGood>=50){// 根据投影匹配的结果，再次采用3D-2D pnp BA优化位姿nGood = Optimizer::PoseOptimization(&mCurrentFrame);// If many inliers but still not enough, search by projection again in a narrower window// the camera has been already optimized with many points// Step 4.4：如果BA后内点数还是比较少(<50)但是还不至于太少(>30)，可以挽救一下, 最后垂死挣扎 // 重新执行上一步 4.3的过程，只不过使用更小的搜索窗口// 这里的位姿已经使用了更多的点进行了优化,应该更准，所以使用更小的窗口搜索if(nGood>30 && nGood<50){// 用更小窗口、更严格的描述子阈值，重新进行投影搜索匹配sFound.clear();for(int ip =0; ip<mCurrentFrame.N; ip++)if(mCurrentFrame.mvpMapPoints[ip])sFound.insert(mCurrentFrame.mvpMapPoints[ip]);nadditional =matcher2.SearchByProjection(mCurrentFrame,          //当前帧vpCandidateKFs[i],      //候选的关键帧sFound,                 //已经找到的地图点，不会用于PNP3,                      //新的窗口阈值，会乘以金字塔尺度64);                    //匹配的ORB描述子距离应该小于这个阈值// Final optimization// 如果成功挽救回来，匹配数目达到要求，最后BA优化一下if(nGood+nadditional>=50){nGood = Optimizer::PoseOptimization(&mCurrentFrame);//更新地图点for(int io =0; io<mCurrentFrame.N; io++)if(mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io]=NULL;}//如果还是不能够满足就放弃了}}}// If the pose is supported by enough inliers stop ransacs and continue// 如果对于当前的候选关键帧已经有足够的内点(50个)了,那么就认为重定位成功if(nGood>=50){bMatch = true;// 只要有一个候选关键帧重定位成功，就退出循环，不考虑其他候选关键帧了break;}}}//一直运行,知道已经没有足够的关键帧,或者是已经有成功匹配上的关键帧}// 折腾了这么久还是没有匹配上，重定位失败if(!bMatch){return false;}else{// 如果匹配上了,说明当前帧重定位成功了(当前帧已经有了自己的位姿)// 记录成功重定位帧的id，防止短时间多次重定位mnLastRelocFrameId = mCurrentFrame.mnId;return true;}
}

跟踪局部地图

跟踪局部地图用于获取与当前帧更多的匹配地图点，得到更加准确的位姿估计。该部分由bool Tracking::TrackLocalMap() 实现，主要步骤有：更新局部关键地图，搜索局部地图点，位姿估计以及地图点删除，最后根据优化成功的地图点数量，判断跟踪局部地图是否成功。

/*** @brief 用局部地图进行跟踪，进一步优化位姿* * 1. 更新局部地图，包括局部关键帧和关键点* 2. 对局部MapPoints进行投影匹配* 3. 根据匹配对估计当前帧的姿态* 4. 根据姿态剔除误匹配* @return true if success* * Step 1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints * Step 2：在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪* Step 3：更新局部所有MapPoints后对位姿再次优化* Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果* Step 5：决定是否跟踪成功*/
bool Tracking::TrackLocalMap()
{// We have an estimation of the camera pose and some map points tracked in the frame.// We retrieve the local map and try to find matches to points in the local map.// Update Local KeyFrames and Local Points// Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPointsUpdateLocalMap();// Step 2：筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系SearchLocalPoints();// Optimize Pose// 在这个函数之前，在 Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 中都有位姿优化，// Step 3：前面新增了更多的匹配关系，BA优化得到更准确的位姿Optimizer::PoseOptimization(&mCurrentFrame);mnMatchesInliers = 0;// Update MapPoints Statistics// Step 4：更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目for(int i=0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){// 由于当前帧的地图点可以被当前帧观测到，其被观测统计量加1if(!mCurrentFrame.mvbOutlier[i]){// 找到该点的帧数mnFound 加 1mCurrentFrame.mvpMapPoints[i]->IncreaseFound();//查看当前是否是在纯定位过程if(!mbOnlyTracking){// 如果该地图点被相机观测数目nObs大于0，匹配内点计数+1// nObs： 被观测到的相机数目，单目+1，双目或RGB-D则+2if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)mnMatchesInliers++;}else// 记录当前帧跟踪到的地图点数目，用于统计跟踪效果mnMatchesInliers++;}// 如果这个地图点是外点,并且当前相机输入还是双目的时候,就删除这个点// ?单目就不管吗else if(mSensor==System::STEREO)  mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);}}// Decide if the tracking was succesful// More restrictive if there was a relocalization recently// Step 5：根据跟踪匹配数目及重定位情况决定是否跟踪成功// 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)return false;//如果是正常的状态话只要跟踪的地图点大于30个就认为成功了if(mnMatchesInliers<30)return false;elsereturn true;
}

其中，更新局部地图void Tracking::UpdateLocalMap()的实现主要包含两个部分：更新局部关键帧 void Tracking::UpdateLocalKeyFrames()和更新局部地图点 void Tracking::UpdateLocalPoints()。

更新局部关键帧的具体实现：首先得到与当前帧共视地图点的关键帧集合keyframeCounter，选择出与当前帧共视地图点最多pKFmax，将共视关键帧加入局部关键帧集合mvpLocalKeyFrames。将与共视关键帧共视地图点最多的10个关键帧，共视关键帧的子关键帧和父关键帧加入局部关键帧集合mvpLocalKeyFrames。在此过程使用mnTrackReferenceForFrame变量保证同一个关键帧不会多次加入到局部关键帧集合，且局部关键帧数量不超过80个。

/*** @brief 跟踪局部地图函数里，更新局部关键帧* 方法是遍历当前帧的地图点，将观测到这些地图点的关键帧和相邻的关键帧及其父子关键帧，作为mvpLocalKeyFrames* Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧 * Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧包括以下3种类型*      类型1：能观测到当前帧地图点的关键帧，也称一级共视关键帧*      类型2：一级共视关键帧的共视关键帧，称为二级共视关键帧*      类型3：一级共视关键帧的子关键帧、父关键帧* Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧*/
void Tracking::UpdateLocalKeyFrames()
{// Each map point vote for the keyframes in which it has been observed// Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧map<KeyFrame*,int> keyframeCounter;for(int i=0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(!pMP->isBad()){// 得到观测到该地图点的关键帧和该地图点在关键帧中的索引const map<KeyFrame*,size_t> observations = pMP->GetObservations();// 由于一个地图点可以被多个关键帧观测到,因此对于每一次观测,都对观测到这个地图点的关键帧进行累计投票for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)// 这里的操作非常精彩！// map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对// it->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数// 所以最后keyframeCounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧(mCurrentFrame)的地图点，也就是共视程度keyframeCounter[it->first]++;      }else{mCurrentFrame.mvpMapPoints[i]=NULL;}}}// 没有当前帧没有共视关键帧，返回if(keyframeCounter.empty())return;// 存储具有最多观测次数（max）的关键帧int max=0;KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);// Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有3种类型// 先清空局部关键帧mvpLocalKeyFrames.clear();// 先申请3倍内存，不够后面再加mvpLocalKeyFrames.reserve(3*keyframeCounter.size());// All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points// Step 2.1 类型1：能观测到当前帧地图点的关键帧作为局部关键帧 （将邻居拉拢入伙）（一级共视关键帧） for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++){KeyFrame* pKF = it->first;// 如果设定为要删除的，跳过if(pKF->isBad())continue;// 寻找具有最大观测数目的关键帧if(it->second>max){max=it->second;pKFmax=pKF;}// 添加到局部关键帧的列表里mvpLocalKeyFrames.push_back(it->first);// 用该关键帧的成员变量mnTrackReferenceForFrame 记录当前帧的id// 表示它已经是当前帧的局部关键帧了，可以防止重复添加局部关键帧pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;}// Include also some not-already-included keyframes that are neighbors to already-included keyframes// Step 2.2 遍历一级共视关键帧，寻找更多的局部关键帧 for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++){// Limit the number of keyframes// 处理的局部关键帧不超过80帧if(mvpLocalKeyFrames.size()>80)break;KeyFrame* pKF = *itKF;// 类型2:一级共视关键帧的共视（前10个）关键帧，称为二级共视关键帧（将邻居的邻居拉拢入伙）// 如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);// vNeighs 是按照共视程度从大到小排列for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++){KeyFrame* pNeighKF = *itNeighKF;if(!pNeighKF->isBad()){// mnTrackReferenceForFrame防止重复添加局部关键帧if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId){mvpLocalKeyFrames.push_back(pNeighKF);pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;//? 找到一个就直接跳出for循环？break;}}}// 类型3:将一级共视关键帧的子关键帧作为局部关键帧（将邻居的孩子们拉拢入伙）const set<KeyFrame*> spChilds = pKF->GetChilds();for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++){KeyFrame* pChildKF = *sit;if(!pChildKF->isBad()){if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId){mvpLocalKeyFrames.push_back(pChildKF);pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;//? 找到一个就直接跳出for循环？break;}}}// 类型3:将一级共视关键帧的父关键帧（将邻居的父母们拉拢入伙）KeyFrame* pParent = pKF->GetParent();if(pParent){// mnTrackReferenceForFrame防止重复添加局部关键帧if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId){mvpLocalKeyFrames.push_back(pParent);pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;//! 感觉是个bug！如果找到父关键帧会直接跳出整个循环break;}}}// Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧if(pKFmax){mpReferenceKF = pKFmax;mCurrentFrame.mpReferenceKF = mpReferenceKF;}
}

更新局部地图点则将局部关键帧观测到的地图点加入局部地图点集合mvpLocalMapPoints。

计算运动模型
计算当前帧和上一帧的位姿变换mVelocity,用于下一帧的初始位姿估计。

            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 = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwcmVelocity = mCurrentFrame.mTcw*LastTwc; }

判断关键帧

由bool Tracking::NeedNewKeyFrame()实现。
判断条件有
c1a: 当前帧与上一个关键帧之间的帧数差大于阈值mMaxFrames
c1b:局部建图线程空闲且当前帧与上一个关键帧之间的帧数差大于设定阈值mMinFrames
c1c:当前帧跟踪到的地图点数量小于参考关键帧匹配地图点数量的25%，或者当前帧，具有有效深度点的特征被匹配的数量小于给定阈值(100),没有被匹配的数量大于阈值(70)
c2:当前帧跟踪到的地图点数量小于参考关键帧匹配地图点数量的thRefRatio比例，或者具有有效深度点的特征被匹配的数量小于给定阈值(100),没有被匹配的数量大于给定阈值(70),同时当前帧匹配的地图点个数大于15。
当满足c2条件，以及c1a，c1b或者c1c中的一个条件时，若建图线程空闲，则可以创建关键帧；若建图线程不空闲，则中断建图线程后，若建图线程中的关键帧队列个数小于3，同样可以创建关键帧。否则。当前帧不作为关键帧。

/*** @brief 判断当前帧是否需要插入关键帧* * Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧* Step 2：如果距离上一次重定位比较近，或者关键帧数目超出最大限制，不插入关键帧* Step 3：得到参考关键帧跟踪到的地图点数量* Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧* Step 5：对于双目或RGBD摄像头，统计可以添加的有效地图点总数 和 跟踪到的地图点数量* Step 6：决策是否需要插入关键帧* @return true         需要* @return false        不需要*/
bool Tracking::NeedNewKeyFrame()
{// Step 1：纯VO模式下不插入关键帧if(mbOnlyTracking)return false;// If Local Mapping is freezed by a Loop Closure do not insert keyframes// Step 2：如果局部地图线程被闭环检测使用，则不插入关键帧if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())return false;// 获取当前地图中的关键帧数目const int nKFs = mpMap->KeyFramesInMap();// Do not insert keyframes if not enough frames have passed from last relocalisation// mCurrentFrame.mnId是当前帧的ID// mnLastRelocFrameId是最近一次重定位帧的ID// mMaxFrames等于图像输入的帧率//  Step 3：如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs>mMaxFrames)                                     return false;// Tracked MapPoints in the reference keyframe// Step 4：得到参考关键帧跟踪到的地图点数量// UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 // 地图点的最小观测次数int nMinObs = 3;if(nKFs<=2)nMinObs=2;// 参考关键帧地图点中观测的数目>= nMinObs的地图点数目int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);// Local Mapping accept keyframes?// Step 5：查询局部地图线程是否繁忙，当前能否接受新的关键帧bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();// Check how many "close" points are being tracked and how many could be potentially created.// Step 6：对于双目或RGBD摄像头，统计成功跟踪的近点的数量，如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧int nNonTrackedClose = 0;  //双目或RGB-D中没有跟踪到的近点int nTrackedClose= 0;       //双目或RGB-D中成功跟踪的近点（三维点）if(mSensor!=System::MONOCULAR){for(int i =0; i<mCurrentFrame.N; i++){// 深度值在有效范围内if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth){if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])nTrackedClose++;elsenNonTrackedClose++;}}}// 双目或RGBD情况下：跟踪到的地图点中近点太少 同时 没有跟踪到的三维点太多，可以插入关键帧了// 单目时，为falsebool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);// Step 7：决策是否需要插入关键帧// Thresholds// Step 7.1：设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧float thRefRatio = 0.75f;// 关键帧只有一帧，那么插入关键帧的阈值设置的低一点，插入频率较低if(nKFs<2)thRefRatio = 0.4f;//单目情况下插入关键帧的频率很高    if(mSensor==System::MONOCULAR)thRefRatio = 0.9f;// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion// Step 7.2：很长时间没有插入关键帧，可以插入const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle// Step 7.3：满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);// Condition 1c: tracking is weak// Step 7.4：在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertCloseconst bool c1c =  mSensor!=System::MONOCULAR &&             //只考虑在双目，RGB-D的情况(mnMatchesInliers<nRefMatches*0.25 ||       //当前帧和地图点匹配的数目非常少bNeedToInsertClose) ;                     //需要插入// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.// Step 7.5：和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);if((c1a||c1b||c1c)&&c2){// If the mapping accepts keyframes, insert keyframe.// Otherwise send a signal to interrupt BA// Step 7.6：local mapping空闲时可以直接插入，不空闲的时候要根据情况插入if(bLocalMappingIdle){//可以插入关键帧return true;}else{mpLocalMapper->InterruptBA();if(mSensor!=System::MONOCULAR){// 队列里不能阻塞太多关键帧// tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，// 然后localmapper再逐个pop出来插入到mspKeyFramesif(mpLocalMapper->KeyframesInQueue()<3)//队列中的关键帧数目不是很多,可以插入return true;else//队列中缓冲的关键帧数目太多,暂时不能插入return false;}else//对于单目情况,就直接无法插入关键帧了//? 为什么这里对单目情况的处理不一样?//回答：可能是单目关键帧相对比较密集return false;}}else//不满足上面的条件,自然不能插入关键帧return false;
}

创建关键帧

将当前帧设置为关键帧，并增加新的地图点，对当前帧的特征点深度进行排序，将具有有效深度（深度值小于mThDepth）的特征点转化为ie地图点，如果有效深度的特征点个数小于100，则继续增加地图点，直至增加个数达到100。

/*** @brief 创建新的关键帧* 对于非单目的情况，同时创建新的MapPoints* * Step 1：将当前帧构造成关键帧* Step 2：将当前关键帧设置为当前帧的参考关键帧* Step 3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints*/
void Tracking::CreateNewKeyFrame()
{// 如果局部建图线程关闭了,就无法插入关键帧if(!mpLocalMapper->SetNotStop(true))return;// Step 1：将当前帧构造成关键帧KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);// Step 2：将当前关键帧设置为当前帧的参考关键帧// 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧mpReferenceKF = pKF;mCurrentFrame.mpReferenceKF = pKF;// 这段代码和 Tracking::UpdateLastFrame 中的那一部分代码功能相同// Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作if(mSensor!=System::MONOCULAR){// 根据Tcw计算mRcw、mtcw和mRwc、mOwmCurrentFrame.UpdatePoseMatrices();// We sort points by the measured depth by the stereo/RGBD sensor.// We create all those MapPoints whose depth < mThDepth.// If there are less than 100 close points we create the 100 closest.// Step 3.1：得到当前帧有深度值的特征点（不一定是地图点）vector<pair<float,int> > vDepthIdx;vDepthIdx.reserve(mCurrentFrame.N);for(int i=0; i<mCurrentFrame.N; i++){float z = mCurrentFrame.mvDepth[i];if(z>0){// 第一个元素是深度,第二个元素是对应的特征点的idvDepthIdx.push_back(make_pair(z,i));}}if(!vDepthIdx.empty()){// Step 3.2：按照深度从小到大排序sort(vDepthIdx.begin(),vDepthIdx.end());// Step 3.3：从中找出不是地图点的生成临时地图点 // 处理的近点的个数int nPoints = 0;for(size_t j=0; j<vDepthIdx.size();j++){int i = vDepthIdx[j].second;bool bCreateNew = false;// 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(!pMP)bCreateNew = true;else if(pMP->Observations()<1){bCreateNew = true;mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);}// 如果需要就新建地图点，这里的地图点不是临时的，是全局地图中新建地图点，用于跟踪if(bCreateNew){cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);// 这些添加属性的操作是每次创建MapPoint后都要做的pNewMP->AddObservation(pKF,i);pKF->AddMapPoint(pNewMP,i);pNewMP->ComputeDistinctiveDescriptors();pNewMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pNewMP);mCurrentFrame.mvpMapPoints[i]=pNewMP;nPoints++;}else{// 因为从近到远排序，记录其中不需要创建地图点的个数nPoints++;}// Step 3.4：停止新建地图点必须同时满足以下条件：// 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）// 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出if(vDepthIdx[j].first>mThDepth && nPoints>100)break;}}}