相机的位姿估计
完成了地图初始化之后，我们就可以在此基础上估计相机的位姿，有人把这一任务称为视觉里程计。 一般情况下ORB-SLAM都是先通过匀速运动模型粗略的估计相机位姿，再通过特征点匹配和优化方法给出一个较为精确的位姿估计。有时相机可能运动过快，导致里程计跟丢了，此时ORB-SLAM会通过词袋模型， 以及特征点匹配来重定位。
本文，我们先了解一下相机位姿估计的总体流程，再来详细分析里程计以及重定位的具体实现。RGB-D和双目相机都能够采集深度信息， 它们相比于单目相机，只是投影计算方式略有不同而已，本文不加以区分。
1. 位姿估计的总体流程
// Tracking::Track()的代码片段
if (mState == NOT_INITIALIZED) {
// 先给我老老实实的完成了初始化任务再来。
} else {
bool bOK;
if(!mbOnlyTracking) {
if(mState == OK) {
CheckReplacedInLastFrame();
if(mVelocity.empty() ||
mCurrentFrame.mnId < mnLastRelocFrameId+2) {
bOK = TrackReferenceKeyFrame();
} else {
bOK = TrackWithMotionModel();
if (!bOK)
bOK = TrackReferenceKeyFrame();
}
} else {
bOK = Relocalization();
}
} else {
// 省略纯粹定位的应用分支
}
}
我们只要判定地图已经进行了初始化，就可以正常的估计相机位姿了。如右边的代码片段所示， Tracking对象在它的成员函数Track中检查当前的跟踪器的状态， 若处于NOT_INITIALIZED的状态则执行地图初始化任务，否则就可以估计相机位姿了。
ORB-SLAM2有纯定位和建图两种工作模式，位姿跟踪器通过成员变量mbOnlyTracking来进行区分，这里我们只考虑建图模式的实现。当系统状态mState处于OK时， 意味着当前的视觉里程计成功地跟上了相机的运动。如果跟丢了就只能进行重定位了。
论文中说如果上一帧图像成功的跟上了相机的运动， 并成功的估计了速度，就以匀速运动模型来估计相机的位姿。然后以此为参考，把上一帧中观测到的地图点投影到当前帧上，并一个比较小的范围里搜索匹配的特征点。如果找不到足够多的匹配特征点， 就适当的放大搜索半径。找到了足够多的匹配特征点后，就进行一次优化提高相机位姿估计的精度。这就是第13行中调用的函数TrackWithMotionModel所实现的功能。
第11和15行中调用的TrackReferenceKeyFrame，在论文中并没有找到特定的描述。我理解它也是一个重定位只是没有Relocalization的搜索范围大而已。第9、10行的分支条件如果成立， 意味着上一帧可能进行了重定位，所以机器人的速度估计丢失了，帧ID也可能因为重定位而向前发生了跳转。此时再用匀速运动模型来估计位姿就显得不合适了，所以要从参考关键帧上估计位姿。 如果匀速运动模型失效了，可以先通过TrackReferenceKeyFrame补救一下。若仍然无法成功估计位姿，意味着我们跟丢了，需要进行重定位。 其实从代码来看，如果TrackWithMotionModel和TrackReferenceKeyFrame都失效了，当前帧将被抛弃掉，在下一帧图像到来的时候才有可能调用Relocalization进行重定位。 不知道作者是有什么考量故意如此的，还是说这纯粹就是个笔误。
下面我们来具体看一下这三个函数的实现。
2. 匀速运动模型定位
下面是函数TrackWithMotionModel的代码片段，该函数实现了基于匀速运动模型的跟踪定位方法。在函数一开始，先创建了一个ORB匹配器用于后面在当前帧上搜索与重映射之后的地图点匹配的特征点。 我们给匹配器的构建函数传递了两个参数，第一个参数是一个接受最佳匹配的系数，只有当最佳匹配点的汉明距离小于次加匹配点距离的0.9倍时才接收匹配点，第二个参数表示匹配特征点时是否考虑方向。 关于ORB匹配器的具体实现，我们将放到附录中介绍。 接下来该函数还调用了UpdateLastFrame，这个函数主要是在纯定位模式下构建一个视觉里程计，对于建图模式作用不大，不再展开。
bool Tracking::TrackWithMotionModel() {
ORBmatcher matcher(0.9,true);
UpdateLastFrame();
然后就通过匀速运动模型估计当前帧的位姿。所谓的匀速运动模型，就是假设从上一帧到当前帧的这段时间里机器人的线速度和角速度都没有变化， 直接通过速度和时间间隔估计前后两帧的相对位姿变换，再将之直接左乘到上一帧的位姿上，从而得到当前帧的位姿估计。 在下面的代码片段中，我们只看到了速度相关的变量mVelocity。在后面分析关键帧生成的时候，我们会看到这个变量实际上是上一次更新的时候，上一帧和上上帧之间的相对位姿变换。 如果帧率稳定，帧间的时间间隔固定，再加上速度恒定，就可以认为帧间的相对位姿变换是一样的。
mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);
估计了当前帧位姿之后，就可以将上一帧中看到的地图点投影到当前帧上，在当前帧的指定范围里搜索匹配的特征点。这点是通过ORB匹配器的SearchByProjection接口实现的， 这个接口有四个参数，前两个分别是当前帧和上一帧。第三个参数th是一个控制搜索半径的参数，最后一个参数用于判定是否为单目相机。由于RGB-D和双目相机都能够采集深度信息， 所以它们的投影计算方式略有不同，我们将在以后分析。
如下面的代码片段所示，一开始的搜索范围比较小，如果找不到足够多的匹配特征点，将把搜索半径扩大两倍再进行搜索。如果匹配点的数量仍然不够，就认为匀速运动模型跟丢了。
fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint*>(NULL));
int th = (mSensor != System::STEREO) ? 15 : 7;
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);
}
if (nmatches < 20)
return false;
当我们找到了足够多的匹配点，就通过优化器根据匹配的特征点优化相机位姿。ORB-SLAM使用g2o进行优化，我们将在以后的文章中详细分析优化器。
Optimizer::PoseOptimization(&mCurrentFrame);
按照文章中的说法，优化了位姿之后，基于匀速运动模型的跟踪定位就应该结束了。但该函数并没有退出，它还进一步的检查了当前帧看到的各个地图点，抛弃了那些野点(outlier)。 它定义了一个局部整型nmatchesMap用于记录当前帧实际看到的地图点数量，并在一个for循环中遍历所有特征点。当前帧对象的容器mvpMapPoints和mvbOutlier与特征点是一一对应的， mvpMapPoints记录了各个特征点所对应的地图点指针，如果没有对应地图点则为NULL。mvbOutlier记录了各个特征点是否为野点(在优化的时候会更新这个状态)，若是野点则直接抛弃之。
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++;
}
}
在最后退出的时候，在看一下还剩下多少匹配的地图点，如果太少就认为跟丢了。
if(mbOnlyTracking) {
mbVO = nmatchesMap < 10;
return nmatches > 20;
}
return nmatchesMap >= 10;
}
3. 从参考关键帧定位
当系统认为当前的轨迹还没有跟丢，但是使用匀速运动模型不能很好的估计相机位姿的时候，就会通过函数TrackReferenceKeyFrame来从参考帧上进行定位。 下面是该函数的代码片段，一开始先计算当前帧的词袋向量。
bool Tracking::TrackReferenceKeyFrame() {
mCurrentFrame.ComputeBoW();
再构建ORB特征匹配器，通过接口SearchByBoW将当前帧与参考关键帧进行特征点匹配，匹配到的地图点保存在临时的容器vpMapPointMatches中，nmatches将获得匹配的特征点数量。 如果匹配的点太少就认为跟丢了。
ORBmatcher matcher(0.7,true);
vector<MapPoint*> vpMapPointMatches;
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
if (nmatches < 15)
return false;
然后用vpMapPointMatches更新当前帧匹配的地图点，并使用上一帧的位姿变换作为当前帧的优化初值，调用优化器的PoseOptimization优化相机的位姿。
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw);
Optimizer::PoseOptimization(&mCurrentFrame);
最后，像TrackWithMotionModel那样对野点进行筛选，返回实际匹配到的地图点数量是否足够多。
// Discard outliers
int nmatchesMap = 0;
for(int i =0; i < mCurrentFrame.N; i++)
...
return nmatchesMap >= 10;
}
4. 重定位
如果上述过程没找到足够多的匹配特征点，就认为跟丢了，此时将进行重定位。ORB-SLAM会先将当前帧转换成词袋，然后在数据库粗选几个候选帧。 它会使用RANSAC策略和PnP算法根据每个候选帧来估计相机位姿，选取具有足够多内点(inlier)的候选帧作为重定位后的参考关键帧。
下面是函数Relocalization的代码片段，一开始先计算当前帧的词袋模型，然后从关键帧数据库mpKeyFrameDB中查找候选关键帧，如果候选集为空就报错退出。 关于这个计算候选帧的筛选过程我们将在后续介绍词袋模型的时候详细解释。它大体上是先遍历一下所有的关键帧筛选出具有共享词的那些，再通过共享词的数量以及BoW相似性得分做进一步的筛选。
bool Tracking::Relocalization() {
mCurrentFrame.ComputeBoW();
vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
if(vpCandidateKFs.empty())
return false;
接下来我们创建了一个ORB匹配器，并根据候选关键帧的数量创建了几个容器。
ORBmatcher matcher(0.75,true);
const int nKFs = vpCandidateKFs.size();
vector<PnPsolver*> vpPnPsolvers;
vpPnPsolvers.resize(nKFs);
vector<vector<MapPoint*>> vvpMapPointMatches;
vvpMapPointMatches.resize(nKFs);
vector<bool> vbDiscarded;
vbDiscarded.resize(nKFs);
我们将先用这个ORB匹配器遍历一下所有的候选帧，当有足够多的匹配点时为之创建一个PnP求解器。容器vpPnPsolvers就是用来记录各个候选帧的求解器的， vvpMapPointMatches则用于保存各个候选帧与当前帧的匹配关键点，vbDiscarded标记了对应候选帧是否因为匹配点数量不足而被抛弃。
int nCandidates=0;
for(int i=0; i < nKFs; i++) {
KeyFrame* pKF = vpCandidateKFs[i];
if (pKF->isBad())
vbDiscarded[i] = true;
else {
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
if(nmatches < 15) {
vbDiscarded[i] = true;
continue;
} else {
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
}
}
接下来就是通过PnP求解器根据候选帧与当前帧的匹配关系来求解相机位姿的过程。这是一个不断迭代优化的过程，随着外层while循环的迭代，相机的位姿估计精度将不断得到提升。 因为位姿估计更加合理，得到的匹配关键点数量也会逐渐增加。如果在某次while循环中发现在某个候选帧下位姿估计差得离谱，就会抛弃之。随着迭代的持续进行，要么所有的候选帧都被抛弃，重定位失败； 要么找到一个候选帧具有足够多的内点(inlier)，也就是与当前帧的匹配程度满足要求，接受之，重定位成功。
在进行新的筛选之前，先创建了一个标识重定位是否成功的布尔变量bMatch，和一个用于对候选帧的关键点进行投影匹配的ORB匹配器matcher2。
bool bMatch = false;
ORBmatcher matcher2(0.9,true);
然后构建了一个while和for的双层循环，while循环用于推进位姿估计的优化迭代，for循环用于遍历候选关键帧。
while(nCandidates > 0 && !bMatch) {
for(int i = 0; i < nKFs; i++) {
if(vbDiscarded[i])
continue;
针对每个关键帧先通过PnP求解器估计相机的位姿，结果保存在局部变量Tcw中。这里定义的局部容器vbInliers记录了候选帧中成功匹配上的地图点，nInliers则记录了匹配点的数量， bNoMore用于标记PnP求解是否达到了最大迭代次数。
vector<bool> vbInliers; int nInliers; bool bNoMore;
PnPsolver* pSolver = vpPnPsolvers[i];
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
如果达到了最大迭代次数，那么意味着通过PnP算法无法得到一个比较合理的位姿估计，所以才会迭代了那么多次。因此需要抛弃该候选帧。
if(bNoMore) {
vbDiscarded[i]=true;
nCandidates--;
}
如果成功的求解了PnP问题，并得到了相机的位姿估计，那么就进一步的对该估计进行优化。首选需要用刚刚计算得到的位姿估计来更新当前帧的位姿，在通过优化器的接口PoseOptimization来优化位姿。 局部变量nGood评价了匹配程度，如果太低就结束当此迭代。
if(!Tcw.empty()) {
Tcw.copyTo(mCurrentFrame.mTcw);
// 省略一些局部变量的定义和准备工作
int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
if (nGood < 10)
continue;
如果内点数量比较少，就以一个较大的窗口将候选帧的地图点投影到当前帧上获取更多的可能匹配点，并重新进行优化。
if (nGood < 50) {
int nadditional = matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
if (nadditional+nGood >= 50) {
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
如果内点数量得到了增加但仍然不够多，就再次将候选帧的地图点投影到当前帧上搜索匹配点，只是这次投影的窗口比较小。再进行一次优化。
if (nGood > 30 && nGood < 50) {
// 省略一些局部变量的定义和准备工作
nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
if (nGood+nadditional >= 50) {
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
// 省略剔除野点的操作
} } } }
如果找到一个候选帧经过一次次的优化之后，具有足够多的匹配点，就接受它，认为重定位成功，退出循环迭代。
if(nGood >= 50) {
bMatch = true;
break;
} } } }
最后根据是否成功找到匹配关键帧返回重定位是否成功。
if (!bMatch)
return false;
mnLastRelocFrameId = mCurrentFrame.mnId;
return true;
}
5. 完
ORB-SLAM使用了三种方式来估计相机的位姿。如果一切正常就使用匀速运动模型来粗略的估计相机位姿，在通过优化提高定位精度。如果跟丢了，就通过词袋模型进行重定位，重新确认参考关键帧。 当使用匀速运动模型不能有效估计相机位姿，或者因为重定位的发生导致没有速度估计或者帧ID发生跳转时，ORB-SLAM就会通过参考关键帧来估计位姿。