1、参考关键帧追踪模式

bool Tracking::TrackReferenceKeyFrame()

对参考关键帧中的路标点进行跟踪。在Tracking线程中，每传入一帧，都会进行位姿优化。
以上一帧的位姿为当前位姿进行优化。
（1）计算当前帧的词袋

mCurrentFrame.ComputeBoW();

（2）通过词袋加速算法，获得当前帧的特征点与参考帧的路标点间的联系。当匹配数小于15时，退出。

    // Step 2：通过特征点的BoW加快当前帧与参考帧之间的特征点匹配// 特征点的匹配关系由MapPoints进行维护int nmatches = matcher.SearchByBoW(mpReferenceKF,          //参考关键帧mCurrentFrame,          //当前帧vpMapPointMatches);     //存储匹配关系//这里的匹配数超过15才继续if(nmatches<15)return false;

（3）以上一帧的位姿为当前位姿，通过重投影误差进行优化

mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些
// Step 4:通过优化3D-2D的重投影误差来获得位姿Optimizer::PoseOptimization(&mCurrentFrame);

（4）删除外点
根据BA优化的结果，判断那些点是外点。如果是外点的话，就删除其痕迹：路标点的指针置为NULL等。如果是内点的话，计数器加一。

 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++;}}

（4）如果内点的数目大于10的话，返回追踪成功。此时当前帧的位姿已经被优化。

2、恒速运动模型

bool Tracking::TrackWithMotionModel

&emps;根据前两帧的运动估计当前帧的位姿，作为优化初值。

（1）更新上一帧位姿。如果是双目、RGB-D相机的话，还会将未创建为地图点的三维点（双目相机可获得深度信息）作为临时地图点加入到地图中。

UpdateLastFrame();

（2）根据恒速运动模型获得当前帧的位姿。mVelocity为估计的上一帧到当前帧的位姿变换Tcl。Tcl*Tlw=Tcw

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

（3）根据上一帧特征点对应地图点进行投影匹配，返回匹配上的个数。
（4）如果匹配个数不够的话，扩大搜索半径。如果匹配个数（特征点-特征点的联系、特征点-路标点的联系含义是一样的）还不够，直接返回。
（4）仅优化位姿。
（5）去除外点，根据剩余的匹配数量，返回是否匹配成功。追踪模式的要求更加严格（个人认为是追踪模式只有位姿约束，因此需要更多的位姿进行约束。）。

重定位（最后的拯救措施）

以PNP估计结果作为优化初值。

（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;

（2）使用ORB特征点检测候选关键帧，对合适的关键帧进行优化
vvpMapPointMatches：存放当前帧对每个关键帧成功匹配上的路标点的数量。
vbDiscarded：某个候选关键帧是否要放弃。
1）使用词袋快速匹配，匹配成功数量小于15的话，直接放弃

            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);// 如果和当前帧的匹配数小于15,那么只能放弃这个关键帧if(nmatches<15){vbDiscarded[i] = true;continue;}

2）对匹配的路标点数目大于15的每个候选关键帧，设置pnp求解参数

             // 参数为当前帧、当前帧的特征点索引---->路标点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++;

（3）遍历候选关键帧，进行RANSAC+EPNP迭代估计相机位姿。

            // Step 4.1：通过EPnP算法估计姿态，迭代5次PnPsolver* pSolver = vpPnPsolvers[i];// 获得初步估计的相机位姿cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

（4）对每个候选关键帧的位姿进行优化

int nGood = Optimizer::PoseOptimization(&mCurrentFrame);

（5）如果匹配到的内点数目不够，则将参考关键帧中的地图点投影至当前帧中，形成新的匹配。如果够了的话，直接结束循环。

之前是通过词袋加速算法获得了当前帧中的匹配到的路标点，会有遗漏。这里将参考关键帧中的所有路标点再次进行投影。
（6）再次进行优化

nGood = Optimizer::PoseOptimization(&mCurrentFrame);

（7）如果不行的话，再用严格的标准投影一遍

                        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;}//如果还是不能够满足就放弃了}

（8）如果还行，只能放弃

ORB_SLAM2中Tracking线程的三种追踪方式相关推荐

ORB_SLAM2中Tracking线程
Tracking线程是ORB_SLAM2的主线程.在System.cc中,使用构造函数进行了初始化,开启了三个线程. 可执行程序->System构造函数(初始化三个线程)->处理输入的 ...
java 终止方法_Java中终止线程的三种方法
Java中终止线程的三种方法 Thread.stop, Thread.suspend, Thread.resume 和Runtime.runFinalizersOnExit 这些终止线程运行的方法已经 ...
线程的三种创建方式和他们的优缺点
文章目录一.线程的三种创建方式 1.继承Thread 2.实现Runable接口 3.实现Callable接口二.三种创建方式的优缺点 1.使用Runnable接口比使用继承Thread的优势 2 ...
oracle if=,oracle中if/else的三种实现方式详解
1.标准sql规范 1.单个IF IF v=... THEN END IF; 2.IF ... ELSE IF v=... THEN ELSE t....; END IF; 3.多个IF IF v=. ...
Java中List集合的三种遍历方式(全网最详)
Map集合:链接: Map集合的五种遍历方式及Treemap方法 Set集合:链接: Java中遍历Set集合的三种方法 TreeSet集合:链接: Java深入了解TreeSet,和迭代器遍历方法 ...
MyEclipse网站服务器,MyEclipse中web服务器的三种配置方式
初学Javaweb开发的人们都会遇到一个问题,就是服务器环境的搭建配置问题.下面介绍三种服务器的搭建方式. 直接修改server.xml文件当你写了一个web应用程序(jsp/servlet),想通 ...
java如何实现线程_java中线程的三种实现方式
packagecom.four.day01;importjava.util.concurrent.Callable;importjava.util.concurrent.ExecutionExcept ...
线程的三种创建方式以及代码实现
线程和进程的概念一个进程可以有多个线程程序:是指令和数据的有序集合(静态的) 进程:是执行程序的一次执行过程(动态的),是系统资源分配的单位.在操作系统中运行的程序就是进程. 通常在一个进程中可以 ...
线程池三种创建方式和自定义线程池ThreadPoolExecutor
线程池的优势: 线程池做的工作只要是控制运行的线程数量,处理过程中将任务放入队列,然后在线程创建后启动这些任务,如果线程数量超过了最大数量,超出数量的线程排队等候,等其他线程执行完毕,再从队列中取出任 ...

ORB_SLAM2中Tracking线程的三种追踪方式

1、参考关键帧追踪模式

2、恒速运动模型

重定位（最后的拯救措施）

ORB_SLAM2中Tracking线程的三种追踪方式相关推荐

最新文章

热门文章