根据局部地图优化位姿
通过匀速运动模型或者重定位， 我们就初步估计出了相机的位姿，同时找到了一个与当前帧初步匹配的地图点集合。 此时，我们可以把地图投影到当前帧上找到更多的匹配地图点。进而再次优化相机的位姿，得到更为精准的估计。 为了限制这一搜索过程的复杂度，ORB-SLAM只对一个局部地图进行投影。 这个局部地图包含了共视图中与当前帧具有两级邻接关系的关键帧\(\mathcal{K_1}、\mathcal{K_2}\)， 其中\(\mathcal{K_1}\)是与当前帧具有共视关系的关键帧集合，\(\mathcal{K_2}\)则是与\(\mathcal{K_1}\)的元素在共视图中的临接节点。
本文中，我们将详细分析 ORB-SLAM2 是如何组织管理共视图的，针对当前帧如何查找共视图生成集合\(\mathcal{K_1}、\mathcal{K_2}\)。 进一步的 ORB-SLAM2 还会对地图点进行筛选并优化相机位姿。
1. 局部地图更新的总体流程
// Tracking::Track()的代码片段
if(!mbOnlyTracking) {
if(bOK)
bOK = TrackLocalMap();
} else { // 省略纯跟踪模式下相关代码 }
mState = bOK ? OK : LOST;
在轨迹跟踪的总体过程中， 类 Tracking 在成员函数 Track 中完成地图初始化并估计出相机的位姿之后， 将开启局部地图的更新操作。右侧是该函数中相关操作的代码片段，其中成员变量 mbOnlyTracking 指示了当前系统处于建图模式还是纯跟踪模式，这里我们只研究建图相关的内容，所以省略else分支下的语句。 在建图模式下，局部变量 bOK 标识这当前里程计是否工作正常，能够正确跟踪相机位姿。如果跟丢了再进行局部地图更新就没有意义了。
函数 TrackLocalMap 具体完成了局部地图的更新以及相机位姿的进一步优化工作，它将返回一个布尔数据表示更新和优化工作是否成功。只有成功地完成了这一项任务， ORB-SLAM2 才认可当前的相机位姿估计，认为前端视觉里程计完成了位姿跟踪的操作。
bool Tracking::TrackLocalMap() {
// 1. 更新局部地图，并优化相机位姿
UpdateLocalMap();
SearchLocalPoints();
Optimizer::PoseOptimization(&mCurrentFrame);
// 2. 统计匹配地图点数量
mnMatchesInliers = 0;
for(int i = 0; i < mCurrentFrame.N; i++)
// 省略统计优化位姿后仍然匹配的地图点数量 mnMatchesInliers
// 3. 判定是否成功更新了局部地图
if (mCurrentFrame.mnId < mnLastRelocFrameId+mMaxFrames &&
mnMatchesInliers < 50)
return false;
return ((mnMatchesInliers < 30) ? false : true);
}
左侧是该函数的代码片段，大体上可以分为三个阶段：先更新局部地图并进行优化，再统计优化位姿后仍然匹配的地图点数量，最后根据匹配的地图点数量判定局部地图更新任务是否成功。
从左侧的代码片段可以看到，函数 TrackLocalMap 调用了另外两个成员函数 UpdateLocalMap 和 SearchLocalPoints。其中，UpdateLocalMap 用于计算关键帧集合\(\mathcal{K_1}、\mathcal{K_2}\)。 SearchLocalPoints 根据\(\mathcal{K_1}、\mathcal{K_2}\)构成的局部地图，将地图点投影到当前帧上，根据视角以及深度信息筛选匹配的地图点。 完成筛选之后，就可以通过优化器的 PoseOptimization 接口进一步的完成位姿估计的优化工作。
此外，再看一下左侧代码片段中的第11, 12行的条件语句。这个条件语句由两个条件的与操作构成，第一个条件实际上是在判定当前帧是否发生了重定位操作。 按照源码中的注释，作者的本意应该是想给重定位添加一个更严格的约束。即如果发生了重定位， 需要有更多的匹配特征点，降低重定位出错的概率，进而减弱误匹配的负面影响。
下面我们来详细查看一下成员函数 UpdateLocalMap 和 SearchLocalPoints 的代码，了解具体的局部地图计算过程和地图点的筛选过程。关于相机位姿优化的函数 PoseOptimization， 我们已经在匀速运动模型估计相机位姿时详细分析过。
2. 计算局部地图
void Tracking::UpdateLocalMap() {
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
UpdateLocalKeyFrames();
UpdateLocalPoints();
}
我们所要更新的这个局部地图包含了两个关键帧集合\(\mathcal{K_1}、\mathcal{K_2}\)。\(\mathcal{K_1}\)中的关键帧都与当前帧有共视的地图点， \(\mathcal{K_2}\)则是\(\mathcal{K_1}\)的元素在共视图中的临接节点。我们可以通过遍历共视图的临接表来计算局部地图。右面是 UpdateLocalMap() 的代码， 内容很简单，只有三句话。第一句用于可视化，然后调用成员函数 UpdateLocalKeyFrames() 计算关键帧集合\(\mathcal{K_1}、\mathcal{K_2}\)，接着使用成员函数 UpdateLocalPoints() 搜索局部地图中的地图点。
在帧 Frame 和 关键帧 KeyFrame 中都有一个std::vector<MapPoint*>类型的容器 mvpMapPoints，以指针的形式记录着帧/关键帧观测到的地图点。
地图点 MapPoint 中有一个std::map<KeyFrame*,size_t>类型的容器 mObservations，它记录了观测到该地图点的所有关键帧及其在关键帧中的索引。
这两个容器建立了关键帧的共视关系，也就是所谓的共视图。
void Tracking::UpdateLocalKeyFrames() {
map<KeyFrame*,int> keyframeCounter;
for (int i=0; i < mCurrentFrame.N; i++) {
if(mCurrentFrame.mvpMapPoints[i]) {
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(!pMP->isBad()) {
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
for(auto it=observations.begin(); it != observations.end(); it++)
keyframeCounter[it->first]++;
} else { mCurrentFrame.mvpMapPoints[i]=NULL; }
} }
if(keyframeCounter.empty())
return;
左侧是函数 UpdateLocalKeyFrames 的代码片段。首先遍历当前帧的地图点容器 mvpMapPoints 统计与当前帧有共视关系的关键帧以及共视地图点数量。 相关的结果被保存在一个局部容器 keyframeCounter 中，得到当前帧的一级邻接 \(\mathcal{K_1}\)。
在左侧的这个代码片段中，通过一个for循环遍历了当前帧的每一个地图点，通过地图点对象的 GetObservations 获取观测到对应地图点的关键帧集合。 并嵌套一个for循环来遍历这些关键帧，统计共视地图点数量。
如果容器keyframeCounter为空，意味着没有找到与当前帧具有共视关系的关键帧，直接返回。
容器 mvpLocalKeyFrames 用于保存构成局部地图的关键帧集合\(\mathcal{K_1, K_2}\)。 下面的代码片段在一个 for 循环中遍历 keyframeCounter，将其中的关键帧添加到 mvpLocalKeyFrames 中， 同时用临时变量 pKFmax 和 max 用于统计与当前帧具有最多共视地图点的关键帧和共视地图点数量。
int max=0;
KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);
mvpLocalKeyFrames.clear();
mvpLocalKeyFrames.reserve(3*keyframeCounter.size());
for (auto it = keyframeCounter.begin(); it != keyframeCounter.end(); it++) {
KeyFrame* pKF = it->first;
if(pKF->isBad())
continue;
if(it->second > max) {
max = it->second;
pKFmax = pKF;
}
mvpLocalKeyFrames.push_back(it->first);
pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
}
然后，计算集合\(\mathcal{K_2}\)。下面在一个for循环中，遍历集合\(\mathcal{K_1}\)中的所有元素，获取它们在共视图中的临接节点。为了节约计算资源， ORB-SLAM2将集合\(\mathcal{K_1}、\mathcal{K_2}\)中的关键帧数量限制在了80帧。
for(auto itKF=mvpLocalKeyFrames.begin(); itKF != mvpLocalKeyFrames.end(); itKF++) {
if(mvpLocalKeyFrames.size() > 80)
break;
通过关键帧的接口函数GetBestCovisibilityKeyFrames获取考察关键帧的共视图临接节点，在调用的时候为之传递了一个参数10，表示获取最多10个临接关键帧。估计这也是为了节约计算资源而做的限制。
KeyFrame* pKF = *itKF;
const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
嵌套一个for循环，把查询到的最多10个临接关键帧放入容器mvpLocalKeyFrames中，同时更新每个关键帧的成员变量mnTrackReferenceForFrame为当前帧的ID，以防止重复添加某个关键帧。
for(auto itNeighKF=vNeighs.begin(); itNeighKF != vNeighs.end(); itNeighKF++) {
KeyFrame* pNeighKF = *itNeighKF;
if (!pNeighKF->isBad()) {
if (pNeighKF->mnTrackReferenceForFrame != mCurrentFrame.mnId) {
mvpLocalKeyFrames.push_back(pNeighKF);
pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
break;
} } }
类似的，还要考察生成树中的临接节点。实际上生成树是共视图的一个子图，对于一个关键帧，它只记录了与之具有最多共视点的那个关键帧。
const set<KeyFrame*> spChilds = pKF->GetChilds();
for(auto sit = spChilds.begin(); sit != spChilds.end(); sit++) {
KeyFrame* pChildKF = *sit;
if (!pChildKF->isBad()) {
if (pChildKF->mnTrackReferenceForFrame != mCurrentFrame.mnId) {
mvpLocalKeyFrames.push_back(pChildKF);
pChildKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
break;
} } }
KeyFrame* pParent = pKF->GetParent();
if (pParent) {
if (pParent->mnTrackReferenceForFrame != mCurrentFrame.mnId) {
mvpLocalKeyFrames.push_back(pParent);
pParent->mnTrackReferenceForFrame = mCurrentFrame.mnId;
break;
} }
}
最后，更新当前帧的参考关键帧为\(\mathcal{K_1}\)中具有最多共视地图点的关键帧。
if(pKFmax) {
mpReferenceKF = pKFmax;
mCurrentFrame.mpReferenceKF = mpReferenceKF;
} }
下面是搜索局部地图中地图点的成员函数UpdateLocalPoints的实现。它的逻辑很清晰，就是遍历刚刚计算的关键帧集合\(\mathcal{K_1}、\mathcal{K_2}\)，把它们的地图点一个个的都给抠出来， 放到容器mvpLocalMapPoints中。通过地图点的成员变量mnTrackReferenceForFrame来防止重复添加地图点。
void Tracking::UpdateLocalPoints() {
mvpLocalMapPoints.clear();
for (auto itKF = mvpLocalKeyFrames.begin(); itKF != mvpLocalKeyFrames.end(); itKF++) {
KeyFrame* pKF = *itKF;
const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
for(auto itMP=vpMPs.begin(); itMP != vpMPs.end(); itMP++) {
MapPoint* pMP = *itMP;
if(!pMP)
continue;
if(pMP->mnTrackReferenceForFrame == mCurrentFrame.mnId)
continue;
if(!pMP->isBad()) {
mvpLocalMapPoints.push_back(pMP);
pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
} } } }
3. 筛选局部地图点
通过 UpdateLocalKeyFrames 得到的局部地图中的地图点是很多的，Tracking 还需要通过成员函数 SearchLocalPoints 来对这些地图点进行进一步的筛选。下面是该函数的代码片段， 首先在一个for循环中遍历了当前帧的所有地图点，增加可视帧计数，同时更新其成员变量mnLastFrameSeen。这些地图点是估计相机位姿时， 通过匀速运动模型或者词袋模型得到的与当前帧匹配的地图点，它们有可能出现在局部地图关键帧集合\(\mathcal{K_1}、\mathcal{K_2}\)中。
void Tracking::SearchLocalPoints() {
for (auto vit=mCurrentFrame.mvpMapPoints.begin(); vit != mCurrentFrame.mvpMapPoints.end(); vit++) {
MapPoint* pMP = *vit;
if(pMP) {
if(pMP->isBad())
*vit = static_cast(NULL);
else {
pMP->IncreaseVisible();
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
pMP->mbTrackInView = false;
} } }
接下来通过类 Frame 的接口函数 isInFrustum 对局部地图中的地图点进行投影筛选。这个函数设定了很多条件来判定考察的地图点是否在当前帧的视野里。 它有两个参数，第一个参数就是待筛的地图点指针，第二个参数则是拒绝待筛地图点的视角余弦阈值，这里是\(0.5 = \cos 60^◦\)。 本文中我会详细分析一下该函数。局部变量 nToMatch 记录了通过筛选的地图点数量。
int nToMatch=0;
for(auto vit=mvpLocalMapPoints.begin(), vend=; vit != mvpLocalMapPoints.end(); vit++) {
MapPoint* pMP = *vit;
if (pMP->mnLastFrameSeen == mCurrentFrame.mnId)
continue;
if (pMP->isBad())
continue;
if(mCurrentFrame.isInFrustum(pMP,0.5)) {
pMP->IncreaseVisible();
nToMatch++;
} }
如果最后发现有地图点通过了筛选，就对当前帧进行一次投影特征匹配，扩展匹配地图点。这里定义的局部参数th限定了投影搜索范围，该值越大则搜索范围就越大。 第28行的条件如果为真，意味着发生了重定位，此时需要适当的放大搜索范围。
if(nToMatch > 0) {
ORBmatcher matcher(0.8);
int th = 1;
if(mSensor==System::RGBD)
th=3;
if(mCurrentFrame.mnId < mnLastRelocFrameId+2)
th=5;
matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
} }
下面是类Frame的成员函数isInFrustum的代码片段，该函数用于判定一个地图点是否会出现在但前相机的视野范围内。它有两个参数，其中pMP是待考察的地图点指针， viewingCosLimit是拒绝地图点的视角余弦阈值，如果当前相机的视角方向\(\boldsymbol{v}\)与地图点的平均视角方向\(\boldsymbol{n}\)夹角的余弦值小于该参数，就认为地图点不在相机的视野范围内。
bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) {
在该函数的一开始，先获取考察地图点的世界坐标，并将之转换到当前帧的相机坐标系下。
cv::Mat P = pMP->GetWorldPos();
const cv::Mat Pc = mRcw*P+mtcw;
const float &PcX = Pc.at<float>(0);
const float &PcY = Pc.at<float>(1);
const float &PcZ = Pc.at<float>(2);
如果投影之后，地图点的深度为负数，意味着该地图点在相机的后面，抛弃之。
if (PcZ < 0.0f)
return false;
再然后通过针孔相机模型，计算地图点在成像平面上的坐标，如果超出了图像范围，抛弃之。
const float invz = 1.0f / PcZ;
const float u = fx * PcX * invz + cx;
const float v = fy * PcY * invz + cy;
if (u < mnMinX || u > mnMaxX)
return false;
if (v < mnMinY || v > mnMaxY)
return false;
接着计算地图点到相机的距离，如果不在地图点的尺度不变距离的范围(scale invariance region)内，抛弃之。这里计算的临时变量PO是地图点相对于相机的位置矢量。
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
const cv::Mat PO = P-mOw;
const float dist = cv::norm(PO);
if(dist < minDistance || dist > maxDistance)
return false;
然后检查视角，根据输入参数viewingCosLimit，限定当前帧观测地图点的视角方向\(\boldsymbol{v}\)与地图点的平均视角方向\(\boldsymbol{n}\)夹角的余弦值不小于\(\cos 60^◦\)， 即当前帧的时间角不能偏离平均视角\(60^◦\)。
cv::Mat Pn = pMP->GetNormal();
const float viewCos = PO.dot(Pn)/dist;
if(viewCos < viewingCosLimit)
return false;
最后计算地图点在当前帧中的图像尺度，并更新地图点中用于轨迹跟踪的成员变量。
const int nPredictedLevel = pMP->PredictScale(dist,this);
pMP->mbTrackInView = true;
pMP->mTrackProjX = u;
pMP->mTrackProjXR = u - mbf*invz;
pMP->mTrackProjY = v;
pMP->mnTrackScaleLevel= nPredictedLevel;
pMP->mTrackViewCos = viewCos;
return true;
}
4. 完
在完成相机位姿的初始估计之后，Tracking线程又通过共视关系构建了局部地图，并将局部地图中的地图点投影到当前帧中通过一系列的条件进行筛选，获得更多的匹配地图点集合， 并根据这些扩展的地图点集合再次优化相机的位姿估计。如果优化相机位姿之后无法找到足够多的匹配的地图点，认为局部地图更新失败，视觉里程计跟丢了。