ORB-SLAM2学习笔记——带有运动模型的跟踪匹配
作者:互联网
TrackWithMotionModel函数
1、理论部分.
假设俩帧之间是匀速运动,根据这个条件来缩小匹配范围,加速匹配减少计算量。
2、代码部分
//带有运动模型的跟踪
bool Tracking::TrackWithMotionModel()
{
//匹配点大于90%的总数
ORBmatcher matcher(0.9,true);
// Update last frame pose according to its reference keyframe
//根据参考帧更新上一帧
// Create "visual odometry" points if in Localization Mode
//创建视觉里程计
//更新上一帧
UpdateLastFrame();
//假设这俩帧的相对运动与之前两帧的相对运动相同,估计当前帧的位姿
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
//清空当前帧的地图点
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
// Project points seen in previous frame
//设定阈值,也就是搜索半径
int th;
//如果不是双目,设定阈值为15,是双目阈值为7
if(mSensor!=System::STEREO)
th=15;
else
th=7;
//在当前帧与上一帧之间进行匹配,输出匹配的点的数目
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
// If few matches, uses a wider window search
//如果匹配数目<20,则清空地图点,扩大阈值(搜索范围)在匹配
if(nmatches<20)
{
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);
}
//若还是数量 < 20 ,那么判定搜索失败。。
if(nmatches<20)
return false;
// Optimize frame pose with all matches
//优化当前帧姿
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
//剔除外点
int nmatchesMap = 0;
//遍历当前帧所有点
for(int i =0; i<mCurrentFrame.N; i++)
{
//如果是地图点
if(mCurrentFrame.mvpMapPoints[i])
{
//如果是外点
if(mCurrentFrame.mvbOutlier[i])
{
//pmp相当于一个点到地图点的一个桥梁
//把这个外点对应的地图点给pmp
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
//清空地图上外点,也就是把该外点设置为空 -1
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;
}
return nmatchesMap>=10;
}
标签:匹配,mCurrentFrame,mvpMapPoints,nmatches,SLAM2,笔记,th,外点,ORB 来源: https://blog.csdn.net/weixin_51326570/article/details/114833418