(瞎忙了一阵,码没撸太多忙着挖雷,深感惭愧....)
进入正题,上次说完了orb-slam设计的自动地图初始化,现在从前台线程tracking继续讲。当orb-slam每接收到一帧新的图像,都会唤起这个线程。它主要负责每一帧的定位,并且通过motion-only BA对刚刚得到的初始camera pose进行第一次优化。在此基础之上,我们会确定一个当前帧的local map,这里是通过当前帧与周围帧的公视关系确定的。然后,该线程将当前的特征点投回local map中的其他帧中,通过计算重投影误差再一次优化camera pose。最后,该线程会通过一系列标准来确定当前帧是否为关键帧,如果是关键帧,则唤醒local mapping线程并将当前帧作为KF传给它。
有了成功的初始化之后,slam系统每接收新的图像帧,就可以开始图中的tracking流程了。ORB1和ORB2在做pose prediction之前都会提取orb特征,最初版本只支持单目,后面开放了双目相机以及RGBD相机的接口,加入了相对应的处理机制,这里不过多讨论。在成功提取当前帧特征的基础上,ORB2提供了三种模式用来对pose做initial prediction,选择不同模式在Tracking::Track()开始部分的代码,注意mbOnlysTracking和mbVO两个变量,前者为true代表slam只做定位不建图,后者为true代表当前帧是找不到与地图的match的,只能追踪temporal visual odometry points。
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
else
{
// System is initialized. Track Frame.
bool bOK; //tracking result record
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
CheckReplacedInLastFrame();
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackWithMotionModel();
if(!bOK)
bOK = TrackReferenceKeyFrame();
}
}
else
{
bOK = Relocalization();
}
}
else //VO,localmapping not working
{
// Localization Mode: Local Mapping is deactivated
if(mState==LOST) //tracking lost
{
bOK = Relocalization();
}
else
{
if(!mbVO)
// In case of performing only localization, mbVO is true when there are no matches to
// points in the map. Still tracking will continue if there are enough matches with temporal points.
// In that case we are doing visual odometry. The system will try to do relocalization to recover
// "zero-drift" localization to the map.
{
// In last frame we tracked enough MapPoints in the map
if(!mVelocity.empty())
{
bOK = TrackWithMotionModel();
}
else
{
bOK = TrackReferenceKeyFrame();
}
}
else
{
// In last frame we tracked mainly "visual odometry" points.
// We compute two camera poses, one from motion model and one doing relocalization.
// If relocalization is sucessfull we choose that solution, otherwise we retain
// the "visual odometry" solution.
bool bOKMM = false;
bool bOKReloc = false;
vector<MapPoint*> vpMPsMM;
vector<bool> vbOutMM;
cv::Mat TcwMM;
if(!mVelocity.empty())
{
bOKMM = TrackWithMotionModel();
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.mTcw.clone();
}
bOKReloc = Relocalization();
if(bOKMM && !bOKReloc)
{
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
if(mbVO)
{
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
{
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
}
}
}
}
else if(bOKReloc)
{
mbVO = false;
}
bOK = bOKReloc || bOKMM;
}
}
}
1. TrackWithMotionModel():使用恒速模型的假设是摄像头运动是匀速运动的,利用匀速运动之后的pose来初始化当前帧。若函数返回false证明该方法跟踪失败,然后采用trackreferencekeyframe(),同时注意如果一上来获取不到速度或者是刚刚做了重定位不久,就直接采用跟踪referenceKF的方法。
2. TrackReferenceKeyFrame():如果恒速模型失效,就开始尝试和相邻的关键帧来进行匹配,并且可以使用BoW来加速匹配,首先计算当前帧的BoW,并设定初始位姿为上一帧的位姿,然后根据位姿和BoW词典来寻找特征匹配,进一步优化位姿。
3. Relocalization():到全局的地图中查找匹配帧,计算位姿(solvePnP实现)。
基于前面的工作已经获得了当前帧的位姿估计值以及一部分特征点与地图点的对应。在TrackLocalMap()中,我们用该帧追踪一个局部地图以获取更多的2D-3D点的匹配,这里用局部地图而不是之前所有的关键帧也是为了bound complexity。这个局部地图包含了关键帧集合К1和К2,其中К1包含了与当前帧有共视关系的所有关键帧, К2包含了К1的每一帧在共视图中的临近帧。之后在局部地图中再次优化当前帧的位姿。ps:在前端线程依靠跟踪邻近帧来进行局部优化从而保证快速而准确的追踪的工作,个人觉得这对后面的关键帧选取以及建图都是有很大的好处的,idea很棒,后面准备单独写一章捋一捋这部分的代码,先挖个坑。
至此在Tracking线程中,当前帧已经历了至少两次pose优化。
最后我们要判断该帧是不是我们想要的关键帧,如果是的话需同时满足如下条件:
- 该帧距离上一次重定位已经过了至少二十帧。
- 距离上一次关键帧的插入(唤醒localmapping线程)至少过了二十帧。(同时如果当前帧条件都满足但发现上一次唤醒的建图线程还没停,那么先把他强制停了。保证关键帧的快速插入。)
- 至少跟踪了五十个特征点。
- 为了保证一定的视觉变化,该帧追踪到的点应少于局部地图中与该帧共视关系最强帧追踪到的点数的90%。(虽然到了建图线程也有删除KFs的机制,但个人觉得这一条的目的就是防止过早地出现冗余关键帧,让新的帧包含一定比重的新的信息。这4条组合的策略下插帧已经很快了,所以冗余的在这个节点该删就删呗。)
确定关键帧后,我们将其送到建图线程,维护我们的地图,其中包括地图点的添加删除,以及冗余关键帧的删除等,这些我们后面继续说。
(以上全部为个人学习心得,如有错误欢迎评论指正,如有幸被转发,请标明转载出处)