新地图点生成
ORB-SLAM2 通过不断的增加新的关键帧和地图点，来实现地图的扩张。关键帧之间通过共视的地图点建立联系，形成共视图和基图。 所以新地图点的生成是ORB-SLAM建图的一个重要环节，我们可以将所有地图点构成的集合，当做一个稀疏的地图。
在局部建图的总体过程中我们提到， 插入新的关键帧的时候 ORB-SLAM2 会在共视图中，对轨迹跟踪环节没有成功定位的 ORB 特征点， 再次进行特征点匹配。然后对成功匹配的特征点三角化，以确定其空间位置，进而生成新的地图点。 在介绍单目相机初始化的时候，我们曾分析过如歌通过两帧图像进行三角测量定位特征点。 实际上在共视图中，同一个地图点会出现在多个关键帧中。讲道理通过多个观测值来估计一个点的空间坐标，应该比两帧图像来的更准确一点。
本文中，我们先看一下 LOCAL_MAPPING 生成新地图点的总体过程，再进一步讨论多帧图像完成三角化定位的算法原理和实现代码。
1. 总体流程
当 LocalMapping 对象在它的业务循环函数 Run() 中完成地图点的筛选之后， 就会调用函数 CreateNewMapPoints() 来考察新关键帧中的 ORB 特征点，尝试对其进行三角化定位，生成新的地图点。下面是这个函数的代码片段， 它先通过关键帧的接口 GetBestCovisibilityKeyFrames() 来获取与其具有最多共视关系的关联帧，其输入的参数 nn 指定了获取的关键帧数量。 同时函数 CreateNewMapPoints() 还构建了一个局部的特征点匹配器。
void LocalMapping::CreateNewMapPoints() {
int nn = mbMonocular ? 20 : 10;
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
ORBmatcher matcher(0.6,false);
接着获取当前帧的位姿Tcw1和相机中心坐标Ow1，以及相机内参fx1, fy1……
|
|
然后，根据当前关键帧的尺度因子，计算ratioFactor，它将用于根据尺度一致性来判定新建的地图点是否合理。此外创建了一个临时的计数器nnew用于对新建的地图点计数。
const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;
int nnew=0;
完成了，上述的准备工作之后，就可以在一个for循环中遍历各个临接关键帧了。在这个for循环中，首先检查是否又有新关键帧生成了，若是就不再继续与其它关键帧进行匹配三角化了。
for(size_t i = 0; i < vpNeighKFs.size(); i++) {
if(i > 0 && CheckNewKeyFrames())
return;
然后获取临接关键帧的相机中心坐标，计算基线。如果基线太短，计算误差会比较大，就不适合进行三角化了。
KeyFrame* pKF2 = vpNeighKFs[i];
cv::Mat Ow2 = pKF2->GetCameraCenter();
cv::Mat vBaseline = Ow2-Ow1;
const float baseline = cv::norm(vBaseline);
if (!mbMonocular) {
if (baseline < pKF2->mb)
continue;
} else {
const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
const float ratioBaselineDepth = baseline/medianDepthKF2;
if (ratioBaselineDepth < 0.01)
continue;
}
紧接着，调用LocalMapping对象的成员函数ComputeF12，根据当前帧与临接帧的位姿关系计算特征矩阵， 并进行特征点匹配。匹配的特征点对在两个关键帧中的索引被保存在容器vMatchedIndices中，匹配器的接口SearchForTriangulation还会对匹配的特征点进行筛选，只保留那些满足对极约束的点对。
cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
vector<pair<size_t,size_t> > vMatchedIndices;
matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);
接下来，在获取临接帧的位姿(Tcw2)和相机内参(fx2,fy2 ……)之后，就在一个for循环中遍历所有的匹配点对，并分别进行三角化构建地图点。如果成功进行了三角化，就会新建一个地图点， 并相应的更新关键帧与该地图点之间的可视关系。最后通过一个计数器nnew来累计新建的地图点数量。
// 省略Tcw2, fx2……等数据的获取
const int nmatches = vMatchedIndices.size();
for (int ikp=0; ikp < nmatches; ikp++) {
// 省略具体三角化的实现，见后文
MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
pMP->AddObservation(mpCurrentKeyFrame,idx1);
pMP->AddObservation(pKF2,idx2);
mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
pKF2->AddMapPoint(pMP,idx2);
pMP->ComputeDistinctiveDescriptors();
pMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pMP);
mlpRecentAddedMapPoints.push_back(pMP);
nnew++;
} } }
2. 三角化原理
所谓的三角化(triangulation)，其实就是根据两个不同地点观察同一点的视角和几何约束，来估计该点在空间中的位置。 如下图(a)所示，理想情况下，从两帧图像的相机中心\(C_1, C_2\)， 经过归一化平面上匹配的两个点\(x_1, x_2\)分别做两条射线，这两条射线应当相交于点\(P\)。点\(x_1, x_2, P\)就构成了一个三角形，根据它们的几何关系， 我们有很多方法来计算\(P\)点的空间坐标。
 |
 |
 |
