Tracking::TrackWithMotionModel() ORB关于运动模型跟踪代码注释

ORBmatcher matcher(0.9,true);　设置初始化的一些参数，见具体定义

 

// 步骤1：对于双目或rgbd摄像头，根据深度值为上一关键帧生成新的MapPoints

// （跟踪过程中需要将当前帧与上一帧进行特征点匹配，将上一帧的MapPoints投影到当前帧可以缩小匹配范围）

// 在跟踪过程中，去除outlier的MapPoint，如果不及时增加MapPoint会逐渐减少

// 这个函数的功能就是补充增加RGBD和双目相机上一帧的MapPoints数

具体的实现看tracking::updatelastframe函数功能，大致就是利用深度值创造一些新的mappoint 这里面肯定是包括了mappoint的类型的初始化，当地图点的个数大于１００的时候会进入break，跳出创建新的mappoint

UpdateLastFrame();　

// 其中mVelocity表示的是Ｔcl　所以mCurrentFrame保存的是当前帧Ｔcw

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);　

//    fill实现mcurrentframe的mvpmappoints初值，即为一个空指针

fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));

 

int th;

if(mSensor!=System::STEREO)

th=15;

else

th=7;

//查看orbmatcher.searchbyprojection利用运动模型计算出currentframe的位姿Ｔcw，然后将上一帧的mappoint投影过来，然后计算出这些点大概在当前帧的一个大概uv，在对这个点画一个框，最后利用当前点与这个块状区域上每一个点计算描述子的距离，判断是否匹配成功

int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);

// 如果跟踪的点少，则扩大搜索半径再来一次

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

}

 

// 步骤3：优化位姿　优化的时候只是优化了当前帧的ＳＥ３

Optimizer::PoseOptimization(&mCurrentFrame);

 

// Discard outliers

// 步骤4：优化位姿后剔除outlier的mvpMapPoints

int nmatchesMap = 0;

for(int i =0; i<mCurrentFrame.N; i++)