单应矩阵的求解过程
我们在帧间位姿估计和地图初始化总体过程中分析了单目相机的初始化过程， 其中提到， 针对平面场景(planar scene)和非平面场景(non-planar scene)，ORB-SLAM2提供了单应(homography)和基础矩阵(fundamental matrix)两种几何模型。 它会在两个线程中同时计算这两个模型，然后根据一些启发式信息选取一个较合适的模型。
在 ORB-SLAM2 中针对平面场景，根据摄影变换关系建立了两幅图像之间各点的一一对应关系。 这个映射关系可以用一个\(3 \times 3\)的齐次矩阵\(\boldsymbol{H}\)来表示，我们称之为单应矩阵。 假设\(\boldsymbol{x}, \boldsymbol{x'}\)分别是初始化过程中的参考帧和当前帧中匹配的两个特征点，\(K\)为相机的内参，被观测的平面的法向量是\(N\)，平面到参考帧的距离为\(d\)， 可以推导出\(\boldsymbol{x}, \boldsymbol{x'}\)存在如下的关系:
$$ \begin{equation}\label{f_homography_dlt} \begin{array}{rl} & \boldsymbol{x'} = \cfrac{z_1}{z_2}\boldsymbol{K}\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}\boldsymbol{x} \\ \Rightarrow & \boldsymbol{x'} = s\boldsymbol{H}\boldsymbol{x} \end{array} \end{equation} $$其中\(\boldsymbol{H} = \boldsymbol{K}\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}\)，是两幅图像的单应矩阵。 \(s = z_1 / z_2 \)是两幅图像之间的尺度因子。
在前文中， 我们了解到 ORB-SLAM2 专门定义了类 Initializer 来完成系统的初始化操作，其成员函数 FindHomography 负责计算单应矩阵，函数 ReconstructH 对单应矩阵进行分解求得相机位姿。 本文中，我们先介绍计算单应矩阵的 DLT 算法，再分析函数 FindHomography 及其调用的其它函数了解实现过程。 然后，推导从\(H\)矩阵分解出相机位姿\(R,\boldsymbol{t}\)的公式，再分析函数 ReconstructH 了解实现过程。
1. 求解单应矩阵的 DLT 算法
ORB-SLAM2 使用直接线性变换(Direct Linear Transformation, DLT) 算法求解单应矩阵。式(\(\ref{f_homography_dlt}\))建立了两幅图像中像素点之间的映射关系， 展开成齐次坐标的形式如下：
$$ \begin{equation}\label{f_homography_ext} \begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix} = s \begin{bmatrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} \end{equation} $$其中，\((u_2, v_2)\)为当前帧中特征点的像素坐标，\((u_1, v_1)\)为参考帧中匹配点的像素坐标。\(h_1 \cdots h_9\)是矩阵\(H\)中的9个元素。
$$ \begin{cases} u_2 = s(h_1 u_1 + h_2 v_1 + h_3) \\ v_2 = s(h_4 u_1 + h_5 v_1 + h_6) \\ 1 = s(h_7 u_1 + h_8 v_1 + h_9) \end{cases} \Rightarrow \begin{cases} u_2 = \frac{h_1 u_1 + h_2 v_1 + h_3}{h_7 u_1 + h_8 v_1 + h_9} \\ v_2 = \frac{h_4 u_1 + h_5 v_1 + h_6}{h_7 u_1 + h_8 v_1 + h_9} \end{cases} $$一对匹配点我们可以根据上式写出两个约束，整理上式有：
$$ \begin{cases} h_1 u_1 + h_2 v_1 + h_3 - h_7 u_1 u_2 - h_8 v_1 u_2 - u_2 = 0 \\ h_4 u_1 + h_5 v_1 + h_6 - h_7 u_1 v_2 - h_8 v_1 v_2 - v_2 = 0 \end{cases} $$假设我们有 \(m\) 对匹配点，根据上式我们可以写出 \(2m\) 个约束，可以写成 \(A\hat{H} = \boldsymbol{0}\)的矩阵形式，如下， 为了与 ORB-SLAM2 的代码保持一致，我们特意交换了上式中两个约束的顺序。 由于尺度因子的存在，求解时一般认为\(h_9 = 1\)，所以向量\(\hat{H}\)中只有8个未知数，至少需要4对匹配点，写出8个线性方程才可以求解。
$$ \begin{equation}\label{f_DLT_equations} \begin{bmatrix} 0 & 0 & 0 & u_1^1 & v_1^1 & 1 & -u_1^1 v_2^1 & -v_1^1 v_2^1 & -v_2^1 \\ u_1^1 & v_1^1 & 1 & 0 & 0 & 0 & -u_1^1 u_2^1 & -v_1^1 u_2^1 & -u_2^1 \\ & & & & & \vdots \\ 0 & 0 & 0 & u_1^m & v_1^m & 1 & -u_1^m v_2^m & -v_1^m v_2^m & -v_2^m \\ u_1^m & v_1^m & 1 & 0 & 0 & 0 & -u_1^m u_2^m & -v_1^m u_2^m & -u_2^m \\ \end{bmatrix} \begin{bmatrix} h_1 \\ h_2 \\ h_3 \\ h_4 \\ h_5 \\ h_6 \\ h_7 \\ h_8 \\ 1 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ \vdots \\ 0 \\ 0 \end{bmatrix} \end{equation} $$对上式解零空间即可求得\(h_1 \cdots h_8\)，既对等式左侧的参数矩阵进行 SVD 分解，取最小的奇异值在\(V^T\)空间中对应的行向量，即为我们想要的解。 直接拿特征点的像素坐标构造上式，并通过SVD分解求得\(\hat{H}\)的过程就是最朴素的 DLT 算法。
朴素的 DLT 算法思想很简单直接，但是由于尺度因子的存在，以及测量噪声，该算法并不稳定。尤其是在 RANSAC 这样的算法加持下，得到的结果通常都不怎么样。 所以，人们在进行 DLT 之前，还会进行一次归一化的操作，将两幅图像的匹配点进行平移和缩放，让它们各自的质心为\(\begin{bmatrix} 0 & 0\end{bmatrix}\)， 各点到质心的平均距离为 \(\sqrt{2}\)。
2. 矩阵 \(H\) 的求解过程
2.1 函数 FindHomography
下面是函数 FindHomography 的代码片段，它有输出三个参数。在函数运行结束后，H21 输出了两帧图像之间的单应矩阵， score 则给出了 H21 的评分 \(S_H\)，vbMatchesInliers 将标注所有的匹配点对是否为内点。即，是否不是误匹配的点对。
void Initializer::FindHomography(std::vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21) {
const int N = mvMatches12.size();
std::vector<cv::Point2f> vPn1, vPn2;
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2inv = T2.inv();
如上面的代码片段所示，在该函数的一开始，先通过成员函数 Normalize 对两帧图像的特征点进行归一化操作，局部变量 vPn1, vPn2 分别记录了归一化之后的特征点坐标， T1, T2 记录进行归一化时用到的 3x3 的变换矩阵，包含了平移和缩放。接下来定义了一些局部的变量，准备以 RANSAC 的方式，遍历刚刚生成的样本组。
score = 0.0; // 记录最佳评分
vbMatchesInliers = vector<bool>(N,false); // 记录最佳评分时，各匹配点对是否为内点
vector<cv::Point2f> vPn1i(8); // 用于记录某个 RANSAC 样本组中，参考帧的特征点归一化之后的坐标
vector<cv::Point2f> vPn2i(8); // 用于记录某个 RANSAC 样本组中，当前帧的特征点归一化之后的坐标
cv::Mat H21i, H12i; // 根据某个 RANSAC 样本组得到的单应矩阵及其逆
vector<bool> vbCurrentInliers(N,false); // 某个 RANSAC 结果下，匹配点是否为内点的判定结果
float currentScore; // 某个 RANSAC 结果的评分
然后遍历所有的样本组，取出各组的8个样本点。虽然理论上只需要4个匹配点就可以计算齐次矩阵了，但是这里仍然用了8个点。 个人理解，这个有点违背 RANSAC 算法选取最小样本的思想。8个样本点相比于4个而言，样本组中更容易包含误匹配的特征点对。
for(int it = 0; it < mMaxIterations; it++) {
for (size_t j = 0; j < 8; j++) {
int idx = mvSets[it][j];
vPn1i[j] = vPn1[mvMatches12[idx].first];
vPn2i[j] = vPn2[mvMatches12[idx].second];
}
接着通过成员函数 ComputeH21 运行 DLT 算法，为每个样本组求解单应矩阵。然后根据归一化的矩阵恢复 H21， 并通过函数 CheckHomography 评估计算结果，筛选匹配点。
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
H21i = T2inv*Hn*T1;
H12i = H21i.inv();
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
最后，选出评分最高的一组样本组，作为最后的输出，更新输出参数。
if(currentScore > score) {
H21 = H21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
} } }
下面，我们依次看一下 Normalize, ComputeH21, CheckHomography 三个函数。
2.2 函数 Normalize
下面是函数 Normalize 的代码片段，该函数用于对输入的特征点集合进行归一化，使得特征点的质心坐标为\(\begin{bmatrix} 0, 0 \end{bmatrix}\)，各点到质心的平均距离为\(\sqrt{2}\)。 它有三个参数，其中 vKeys 是一个输入参数，记录了原始的特征点，容器 vNormalizedPoints 输出了归一化之后的特征点坐标与vKeys一一对应，T输出了进行归一化操作的3x3的变换矩阵。
void Initializer::Normalize(const std::vector<cv::KeyPoint> &vKeys, std::vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T) {
const int N = vKeys.size();
vNormalizedPoints.resize(N);
接下来，先求取特征点的均值坐标，既特征点的质心，如下面的左侧代码所示。再将特征点减去质心完成平移操作，同时计算各点到质心的在x,y两轴上的平均距离，如下面的右侧代码所示。
|
|
最后，先完成特征点坐标的缩放，使得x,y各轴上的平均距离为1，既各点到质心的平均距离为\(\sqrt{2}\)。再将平移和缩放的变换矩阵记录到输出参数 T 中。
|
|
2.3 函数 ComputeH21
下面是函数 ComputeH21 的代码片段，该函数逻辑也很清晰，单纯的实现了最朴素的 DLT 算法。它有两个输入参数vP1和vP2，分别记录了归一化之后的参考帧和当前帧的特征点坐标。 在调用该函数之前，ORB-SLAM2 已经通过函数 Initialize 和 FindHomography 以 RANSAC 的形式采样了 8 组匹配点对。
cv::Mat Initializer::ComputeH21(const std::vector<cv::Point2f> &vP1, const std::vector<cv::Point2f> &vP2) {
const int N = vP1.size();
接下来，根据式(\(\ref{f_DLT_equations}\))构造 DLT 方程组 \(A\hat{H} = \boldsymbol{0}\) 中的系数矩阵\(A\)。
cv::Mat A(2*N,9,CV_32F);
for(int i = 0; i < N; i++) {
const float u1 = vP1[i].x; const float v1 = vP1[i].y; const float u2 = vP2[i].x; const float v2 = vP2[i].y;
|
|
}
最后，通过OpenCV的接口对矩阵\(A\)进行SVD分解，取最小的奇异值在\(V^T\)空间中对应的行向量构建单应矩阵。只是需要注意，这里返回的单应矩阵是归一化之后的，还不能直接使用， 需要恢复归一化之前的矩阵才行。
cv::Mat u,w,vt;
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
return vt.row(8).reshape(0, 3);
}
2.4 函数 CheckHomography
在查看函数 CheckHomography 之前，先简单了解一下如何评价一个单应矩阵的优劣。 由于测量噪声的存在，我们通过 SVD 分解出的单应矩阵 \(H\) 并不能保证 \(A\hat{H} = \boldsymbol{0}\)，不妨令\(\boldsymbol{\varepsilon} = A\hat{H}\)， 称之为误差向量(residual vector)。 DLT 算法实际上是在求解一个优化问题，找到一组\(h_1 \cdots h_8\)使得\(\left\|\boldsymbol{\varepsilon}\right\|^2\)最小。 在 MVG 一书中，将整个损失函数称为代数误差algebraic error。
正如其字面意思，这个损失函数只是代数形式上的误差，并没有任何的几何和统计意义，所以求解它往往并不能得到一个很理想的解。 人们会构造一些更为复杂的损失函数，以 DLT 算法的解为初值，进一步通过迭代优化的方法找到更优的解。比如下面的几何距离Geometric distance。
既然单应矩阵建立了两帧之间的映射关系，那么我们完全可以将参考帧中的特征点经过矩阵\(H\)投射到当前帧上，然后计算投射后的点与匹配点的像素坐标的欧氏距离。 理想情况下，如果特征点的匹配完全正确，那么这一欧式距离完全由单应矩阵\(H\)决定。单应矩阵\(H\)越准确，距离越小。这个就是所谓的几何距离，可以形式化的写成下式:
$$ \sum_{i} d(\boldsymbol{x'}, \tilde{H}\boldsymbol{x})^2 $$其中，\(\boldsymbol{x'} = \begin{bmatrix} u' & v' & 1 \end{bmatrix}^T\)为当前帧的特征点坐标， \(\boldsymbol{x} = \begin{bmatrix} u & v & 1 \end{bmatrix}\)为参考帧的特征点坐标，\(\tilde{H}\)为单应矩阵的估计值。 特征点的提取和匹配本身就存在很多误差和不确定性因素，所以大多时候，人们都是用的对称的几何距离，既将当前帧的特征点再投射到参考帧上计算欧式距离：
$$ \sum_{i} d(\boldsymbol{x'}, \tilde{H}\boldsymbol{x})^2 + \sum_{i} d(\boldsymbol{x}, \tilde{H}^{-1}\boldsymbol{x'})^2 $$上式被称为对称变换误差symmetric trannsfer errors，函数 CheckHomography 正是通过该误差来评价单应矩阵的优劣。 论文中说， 他根据\(\chi^2\)检验选择95%的接受域确定阈值\(\Gamma = 5.99\)，如果一对匹配点在单幅图像上的几何距离超过该阈值，认为该匹配点是野点，将被抛弃掉。 ORB-SLAM2 实际使用的单应矩阵评分函数如下:
$$ S_H = \sum_i \left(\rho\left(d(\boldsymbol{x'}, \tilde{H}\boldsymbol{x})^2 \right) + \rho\left(d(\boldsymbol{x}, \tilde{H}^{-1}\boldsymbol{x'})^2 \right)\right) $$ $$ \rho(d^2) = \begin{cases} \Gamma - d^2 & \text{if} & d^2 < \Gamma \\ 0 & \text{if} & d^2 ≥ \Gamma \end{cases} $$下面是函数 CheckHomography 的代码片段，该函数有4个参数。其中，H21 和 H12 是一对互逆的 3x3 的矩阵分别表示当前帧到参考帧的单应矩阵，以及参考帧到当前帧的单应矩阵。 输出参数 vbMatchesInliers 是与匹配点对一一对应的布尔数组，标识着在 H21 和 H12 的变换下对应的匹配点对是否未发生误匹配。 sigma 是一个标准差的参数，假设几何距离服从方差为\(\sigma^2\)的高斯分布。函数 FindHomography 调用的时候传参为 1.0， 论文中声称该方差的意义是一个像素。
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, std::vector<bool> &vbMatchesInliers, float sigma) {
const int N = mvMatches12.size();
接下来的这一大段只是把 H21 和 H12 中的元素用一堆临时的变量标记下来，方便后续通过式(\(\ref{f_homography_ext}\))将特征点投射到另一幅图像上。
|
|
接着，构建局部变量 score 记录单应矩阵的评分\(S_H\)，设定阈值 th 为5.991，遍历所有的匹配特征点。局部变量 bIn 用于标记考察的匹配点对是否为内点。
vbMatchesInliers.resize(N);
float score = 0;
const float th = 5.991;
const float invSigmaSquare = 1.0/(sigma*sigma);
for(int i = 0; i < N; i++) {
bool bIn = true;
const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
const float u1 = kp1.pt.x; const float v1 = kp1.pt.y;
const float u2 = kp2.pt.x; const float v2 = kp2.pt.y;
然后根据式(\(\ref{f_homography_ext}\))先将当前帧的特征点投射到参考帧上，并计算几何距离，判定是否超出阈值。再将参考帧的特征点投射到当前帧上，计算 \(S_H\)。
const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);
const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;
const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);
const float chiSquare1 = squareDist1*invSigmaSquare;
if(chiSquare1 > th)
bIn = false;
else
score += th - chiSquare1;
const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;
const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
const float chiSquare2 = squareDist2*invSigmaSquare;
if(chiSquare2 > th)
bIn = false;
else
score += th - chiSquare2;
最后根据局部变量 bIn 记录下各个匹配点对是否为内点。 看到这么多重复的代码，实在有点不舒服，也很影响我的排版。
if (bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
}
return score;
}
3. 分解单应矩阵的算法
根据 Faugeras 等人在 1988年发表的文章，可以从矩阵 \(H\) 中分解出8种可能的解。 一个合理的解应当能够保证三角测量后的特征点空间坐标都在相机的前面。所以 ORB-SLAM2 针对每一种解，对特征点进行三角测量，选取使得在相机之前的特征点最多的那组解输出。
根据关系\(\boldsymbol{H} = \boldsymbol{K}\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}\)， 按照下式定义矩阵\(\boldsymbol{A}\)。 若 \(\boldsymbol{A}\) 有三个不同的奇异值，则有8种不同的解；若有一个二重奇异值，则只有4种解；若只有一个3重奇异值，那么解是partially undetermined。
$$ \begin{equation}\label{f_ha} \boldsymbol{A} = d\boldsymbol{K}^{-1}\boldsymbol{H}\boldsymbol{K} = d\boldsymbol{R} + \boldsymbol{t}\boldsymbol{N}^T \end{equation} $$Proposition 3 Equation (\(\ref{f_ha}\)) has, in general, 8 different solutions. It has only 4 iff \(\boldsymbol{A}\) has a singular value of multiplicity 2. The problem is partially undetermined iff \(\boldsymbol{A}\) has a singular value of multiplicity 3.
1988. Motion and structure from motion in a piecewise plannar environment. 第4章
对矩阵\(\boldsymbol{A}\)进行 SVD 分解，有\(\boldsymbol{A} = \boldsymbol{U\Lambda V^T}\)。 其中，\(\boldsymbol{U, V}\)是两个3x3的正交矩阵，满足\(\boldsymbol{UU^T} = \boldsymbol{VV^T} = \boldsymbol{I}\)， \(\boldsymbol{\Lambda} = \begin{bmatrix} d_1 & 0 & 0 \\ 0 & d_2 & 0 \\ 0 & 0 & d_3 \end{bmatrix}\)是奇异值按大小顺序排列的对角阵，有\(d_1 ≥ d_2 ≥ d_3\)。 代入式(\(\ref{f_ha}\))有：
$$ \begin{array}{cl} & \boldsymbol{\Lambda} = \boldsymbol{U}^T \boldsymbol{A} \boldsymbol{V} = \boldsymbol{U}^T \left(d\boldsymbol{R} + \boldsymbol{t}\boldsymbol{N}^T\right) \boldsymbol{V} \\ \Rightarrow & \boldsymbol{\Lambda} = d\boldsymbol{U}^T \boldsymbol{R} \boldsymbol{V} + (\boldsymbol{U}^T \boldsymbol{t})(\boldsymbol{V}^T \boldsymbol{N})^T \end{array} $$令\(s = |\boldsymbol{U}||\boldsymbol{V}|\)，因为\(\boldsymbol{U, V}\)是正交的，有\(s^2 = 1\)，代入上式有：
$$ \begin{equation}\label{f_Lambda} \begin{array}{cl} \boldsymbol{\Lambda} & = s^2d\boldsymbol{U}^T \boldsymbol{R} \boldsymbol{V} + (\boldsymbol{U}^T \boldsymbol{t})(\boldsymbol{V}^T \boldsymbol{N})^T \\ & = \underbrace{(sd)}_{d'} \underbrace{(s\boldsymbol{U}^T \boldsymbol{R} \boldsymbol{V})}_{\boldsymbol{R}'} + \underbrace{(\boldsymbol{U}^T \boldsymbol{t})}_{\boldsymbol{t}'} \underbrace{(\boldsymbol{V}^T \boldsymbol{N})^T}_{\boldsymbol{N}'^T} \\ \end{array} \end{equation} $$\(\boldsymbol{R, t, N}, d\) 与 \(\boldsymbol{R', t', N'}, d'\)存在如下关系:
$$ \begin{equation}\label{f_ha_Lambda} \begin{cases} \boldsymbol{R} & = s\boldsymbol{UR'V^T} \\ \boldsymbol{t} & = \boldsymbol{Ut'} \\ \boldsymbol{N} & = \boldsymbol{VN'} \\ d & = sd' \\ s & = |\boldsymbol{U}||\boldsymbol{V}| \end{cases} \end{equation} $$若\(\boldsymbol{N'} = \begin{bmatrix} x_1 & x_2 & x_3 \end{bmatrix}^T\)，给定一组标准正交基\(\boldsymbol{e_1, e_2, e_3}\)， 有向量\(\boldsymbol{N}' = x_1 \boldsymbol{e_1} + x_2 \boldsymbol{e_2} + x_3 \boldsymbol{e_3}\)，式(\(\ref{f_Lambda}\))可以展开为:
$$ \begin{equation}\label{f_ha_Lambda_ext} \begin{cases} d_1 \boldsymbol{e_1} = d'\boldsymbol{R' e_1} + \boldsymbol{t'}x_1 \\ d_2 \boldsymbol{e_2} = d'\boldsymbol{R' e_2} + \boldsymbol{t'}x_2 \\ d_3 \boldsymbol{e_3} = d'\boldsymbol{R' e_3} + \boldsymbol{t'}x_3 \\ \end{cases} \Rightarrow \begin{cases} d_1 \boldsymbol{e_1} x_2 - d_2 \boldsymbol{e_2} x_1 = d'\boldsymbol{R'}(x_2\boldsymbol{e_1} - x_1\boldsymbol{e_2}) \\ d_2 \boldsymbol{e_2} x_3 - d_3 \boldsymbol{e_3} x_2 = d'\boldsymbol{R'}(x_3\boldsymbol{e_2} - x_2\boldsymbol{e_3}) \\ d_3 \boldsymbol{e_3} x_1 - d_1 \boldsymbol{e_1} x_3 = d'\boldsymbol{R'}(x_1\boldsymbol{e_3} - x_3\boldsymbol{e_1}) \\ \end{cases} \end{equation} $$由于矩阵\(\boldsymbol{R}\)是一个旋转矩阵，并且\(\boldsymbol{U, V}\)是两个正交矩阵，所以\(\boldsymbol{R'}\)也是一个旋转矩阵。旋转矩阵乘以一个向量，不会改变向量的模长。 所以上式左右两侧的向量模长相等:
$$ \begin{equation}\label{f_ha_Lambda_ext_len} \begin{cases} d_1^2 x_2^2 + d_2^2 x_1^2 = d'^2 x_2^2 + d'^2 x_1^2 \\ d_2^2 x_3^2 + d_3^2 x_2^2 = d'^2 x_3^2 + d'^2 x_2^2 \\ d_3^2 x_1^2 + d_1^2 x_3^2 = d'^2 x_1^2 + d'^2 x_3^2 \end{cases} \Rightarrow \begin{cases} (d'^2 - d_2^2) x_1^2 + (d'^2 - d_1^2) x_2^2 = 0 \\ (d'^2 - d_3^2) x_2^2 + (d'^2 - d_2^2) x_3^2 = 0 \\ (d'^2 - d_1^2) x_3^2 + (d'^2 - d_3^2) x_1^2 = 0 \end{cases} \Rightarrow \underbrace{ \begin{bmatrix} (d'^2 - d_2^2) & (d'^2 - d_1^2) & 0 \\ 0 & (d'^2 - d_3^2) & (d'^2 - d_2^2) \\ (d'^2 - d_3^2) & 0 & (d'^2 - d_1^2) \end{bmatrix} }_{\boldsymbol{\Psi}} \underbrace{ \begin{bmatrix} x_1^2 \\ x_2^2 \\ x_3^2 \end{bmatrix} }_{\boldsymbol{N'}} = \boldsymbol{0} \end{equation} $$向量\(\boldsymbol{N}\)是观测平面的法向量模长为1，又\(\boldsymbol{N'} = \boldsymbol{V^T N}\)，并且\(V^T\)正交，所以 \(\boldsymbol{N'}\) 也是一个单位向量， 既\(x_1^2 + x_2^2 + x_3^2 = 1\)。因此，式(\(\ref{f_ha_Lambda_ext_len}\))中的线性方程组\(\boldsymbol{\Psi N'} = \boldsymbol{0}\)有非零解， 说明矩阵\(\boldsymbol{\Psi}\)不满秩，其行列式等于0。
$$ \begin{equation}\label{f_Psi_det} | \boldsymbol{\Psi} | = 2(d'^2 - d_1^2)(d'^2 - d_2^2)(d'^2 - d_3^2) = 0 \end{equation} $$根据奇异值重根的阶数不同，有三种情况，都有 \(d' = ±d_2\):
- \(d_1 > d_2 > d_3\): 若\(d' = ±d_1\)，代入式(\(\ref{f_ha_Lambda_ext_len}\))中， 有\(x_1 = 0, (d_1^2 - d_3^2)x_2^2 + (d_1^2 - d_2^2)x_3^2 = 0\)，可以得出结论\(x_1 = x_2 = x_3 = 0\)，这与\(\boldsymbol{N'}\)是单位向量矛盾，所以\(d' \neq ±d_1\)。 同理有\(d' \neq ±d_3\)。
- \(d_1 = d_2 > d_3\)或者\(d_1 > d_2 = d_3\): 如果 \(d_1 = d_2\)，那么假设\(d' = ±d_3\)，代入式(\(\ref{f_ha_Lambda_ext_len}\))中，可以推导出\(\boldsymbol{N'} = 0\)的错误结论。 同理如果\(d_2 = d_3\)，可以证明\(d' \neq d_1\)。
- \(d_1 = d_2 = d_3\): 显然。
将\(d' = ±d_2\)代入式(\(\ref{f_ha_Lambda_ext_len}\))有
$$ \begin{equation}\label{f_ha_Lambda_ext_d} \underbrace{ \begin{bmatrix} 0 & (d_2^2 - d_1^2) & 0 \\ 0 & (d_2^2 - d_3^2) & 0 \\ (d_2^2 - d_3^2) & 0 & (d_2^2 - d_1^2) \end{bmatrix} }_{\boldsymbol{\Psi}} \underbrace{ \begin{bmatrix} x_1^2 \\ x_2^2 \\ x_3^2 \end{bmatrix} }_{\boldsymbol{N'}} = \boldsymbol{0} \Rightarrow \begin{cases} x_2 = 0 \\ (d_2^2 - d_3^2) x_1^2 = (d_1^2 - d_2^2) x_3^2 \end{cases} \end{equation} $$若\(d_1 \neq d_3\)，考虑到\(x_1^2 + x_2^2 + x_3^2 = 1\)，上式(\(\ref{f_ha_Lambda_ext_d}\))有解：
$$ \begin{equation}\label{f_n_solution} \begin{cases} x_1 = \varepsilon_1 \sqrt{\frac{d_1^2 - d_2^2}{d_1^2 - d_3^2}} \\ x_2 = 0 \\ x_3 = \varepsilon_3 \sqrt{\frac{d_2^2 - d_3^2}{d_1^2 - d_3^2}} \\ \varepsilon_1, \varepsilon_3 = ±1 \end{cases} \end{equation} $$当 \(d' > 0\) 时，将 \(d' = d_2\) 代入式(\(\ref{f_ha_Lambda_ext}\))中，有\(\boldsymbol{R' e_2} = \boldsymbol{e_2}\)， 说明\(\boldsymbol{R'}\)是绕轴\(\boldsymbol{e_2}\)转动的旋转矩阵。
$$ \begin{equation}\label{f_R} \left . \begin{array}{rl} & d_2 \boldsymbol{e_2} = d_2\boldsymbol{R' e_2} + \boldsymbol{t'}x_2 \\ \\ \Rightarrow & \boldsymbol{R' e_2} = \boldsymbol{e_2} \end{array} \right | \Longrightarrow \boldsymbol{R'} = \begin{bmatrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{bmatrix} {\color{Gray} { \underbrace{ \boldsymbol{R'} = \begin{bmatrix} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{bmatrix} }_{ 论文中的原本的\boldsymbol{R'}，只是影响了 \sin \theta 的符号，但是由于 \varepsilon_1, \varepsilon_3 的存在并不影响最后的结论 } }} \end{equation} $$将上式代入式(\(\ref{f_ha_Lambda_ext}\))中
$$ \left . \begin{array}{rl} & d_3 \boldsymbol{e_3} x_1 - d_1 \boldsymbol{e_1} x_3 = d'\boldsymbol{R'}(x_1\boldsymbol{e_3} - x_3\boldsymbol{e_1}) \\ \Rightarrow & \begin{bmatrix} -d_1 x_3 \\ 0 \\ d_3 x_1 \end{bmatrix} = d_2 \begin{bmatrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{bmatrix} \begin{bmatrix} -x_3 \\ 0 \\ x_1 \end{bmatrix} \end{array} \right | \Longrightarrow \begin{bmatrix} -d_2 x_3 & d_2 x_1 \\ d_2 x_1 & d_2 x_3 \\ \end{bmatrix} \begin{bmatrix} \cos \theta \\ \sin \theta \\ \end{bmatrix} = \begin{bmatrix} -d_1 x_3 \\ d_3 x_1 \\ \end{bmatrix} $$根据奇异值重根的阶数不同，有三种情况:
- \(d_1 > d_2 > d_3\): 根据式(\(\ref{f_n_solution}\))，求解上式有: $$ \begin{equation}\label{f_solution_R} \begin{cases} \sin \theta & = (d_3 - d_1)\frac{x_1 x_3}{d_2} & = -\varepsilon_1 \varepsilon_3 \frac{\sqrt{(d_1^2 - d_2^2)(d_2^2 - d_3^2)}}{(d_1 + d_3) d_2} \\ \cos \theta & = \frac{d_1 x_3^2 + d_3 x_1^2}{d_2} & = \frac{d_2^2 + d_1 d_3}{(d_1 + d_3)d_2} \end{cases} \end{equation} $$ 根据式(\(\ref{f_ha_Lambda_ext}\))，有: $$ \begin{equation}\label{f_solution_t} \boldsymbol{t'} = (d_1 - d_3)\begin{bmatrix} x_1 \\ 0 \\ - x_3 \end{bmatrix} \end{equation} $$ \(\varepsilon_1, \varepsilon_3 = ± 1\)，的取值有四种组合，对应\(\boldsymbol{R', t'}\) 的四种解
- \(d_1 = d_2 > d_3\)或者\(d_1 > d_2 = d_3\): 根据式(\(\ref{f_ha_Lambda_ext_d}\))，以及\(x_1^2 + x_2^2 + x_3^2 = 1\)，有： $$ \left . d_1 = d_2 \begin{cases} x_1 = x_2 = 0, x_3 = ±1 \\ \boldsymbol{R'} = \boldsymbol{I} \\ \boldsymbol{t'} = (d_3 - d_1)\boldsymbol{N'} \end{cases} \right | d_2 = d_3 \begin{cases} x_1 = ±1, x_2 = x_3 = 0 \\ \boldsymbol{R'} = \boldsymbol{I} \\ \boldsymbol{t'} = (d_3 - d_1)\boldsymbol{N'} \end{cases} $$
- \(d_1 = d_2 = d_3\): 无法确定\(\boldsymbol{N'}\)，根据式(\(\ref{f_ha_Lambda_ext}\))，有: $$ \boldsymbol{R'} = \boldsymbol{I} \\ \boldsymbol{t'} = 0 $$ 此时相机只有转动，无法求得平面法向量。
当 \(d' < 0\) 时，将 \(d' = -d_2\) 代入式(\(\ref{f_ha_Lambda_ext}\))中，有\(\boldsymbol{R' e_2} = -\boldsymbol{e_2}\)， 如果先绕着 \(\boldsymbol{e_2}\) 转动了 \(\varphi\)，在绕着\(\boldsymbol{e_1}\)转动180°，就可以组合出\(\boldsymbol{R'}\)。 关于旋转矩阵和矩阵相乘的顺序可以参见刚体运动与齐次变换。
$$ \begin{equation}\label{f_nR} \left . \begin{array}{rl} & d_2 \boldsymbol{e_2} = -d_2\boldsymbol{R' e_2} + \boldsymbol{t'}x_2 \\ \\ \Rightarrow & \boldsymbol{R' e_2} = -\boldsymbol{e_2} \end{array} \right | \Longrightarrow \boldsymbol{R'} = \underbrace{ \begin{bmatrix} 1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & -1 \end{bmatrix} }_{绕\boldsymbol{e_1}转动180°} \underbrace{ \begin{bmatrix} \cos \varphi & 0 & \sin \varphi \\ 0 & 1 & 0 \\ -\sin \varphi & 0 & \cos \varphi \end{bmatrix} }_{绕\boldsymbol{e_2}转动\varphi} = \begin{bmatrix} \cos \varphi & 0 & \sin \varphi \\ 0 & -1 & 0 \\ \sin \varphi & 0 & -\cos \varphi \end{bmatrix} \end{equation} $$根据奇异值重根的阶数不同，有三种情况:
- \(d_1 > d_2 > d_3\): 根据式(\(\ref{f_n_solution}\))，求解上式有: $$ \begin{equation}\label{f_solution_nR} \begin{cases} \sin \varphi & = (d_1 + d_3)\frac{x_1 x_3}{d_2} & = \varepsilon_1 \varepsilon_3 \frac{\sqrt{(d_1^2 - d_2^2)(d_2^2 - d_3^2)}}{(d_1 - d_3) d_2} \\ \cos \varphi & = \frac{d_3 x_1^2 - d_1 x_3^2}{d_2} & = \frac{d_1 d_3 - d_2^2}{(d_1 - d_3)d_2} \end{cases} \end{equation} $$ 根据式(\(\ref{f_ha_Lambda_ext}\))，有: $$ \begin{equation}\label{f_solution_nt} \boldsymbol{t'} = (d_1 + d_3)\begin{bmatrix} x_1 \\ 0 \\ x_3 \end{bmatrix} \end{equation} $$ \(\varepsilon_1, \varepsilon_3 = ± 1\)，的取值有四种组合，对应\(\boldsymbol{R', t'}\) 的四种解
- \(d_1 = d_2 > d_3\)或者\(d_1 > d_2 = d_3\): 根据式(\(\ref{f_ha_Lambda_ext_d}\))，以及\(x_1^2 + x_2^2 + x_3^2 = 1\)，有： $$ \left . d_1 = d_2 \begin{cases} x_1 = x_2 = 0, x_3 = ±1 \\ \boldsymbol{R'} = \begin{bmatrix} -1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \\ \boldsymbol{t'} = (d_3 + d_1)\boldsymbol{N'} \end{cases} \right | d_2 = d_3 \begin{cases} x_1 = ±1, x_2 = x_3 = 0 \\ \boldsymbol{R'} = \begin{bmatrix} -1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \\ \boldsymbol{t'} = (d_3 + d_1)\boldsymbol{N'} \end{cases} $$
- \(d_1 = d_2 = d_3\): 对于任意的向量\(\boldsymbol{x}\)都有\(\boldsymbol{R'x}=-\boldsymbol{x}\)，因此有: $$ \boldsymbol{R'} = -\boldsymbol{I} + 2\boldsymbol{N'N'^T} \\ \boldsymbol{t'} = -2d'\boldsymbol{N'} $$
ORB-SLAM2 只处理 \(d_1 > d_2 > d_3\) 的情况，根据\(d', \varepsilon_1, \varepsilon_3\)的符号一共有 8 种不同的解。 下面我们详细分析函数 ReconstructH，了解具体的实现过程。
4. 分解单应矩阵的过程
下面是函数 ReconstructH 的代码片段，它有 9 个参数。其中，vbMatchesInliers 和 H21 是函数 FindHomography() 输出的内点标记和单应矩阵。 K 为相机的内参矩阵。参数 R21, t21, vP3D 分别记录了最后输出的旋转矩阵、平移向量、特征点的三角测量结果。参数 vbTriangulated 标记着每个特征点是否成功被三角测量了。 参数 minParallax 和 minTriangulated 是用来筛选解的两个阈值，表示最小视差角和成功完成三角测量的最少特征点数。
在 H21 的左右两侧分别乘以 K 的逆和 K 就可以得到矩阵\(\boldsymbol{A} = d\boldsymbol{R} + \boldsymbol{t}\boldsymbol{N}^T\)， 按照上一节的原理就可以分解出 8 种可能的解。ORB-SLAM2 针对每一种解，对特征点进行三角测量， 选取使得“在相机之前并且具有足够视差”的特征点最多的那组解输出。如果满足条件的特征点数量小于 minTriangulated 则认为当前帧不适合进行初始化。
bool Initializer::ReconstructH(std::vector<bool> &vbMatchesInliers, // 匹配点对为内点的标记
cv::Mat &H21, // 函数 FindHomography() 输出的单应矩阵
cv::Mat &K, // 相机的内参矩阵
cv::Mat &R21, // 输出旋转矩阵
cv::Mat &t21, // 输出平移向量
std::vector<cv::Point3f> &vP3D, // 对特征点进行三角测量后输出的世界坐标
std::vector<bool> &vbTriangulated, // 特征点是否成功三角测量了
float minParallax, // 最小视差角
int minTriangulated) { // 成功完成三角测量的最少特征点数
一开始，先遍历一遍输入参数 vbMatchesInliers 统计内点的数量，用局部变量 N 记录。
int N=0;
for (size_t i = 0, iend = vbMatchesInliers.size() ; i < iend; i++)
if (vbMatchesInliers[i])
N++;
然后，根据式(\(\ref{f_ha}\))构建矩阵\(\boldsymbol{A}\)，如下面的左侧代码所示。右侧代码，调用了 OpenCV 的接口对矩阵\(\boldsymbol{A}\)进行了 SVD 分解。
|
|
OpenCV 把奇异值按照大小排序之后保存到局部变量 w 中了。在下面左侧的代码片段中，把它们提取出来， 按照论文的符号设定，用局部变量 d1, d2, d3保存。 上一节介绍单应矩阵分解算法的时候了解到，当不存在多重奇异值的时候有 8 种不同的情况。在数值计算时，我们基本不会得到奇异值相等的时候。 所以这里通过比例关系接近于 1 来判定两个奇异值是否差异很小，进而剔除掉那些疑似有多重奇异值的情况，直接放弃分解。
|
|
上面右侧的代码是在按照式(\(\ref{f_n_solution}\))构造 \(\boldsymbol{x_1, x_3}\)。因为\(\varepsilon_1, \varepsilon_3 = ±1\)共有4种组合，分别用数组 x1[] 和 x3[] 枚举之。
接下来，先处理\(d' > 0\)的情况。下面的代码片段按照式(\(\ref{f_solution_R}\))构造\(\sin \theta, \cos \theta\)，同样因为\(\varepsilon_1, \varepsilon_3 = ±1\)的4种组合， \(\sin \theta\)也有4种。 代码的实现完全是按照论文中的描述来的，但是由于我在上一节的推导过程中对式(\(\ref{f_R}\))做了一些细微的改动，所以这里 stheta 的符号需要特别注意一下， 以原始的实现为准。
float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
下面的代码片段中，计算了式(\(\ref{f_ha_Lambda}\)) 中的 \(s = |\boldsymbol{U}||\boldsymbol{V}|\)，用于后续从\(\boldsymbol{R'}\)中恢复\(\boldsymbol{R}\)。 同时定义了数组 vR, vt, vn 分别用于保存 8 种解的旋转矩阵、平移向量、平面法向量。
float s = cv::determinant(U)*cv::determinant(Vt);
vector<cv::Mat> vR, vt, vn;
vR.reserve(8);
vt.reserve(8);
vn.reserve(8);
并在下面的代码中，按照式(\(\ref{f_ha_Lambda}\), \(\ref{f_R}\), \(\ref{f_solution_t}\))构造 \(d' > 0\) 时的四种解。
for (int i = 0; i < 4; i++) {
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=ctheta;
Rp.at<float>(0,2)=-stheta[i];
Rp.at<float>(2,0)=stheta[i];
Rp.at<float>(2,2)=ctheta;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=-x3[i];
tp*=d1-d3;
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
cv::Mat n = V*np;
if(n.at<float>(2) < 0)
n=-n;
vn.push_back(n);
}
类似的，针对\(d' < 0\) 的情况。下面的代码片段按照式(\(\ref{f_solution_nR}\))构造\(\sin \varphi, \cos \varphi\)，考虑到\(\varepsilon_1, \varepsilon_3 = ±1\)，\(\sin \varphi\)也有四种。
float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
接着在下面的代码中，按照式(\(\ref{f_ha_Lambda}\), \(\ref{f_nR}\), \(\ref{f_solution_nt}\))构造 \(d' < 0\)时的四种解。
for(int i=0; i < 4; i++) {
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=cphi;
Rp.at<float>(0,2)=sphi[i];
Rp.at<float>(1,1)=-1;
Rp.at<float>(2,0)=sphi[i];
Rp.at<float>(2,2)=-cphi;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=x3[i];
tp*=d1+d3;
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
cv::Mat n = V*np;
if(n.at<float>(2)<0)
n=-n;
vn.push_back(n);
}
下面的代码片段是在遍历8种解，分别调用函数 CheckRT 来对特征点进行三角测量，选取成功三角测量的特征点数最多的那组解作为最后的输出。 这段代码写的真是巨长，但大多数都是在定义临时变量，保存中间数据。真正有用的是函数 CheckRT，因为篇幅的原因， 该函数我们放到三角测量中介绍。
int bestGood = 0;
int secondBestGood = 0;
int bestSolutionIdx = -1;
float bestParallax = -1;
vector bestP3D;
vector bestTriangulated;
// Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
// We reconstruct all hypotheses and check in terms of triangulated points and parallax
for (size_t i=0; i < 8; i++)
{
float parallaxi;
vector<cv::Point3f> vP3Di;
vector<bool> vbTriangulatedi;
int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);
if(nGood>bestGood)
{
secondBestGood = bestGood;
bestGood = nGood;
bestSolutionIdx = i;
bestParallax = parallaxi;
bestP3D = vP3Di;
bestTriangulated = vbTriangulatedi;
}
else if(nGood>secondBestGood)
{
secondBestGood = nGood;
}
}
if(secondBestGood < 0.75*bestGood && bestParallax >=minParallax && bestGood > minTriangulated && bestGood > 0.9*N)
{
vR[bestSolutionIdx].copyTo(R21);
vt[bestSolutionIdx].copyTo(t21);
vP3D = bestP3D;
vbTriangulated = bestTriangulated;
return true;
}
return false;
}
5. 完
本文中，我们先介绍计算单应矩阵的DLT 算法，再分析函数 FindHomography 及其调用的其它函数了解实现过程。 然后，解读了Faugeras 等人在 1988年发表的文章， 推导从\(H\)矩阵分解出相机位姿\(R,\boldsymbol{t}\)的公式，再分析函数 ReconstructH 了解实现过程。 分析函数 ReconstructH 时，我们遗留了一个函数 CheckRT，将放到三角测量中介绍。