| (a). 三角化的基本原理 | (b). 三角化的实际问题 | (c). 三角化垂足中点 |
但是，很不幸，我们的测量是有噪声的。这就意味着从相机中心经过归一化平面做的这两条射线通常不会相交于一点，如上图(b)所示，我们不能直接通过三角关系来确定\(P\)的确切位置。 有一种比较直观的方法是，计算两条射线的公垂线，然后取公垂线上与两个射线的垂足的中点作为\(P\)的估计。这种方法很好理解，但对于我们的应用不是那么合适。 主要是它根据两帧图像来求解，对于多帧的情况求解起来就比较麻烦。
很多时候，我们会将多帧的观测数据归结为一个最小二乘的问题，通过对观测矩阵的奇异值分解来估计\(P\)的坐标。假设在一帧图像中，相机的位姿的齐次矩阵为\(T_{cw} = \begin{bmatrix} R_{cw} & t_{cw} \end{bmatrix} = \begin{bmatrix} T_{cw}.row(0) \\ T_{cw}.row(1) \\ T_{cw}.row(2) \end{bmatrix} \)，特征点在归一化平面上的齐次坐标为\(\boldsymbol{\alpha} = \begin{bmatrix} u & v & 1\end{bmatrix}^T\)。 那么特征点的归一化坐标\(\boldsymbol{\alpha}\)与它的世界坐标\(\boldsymbol{p} = \begin{bmatrix} x & y & z \end{bmatrix}\)之间存在如下关系：
$$ \boldsymbol{\alpha} = T_{cw} \boldsymbol{p} $$由于向量与其自身的叉积为\(\boldsymbol{0}\)， 所以对上式两端左乘\(\boldsymbol{\alpha}\)的斜对称矩阵后， 可以得到一个关于\(\boldsymbol{p}\)的方程：
$$ [\boldsymbol{\alpha}]_{\times}T_{cw} \boldsymbol{p} = \begin{bmatrix} 0 & -1 & v \\ 1 & 0 & -u \\ -v & u & 0 \end{bmatrix} \begin{bmatrix} T_{cw}.row(0) \\ T_{cw}.row(1) \\ T_{cw}.row(2) \end{bmatrix} \boldsymbol{p} = \boldsymbol{0} \Longrightarrow \begin{bmatrix} v T_{cw}.row(2) - T_{cw}.row(1) \\ T_{cw}.row(0) - u T_{cw}.row(2) \\ u T_{cw}.row(1) - v T_{cw}.row(0) \end{bmatrix}\boldsymbol{p} = \boldsymbol{0} \Longrightarrow \begin{bmatrix} v T_{cw}.row(2) - T_{cw}.row(1) \\ u T_{cw}.row(2) - T_{cw}.row(0) \\ u T_{cw}.row(1) - v T_{cw}.row(0) \end{bmatrix}\boldsymbol{p} = \boldsymbol{0} $$上式中第三行\(u T_{cw}.row(1) - v T_{cw}.row(0)\)可以通过其它两行线性表出， 所以对于一帧图像我们可以写出两个方程\(\begin{bmatrix} v T_{cw}.row(2) - T_{cw}.row(1) \\ u T_{cw}.row(2) - T_{cw}.row(0) \end{bmatrix}\boldsymbol{p} = \boldsymbol{0}\)。对于两帧及以上的图像， 我们用上角标表示图像索引，可以写出方程组:
$$ \begin{equation}\label{f1} \begin{bmatrix} v^0 T_{cw}^0.row(2) - T_{cw}^0.row(1) \\ u^0 T_{cw}^0.row(2) - T_{cw}^0.row(0) \\ v^1 T_{cw}^1.row(2) - T_{cw}^1.row(1) \\ u^1 T_{cw}^1.row(2) - T_{cw}^1.row(0) \\ \cdots \\ \cdots \\ \end{bmatrix}\boldsymbol{p} = \boldsymbol{0} \end{equation} $$上式可以记为\(A\boldsymbol{p} = \boldsymbol{0}\)，对于两帧图像而言，\(A\)就是一个\(4 \times 4\)的矩阵，通过SVD分解，我们就可以求得\(\boldsymbol{p}\)的空间齐次坐标。 这就是所谓的DLT(直接线性变换)的方法。
3. 三角化实现
现在，我们再来具体看一下ORB-SLAM2中三角化的实现。为了节省篇幅、解释方便，这里我们只关注单目相机的情况。 下面的代码中，我们做了一些删减，只保留了单目的相关代码。
首先，获取匹配点对在两帧中的索引，保存在idx1和idx2中。并根据这两个索引获取特征点kp1, kp2。
const int &idx1 = vMatchedIndices[ikp].first;
const int &idx2 = vMatchedIndices[ikp].second;
const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
接着，根据针孔相机模型将像素坐标转换为归一化平面坐标，并计算视差角的余弦值。
cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);
cv::Mat ray1 = Rwc1*xn1;
cv::Mat ray2 = Rwc2*xn2;
const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
如果余弦值为负数，意味着视差角超过了90度。在短时间内出现这种情况的可能性很小，一般都是算错了，出现了误匹配才会发生的。余弦值接近1，意味着视差角太小， 这样的数据进行三角化容易产生较大的计算误差。所以ORB-SLAM2只在有足够大的视差角的情况下对匹配特征点进行三角化。在下面的代码片段中，当通过了视差角的检测之后， 就根据式(\(\ref{f1}\))构建矩阵\(A\)，并通过OpenCV的SVD接口进行分解，局部对象vt中将记录点\(\boldsymbol{p}\)位姿的的齐次坐标估计。最后在第32行中归一化齐次坐标的尺度。
cv::Mat x3D;
if(cosParallaxRays > 0 && cosParallaxRays < 0.9998) {
// Linear Triangulation Method
cv::Mat A(4,4,CV_32F);
A.row(0) = xn1.at(0)*Tcw1.row(2)-Tcw1.row(0);
A.row(1) = xn1.at(1)*Tcw1.row(2)-Tcw1.row(1);
A.row(2) = xn2.at(0)*Tcw2.row(2)-Tcw2.row(0);
A.row(3) = xn2.at(1)*Tcw2.row(2)-Tcw2.row(1);
cv::Mat w,u,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
if(x3D.at(3)==0)
continue;
x3D = x3D.rowRange(0,3)/x3D.at(3);
}
得到特征点在世界坐标下的估计之后，三角化的工作就完成了。但ORB-SLAM2还是对三角化后的地图点进一步的筛选了一下。首先三角化的点一定在两帧相机的前方：
cv::Mat x3Dt = x3D.t();
float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
if(z1 <= 0)
continue;
float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
if(z2 <= 0)
continue;
接着计算三角化后的地图点在两帧图像中的重投影误差，剔除那些误差较大的点。
const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at(0);
const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at(1);
const float invz1 = 1.0/z1;
float u1 = fx1*x1*invz1+cx1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
if ((errX1*errX1+errY1*errY1) > 5.991*sigmaSquare1)
continue;
// 省略在第二帧图像中的重投影误差计算
计算三角化后地图点在两帧图像中的深度比例，以及两帧图像的尺度因子的比例关系，剔除那些差异较大的点。
cv::Mat normal1 = x3D-Ow1;
float dist1 = cv::norm(normal1);
cv::Mat normal2 = x3D-Ow2;
float dist2 = cv::norm(normal2);
if(dist1==0 || dist2==0)
continue;
const float ratioDist = dist2/dist1;
const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave] / pKF2->mvScaleFactors[kp2.octave];
if (ratioDist*ratioFactor < ratioOctave || ratioDist > ratioOctave*ratioFactor)
continue;
至此，经过层层筛选之后，就得到了一个质量过关的地图点。
4. 完
每当我们有新的关键帧插入地图中后，LocalMapping都会从共视图中提取出与之共视关系最密切的关键帧集合。计算它们与新关键帧之间的基础矩阵，并再次进行特征点匹配。 针对每个匹配的特征点对，计算它们的归一化平面坐标，以及视差角余弦值。筛选出那些视差角足够大的匹配点对进行三角化。 这里采用 DLT 的方法进行三角化定位，若成功求得特征点所对应的世界坐标，还将进一步把那些负深度的、重投影误差较大的、违反尺度一致性的匹配点对剔除掉。