![fb9f88ac271cf369ec5a17ce2bddff83.png](https://i-blog.csdnimg.cn/blog_migrate/279180fc969ea7d2b8751514631ad530.png)
文章:[细说ORB-SLAM2系列] 2.特征追踪
作者:盐粒
![589151f025ddc7b2d8a3fc7f1a723fbf.png](https://i-blog.csdnimg.cn/blog_migrate/ddf6f0fa590195afd5220cfec56142e9.png)
ORB-SLAM2是feature-based SLAM最棒的系统,他就像一个图书馆,等待我们去挖掘
1
track()
上一篇文章介绍了系统如何提取特征,以及如何为系统后面tracking和mapping做准备。这里我们将介绍ORB-SLAM2中Track()的策略。
如上图所示,系统的初始化是在这个函数中完成,初始化根据传感器的不同,分为单目和stereo两种方式,其中将RGB-D转化出来双目信息。
MonocularInitialization();
StereoInitialization();
大家都知道,单目系统为了构建一个稳定的地图,有比较复杂的初始化过程。双目则是直接启动。后面我们会专门介绍单目初始化模块。初始化结束之后,会建立关键帧,全局地图。
然后会进行frame-to-frame, map-to-frame的位姿估计,当然如果丢失,则会进行relocalization。
完成对的当前帧的track,就会建立起来当填充前帧中2D 特征对应的3D landmark。然后去判断是否要加入关键帧。并对追踪状态进行更新。下面,详细介绍追踪的过程。
2
Frame-to-Frame tracking
其实,ORB-SLAM2中的追踪策略是非常值得我们借鉴的。首先是给当前帧一个初始的位姿。通过track reference 或 track motion model来获得用重投影误差去优化当前帧的Pose。每个结构里面,按照当前系统状态是否OK,分成下面三种情况:
OK:
速度值不存在,或者初始化后的一帧,或者重定位后的一帧,使用参考帧 进行追踪:bOK = TrackReferenceKeyFrame();
在追踪正常的情况下,存在运动的速度值,使用运动模型进行追踪:
TrackWithMotionModel();
!OK:
Relocalization()
if(mVelocity.empty()||mCurrentFrame.mnId bOK = TrackReferenceKeyFrame();
else
{
bOK = TrackWithMotionModel();
if(!bOK) {
bOK = TrackReferenceKeyFrame();
}
}
具体分析一下这两个函数:
bool Tracking::TrackReferenceKeyFrame()
{
// Compute Bag of Words vector
cout< mCurrentFrame.ComputeBoW();
ORBmatcher matcher(0.7,true);
vector vpMapPointMatches;
//找到 vpMapPointMatches, nmatches
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
if(nmatches<15)
return false;
cout<mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw);
// 通过优化3D-2D的重投影误差来获得位姿
Optimizer::PoseOptimization(&mCurrentFrame);
cout<// 剔除优化后的outlier匹配点(MapPoints)
int nmatchesMap = 0;
for(int i =0; i{
if(mCurrentFrame.mvpMapPoints[i])
{
if(mCurrentFrame.mvbOutlier[i])
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
nmatchesMap++;
}
}
return nmatchesMap>=10;
}
3
map-to-frame
tracking成功后接下来就开始调用TrackLocalMap()函数来优化位姿了。
TrackLocalMap()包括下面几个函数:
UpdateLocalMap():更新局部地图中的keyframes和Local Points
searchLocalPoints():获得局部地图与当前帧的匹配
PoseOptimization():最小化重投影误差来优化位姿
![ff36cbff477fd9bfd3dfaaf47fea4776.png](https://i-blog.csdnimg.cn/blog_migrate/ac7a9c60e7b30cd2e8d2059e0bc9b1a3.png)
![da54e89bd45be0e269489eab1ee56c0e.png](https://i-blog.csdnimg.cn/blog_migrate/cb1a18de220e5ae516af2e8dae27804b.png)
![e5e08854da38c89e7eddc3cef7f231b2.gif](https://i-blog.csdnimg.cn/blog_migrate/2f2f9af8d207808747ff1e9422c3d694.gif)
![3fb716f4fa3321c50fd860f3abcd97fe.png](https://i-blog.csdnimg.cn/blog_migrate/bd6ae27be9f6d77978ff2ed122fc4a6c.png)
扈江离与辟芷兮
纫秋兰以为佩
完