局部地图中关键帧筛选 & LocalMapping 针对双目/深度相机的调整
局部建图的最后一个环节是关键帧筛选，其实它没有什么特别的内容，就是其字面意思。 删除掉那些冗余的关键帧，保证系统的运行效率。由于 LocalMapping 主要是针对关键帧和地图点进行的操作，不再直接处理图像，所以传感器类型对于它的实现影响不大。 LocalMapping 也就是在进行局部 BA 优化的时候针对 Stereo 类型的地图点专门描述了约束边的观测量。
本文先详细分析局部地图中关键帧筛选过程，再介绍针对双目/深度相机如何构造约束边的观测量。
1. 局部地图中关键帧筛选
出于效率的考虑，局部地图管理器还会检查关键帧，删除那些冗余的关键帧。这样可以防止随着系统的长时间运行，关键帧无限制的增长。 检查共视图上当前关键帧的所有邻接，并根据冗余程度，删除那些90%的地图点都能被其它至少三个关键帧观测到的关键帧。 下面是函数 KeyFrameCulling() 的代码片段，首先获取所有邻接关键帧，并遍历它们。由于第一个关键帧是整个地图的初始化基准，所以不能删去。
void LocalMapping::KeyFrameCulling() {
vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
for (auto vit=vpLocalKeyFrames.begin(); vit != vpLocalKeyFrames.end(); vit++) {
KeyFrame* pKF = *vit;
if (pKF->mnId == 0) continue;
接下来获取各个关键帧的地图点，并定义了一些临时的变量用于统计关键帧的冗余程度。
const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
int nObs = 3; // 除了给下面的 thObs 赋了个值，实际上没啥用
const int thObs=nObs; // 地图点观测数阈值
int nRedundantObservations=0; // 冗余的地图点数量
int nMPs=0; // 有效地图点计数
在一个for循环中遍历所有的地图点，这里为了排版美观，我在第13行对判定条件做了一些等价的修改，跳过那些没有匹配的或者标记无效的地图点。
此外对于双目和深度相机，我们需要刨去那些远处的、相机背后的地图点。
在针对双目和深度相机的调整(TRACKING)中曾经提到，
ORB-SLAM2 以40倍基线长度为阈值(pKF->mThDepth)，将双目地图点分成了近(close)、远(far)两种形式。
for (size_t i = 0; i < vpMapPoints.size(); i++) {
MapPoint* pMP = vpMapPoints[i];
if (!pMP || pMP->isBad()) continue;
if(!mbMonocular)
if (pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)
continue;
然后通过 nMPs++ 计数有效地图点。如果有至少3个关键帧在相同或者更细(finer scale)的尺度上观测到地图点，则认为该地图点是冗余的。
在第19行中通过变量 scaleLevel 记录地图点在第 i 个关键帧中的图像金字塔层级。
nMPs++;
if (pMP->Observations() <= thObs) continue;
const int & scaleLevel = pKF->mvKeysUn[i].octave;
遍历所有观测到地图点的关键帧集合，检查其它关键帧的金字塔层级。如果同级或者更细，则通过局部变量 nObs 记录下来。如第29,32行所示，超过了三帧则判定地图点冗余，退出循环。
int nObs=0;
const map<KeyFrame*, size_t> observations = pMP->GetObservations();
for (auto mit=observations.begin(); mit != observations.end(); mit++) {
KeyFrame* pKFi = mit->first;
if (pKFi == pKF) continue;
const int & scaleLeveli = pKFi->mvKeysUn[mit->second].octave;
if (scaleLeveli <= scaleLevel+1) {
nObs++;
if (nObs >= thObs) break;
}
}
if (nObs >= thObs)
nRedundantObservations++;
}
如果超过90%的地图点都是冗余的，那么该关键帧就是冗余的，通过其接口 SetBadFlag() 标记下来，从地图中移除。
if (nRedundantObservations > 0.9*nMPs)
pKF->SetBadFlag();
} }
2. 针对双目/深度相机的调整
在 LocalMapping 中也没有太多的针对双目/深度相机的调整。除了上面筛选关键帧的时候，针对双目地图点跳过了那些远处的点外，就是进行局部BA优化时，专门针对双目地图点设计了约束。
我们在局部BA优化中， 详细分析了函数Optimizer::LocalBundleAdjustment的实现。 该函数以当前帧、一级邻接关键帧、二级邻接关键帧，以及出现在当前帧和一级邻接中的地图点构造了一个局部的地图，通过 g2o 库进行 BA 优化。 下面左侧是该函数的代码片段，其中容器 vpEdgesStereo 是用来保存根据 Stereo 类型的地图点构建的约束边，以后简称为双目边。 vpEdgeKFStereo 和 vpMapPointEdgeStereo 分别记录了双目边连接的关键帧和地图点。thHuberStereo 是一个自由度为3，显著性水平为0.05的卡方阈值，后面用于剔除误差较大的边。
|
|
上面右侧的代码片段，是在一个 for 循环中针对 stereo 类型的地图点构建观测量 obs。其中包含了地图点在左目中的xy像素坐标，以及右目中的像素x坐标。 在下面的代码中，构造了双目边对象，并设定了它的链接的两个节点分别是一个地图点和一个关键帧。id是地图点在局部地图中的节点索引。 此外，还在第4行中设定了双目边的观测量 obs。
g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(obs);
其余的操作与 mono 类型的地图点没有什么大的差别。无非是 Huber 核的参数，以及误差判定阈值为 thHuberStereo 而已。
3. 完
本文中，我们先分析了局部地图中关键帧筛选的实现。如果当前关键帧的邻接关键帧中 90% 的地图点都是冗余的将被剔除掉。判定地图点冗余的条件是，至少有三个关键帧在相同或者更细的尺度上观测到该地图点。
初次之外，还讨论了在局部 BA 优化时针对双目/深度相机做的调整。主要差别在于，观测量中增加了右目相机的像素横坐标，以及 Huber 核函数的参数。