基础矩阵的求解过程

我们在帧间位姿估计和地图初始化总体过程中分析了单目相机的初始化过程，其中提到，针对平面场景(planar scene)和非平面场景(non-planar scene)，ORB-SLAM2提供了单应(homography)和基础矩阵(fundamental matrix)两种几何模型。它会在两个线程中同时计算这两个模型，然后根据一些启发式信息选取一个较合适的模型。上一篇文章中，我们已经讨论了估计单应矩阵的方法以及如何从中分解出相机位姿。本文中，我们来详细看一下基础矩阵的求解过程。

针对更为一般的非平面场景，根据对极约束，可以在两帧图像之间，建立起从一个点到一条线的映射关系：

$$ \boldsymbol{x'}^T\boldsymbol{K}^{-T}[\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} = 0 $$

这是一个等式为零的约束，其中 $\boldsymbol{x, x'}$ 是两帧图像中对应特征点的像素齐次坐标，$\boldsymbol{K}$ 是相机的内参矩阵。 $[\boldsymbol{t}]_{\times}$是相机平移向量$\boldsymbol{t}$构成的斜对称矩阵， $\boldsymbol{R}$ 是相机的旋转矩阵。我们把$\boldsymbol{K}^{-T}[\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}$称为基础矩阵，用符号$\boldsymbol{F}$表示。 $[\boldsymbol{t}]_{\times}\boldsymbol{R}$被称为本征矩阵(Essential Matrix)，记为$\boldsymbol{E}$。上式可以简记如下，被称为对极约束。

$$ \begin{equation}\label{f1} \begin{array}{c} \boldsymbol{x'}^T \boldsymbol{Fx} = 0 \\ \boldsymbol{x'}^T \boldsymbol{K}^{-T}\boldsymbol{E}\boldsymbol{K}^{-1}\boldsymbol{x} = 0 \\ \end{array} \end{equation} $$

由于相机的内参可以通过标定得到，所以求解出上式中的基础矩阵，也就得到了本征矩阵$\boldsymbol{E}$，它是一个$3\times 3$的矩阵，包含了相机的所有变换信息。虽然它有9个未知参数，但由于对极约束是一个等零的约束，所以该矩阵乘上任何一个非零标量都不会改变约束。因此，这个本征矩阵只有8个自由度。 ORB-SLAM2 就是用了经典的八点法配合 RANSAC 来估计基础矩阵。再对其进行奇异值分解后就可以获得平移向量$\boldsymbol{t}$和旋转矩阵$\boldsymbol{R}$。

本文中，我们先介绍八点法估计基础矩阵的原理，再分析函数 FindFundamental 及其子函数了解详细的实现过程。然后，推导从$\boldsymbol{F}$矩阵分解出相机位姿$R,\boldsymbol{t}$的方法，再分析函数 ReconstructF 了解实现过程。

1. 8点法求解基础矩阵

假设$\boldsymbol{x} = \begin{bmatrix} u & v & 1 \end{bmatrix}^T$为参考帧中的像素齐次坐标， $\boldsymbol{x'} = \begin{bmatrix} u' & v' & 1 \end{bmatrix}^T$为当前帧中与之匹配的坐标。那么式($\ref{f1}$)可以展开如下:

$$ \begin{array}{c} \begin{bmatrix} u' & v' & 1 \end{bmatrix} \begin{bmatrix} f_{11} & f_{12} & f_{13} \\ f_{21} & f_{22} & f_{23} \\ f_{31} & f_{32} & f_{33} \end{bmatrix} \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = 0 \end{array} \\ u'uf_{11} + u'v f_{12} + u' f_{13} + v'u f_{21} + v'vf_{22} + v'f_{23} + uf_{31} + vf_{32} + f_{33} = 0 $$

记$\boldsymbol{f} = \begin{bmatrix} f_{11} & f_{12} & f_{13} & f_{21} & f_{22} & f_{23} & f_{31} & f_{32} & f_{33} \end{bmatrix}^T$，上式可以写成向量点乘的形式：

$$ \begin{bmatrix} u'u & u'v & u' & v'u & v'v & v' & u & v & 1 \end{bmatrix} \boldsymbol{f} = 0 $$

假设我们有 $m$ 对匹配点，根据上式我们可以写出 $m$ 个约束，可以写成 $\boldsymbol{Af} = \boldsymbol{0}$的矩阵形式，如下:

$$ \begin{equation}\label{f2} \boldsymbol{Af} = \begin{bmatrix} u_1'u_1 & u_1'v_1 & u_1' & v_1'u_1 & v_1'v_1 & v_1' & u_1 & v_1 & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ u_m'u_m & u_m'v_m & u_m' & v_m'u_m & v_m'v_m & v_m' & u_m & v_m & 1 \\ \end{bmatrix} \boldsymbol{f} = \boldsymbol{0} \end{equation} $$

通常我们会找到很多对匹配点，构建矩阵 $\boldsymbol{A}$，得到一个超定方程组。由于测量噪声的存在，基本上找不到一个解能够使得方程成立。但我们可以通过最小二乘法找到一个$\boldsymbol{f}$，使$\boldsymbol{Af}$尽可能的接近 $\boldsymbol{0}$。根据 MVG 一书的说法，对矩阵 $\boldsymbol{A}$ 进行SVD分解 $\boldsymbol{A} = U \Sigma V^T$，取$V$中的最后一列，就是一个能够最小化 $\|\boldsymbol{Af} \| / \| \boldsymbol{f} \|$的解。我们至少需要8个点才能求得基础矩阵，这也就是所谓的八点法。

2. 矩阵 $\boldsymbol{F}$ 的求解过程

2.1 函数 FindFundamental

下面是函数 FindFundamental 的代码片段，它有输出三个参数。在函数运行结束后，F21 输出了两帧图像之间的单应矩阵， score 则给出了 F21 的评分 $S_F$，vbMatchesInliers 将标注所有的匹配点对是否为内点。即，是否不是误匹配的点对。

        void Initializer::FindFundamental(vector<bool> & vbMatchesInliers, float & score, cv::Mat & F21) {
            const int N = vbMatchesInliers.size();
            vector<cv::Point2f> vPn1, vPn2;
            cv::Mat T1, T2;
            Normalize(mvKeys1,vPn1, T1);
            Normalize(mvKeys2,vPn2, T2);
            cv::Mat T2t = T2.t();

与FindHomography的套路一样，在该函数的一开始，先通过成员函数 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 F21i;                             // 根据某个 RANSAC 样本组得到的基础矩阵
            vector<bool> vbCurrentInliers(N,false);   // 某个 RANSAC 结果下，匹配点是否为内点的判定结果
            float currentScore;                       // 某个 RANSAC 结果的评分

然后遍历所有的样本组，取出各组的8个样本点。

            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];
                }

接着通过成员函数 ComputeF21 运行八点法，为每个样本组求解基础矩阵。然后根据归一化的矩阵恢复 F21i，并通过函数 CheckFundamental 评估计算结果，筛选匹配点。

                cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
                F21i = T2t*Fn*T1;
                currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

最后，选出评分最高的一组样本组，作为最后的输出，更新输出参数。

                if (currentScore > score) {
                    F21 = F21i.clone();
                    vbMatchesInliers = vbCurrentInliers;
                    score = currentScore;
        }   }   }

函数 Normalize 已经在上一篇文章中分析过了。下面我们来看看 ComputeF21 和 CheckFundamental 两个函数。

2.2 函数 ComputeF21

下面是函数 ComputeF21 的代码片段，有两个输入参数vP1和vP2，分别记录了归一化之后的参考帧和当前帧的特征点坐标。在调用该函数之前，ORB-SLAM2 已经通过函数 Initialize 和 FindFundamental 以 RANSAC 的形式采样了 8 组匹配点对。

        cv::Mat Initializer::ComputeF21(const std::vector<cv::Point2f> &vP1, const std::vector<cv::Point2f> &vP2) {
            const int N = vP1.size();

接下来，根据式($\ref{f2}$)构造方程$\boldsymbol{Af} = \boldsymbol{0}$的系数矩阵$A$。

            cv::Mat A(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;

                A.at<float>(i,0) = u2*u1; A.at<float>(i,1) = u2*v1; A.at<float>(i,2) = u2;
                A.at<float>(i,3) = v2*u1; A.at<float>(i,4) = v2*v1; A.at<float>(i,5) = v2;
                A.at<float>(i,6) = u1;    A.at<float>(i,7) = v1;    A.at<float>(i,8) = 1;
            }

然后，通过OpenCV的接口对矩阵$A$进行SVD分解，取最小的奇异值在$V^T$空间中对应的行向量构建基础矩阵。但是基础矩阵的秩只有2，也就是它应当有一个为0的奇异值。对矩阵 A 进行SVD分解时并不能保证这一点，所以在下面的15到17行对构建的矩阵 Fpre 再次进行SVD分解，令最小的奇异值为0，再重组基础矩阵。

            cv::Mat u,w,vt;
            cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
            cv::Mat Fpre = vt.row(8).reshape(0, 3);
            cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
            w.at(2)=0;
            return  u*cv::Mat::diag(w)*vt;
        }

2.3 函数 CheckFundamental

八点法本质上是将对极约束的方程展开，构造一个超定方程组$\boldsymbol{Af} = \boldsymbol{0}$，寻找一组$f_{11}, f_{12}, \cdots, f_{33}$ 满足 $\|\boldsymbol{f} = 1\|$ 使得$\|\boldsymbol{Af}\|$最小。$\|\boldsymbol{Af}\|$可以称作是一个代数误差，并没有具体的几何意义，对齐进行优化得到的解并不是很精确。

根据对极约束的几何意义，它能够将一幅图像中的某个点投影到另一幅图像的极线(epipolar line)上。假设$\boldsymbol{x'}, \boldsymbol{x}$分别是当前帧和参考帧中一对匹配的特征点。根据2D射影空间中的点和直线的关系，我们可以从对极约束$\boldsymbol{x'}^T \boldsymbol{Fx} = 0$得到参考帧中的特征点在当前帧中投影的极线:

$$ \begin{equation}\label{f3} \boldsymbol{l'} = \boldsymbol{Fx} \end{equation} $$

根据对极约束的转置$\boldsymbol{x}^T \boldsymbol{F}^T \boldsymbol{x'} = 0$，可以得到当前帧中的特征点在参考帧中投影的极线:

$$ \begin{equation}\label{f4} \boldsymbol{l} = \boldsymbol{F}^T\boldsymbol{x'} \end{equation} $$

在2D射影空间中，点与线的矢量点积的几何意义是，点到线的距离。所以在当前帧上的几何距离可以写作$\left(\boldsymbol{x'}^T \boldsymbol{l'}\right)^2$。$\left(\boldsymbol{x}^T \boldsymbol{l}\right)^2$则是参考帧中几何距离。由此，我们可以写出基础矩阵的评分函数：

$$ \begin{equation}\label{f5} \begin{array}{c} S_F = \sum_i \left(\rho\left(\left(\boldsymbol{x'}^T \boldsymbol{l'}\right)^2\right) + \rho\left(\left(\boldsymbol{x}^T \boldsymbol{l}\right)^2\right)\right) = \sum_i \left(\rho\left(\left(\boldsymbol{x'}^T \boldsymbol{Fx}\right)^2\right) + \rho\left(\left(\boldsymbol{x}^T \boldsymbol{F}^T\boldsymbol{x'}\right)^2\right)\right) \\ \rho(d^2) = \begin{cases} \Gamma - d^2 & \text{if} & d^2 < T_F \\ 0 & \text{if} & d^2 ≥ T_F \end{cases} \end{array} \end{equation} $$

论文中说，他根据$\chi^2$检验选择95%的接受域确定阈值$T_F = 3.841$，如果一对匹配点在单幅图像上的几何距离超过该阈值，认为该匹配点是野点，将被抛弃掉。为了统一评分标准，作者特意将$\Gamma$设定为5.991。 3.841和5.991分别是自由度为1和2，显著值为0.05(我这里所说的95%的接受域)的$\chi^2$阈值。至于为啥单应矩阵中用自由度为2的5.991到这里就用3.841，我并不清楚。

下面是函数 CheckFundamental 的代码片段，该函数有3个参数。其中，F21 待检查的基础矩阵估计值。输出参数 vbMatchesInliers 是与匹配点对一一对应的布尔数组，标识着在 F21 描述的约束下对应的匹配点对是否未发生误匹配。 sigma 是一个标准差的参数，假设几何距离服从方差为$\sigma^2$的高斯分布。函数 FindFundamental 调用的时候传参为 1.0，论文中声称该方差的意义是一个像素。

        float Initializer::CheckFundamental(const cv::Mat &F21, vector &vbMatchesInliers, float sigma) {
            const int N = mvMatches12.size();

接下来的这一大段只是把 F21 中的元素用一堆临时的变量标记下来，方便后续计算几何距离。

            const float f11 = F21.at<float>(0,0); const float f12 = F21.at<float>(0,1); const float f13 = F21.at<float>(0,2);
            const float f21 = F21.at<float>(1,0); const float f22 = F21.at<float>(1,1); const float f23 = F21.at<float>(1,2);
            const float f31 = F21.at<float>(2,0); const float f32 = F21.at<float>(2,1); const float f33 = F21.at<float>(2,2);

接着，构建局部变量 score 记录单应矩阵的评分$S_F$，设定阈值 th 为3.841用于筛选内点。局部变量 thScore 赋值5.99用于计算评分。在一个 for 循环中遍历所有的匹配点对，其中局部变量 bIn 用于标记考察的匹配点对是否为内点。

            vbMatchesInliers.resize(N);
            float score = 0;
            const float th = 3.841;
            const float thScore = 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{f3}$)将参考帧的特征点投射到当前帧上，计算极线和几何距离，判定是否超出阈值。再根据式($\ref{f4}$)将当前帧的特征点投射到参考帧帧上，计算几何距离。同时根据式($\ref{f5}$)更新$S_F$。

展开参考帧到当前帧投影>>>

                const float a2 = f11*u1+f12*v1+f13;
                const float b2 = f21*u1+f22*v1+f23;
                const float c2 = f31*u1+f32*v1+f33;

                const float num2 = a2*u2+b2*v2+c2;
                const float squareDist1 = num2*num2/(a2*a2+b2*b2);
                const float chiSquare1 = squareDist1*invSigmaSquare;

                if (chiSquare1 > th)
                    bIn = false;
                else
                    score += thScore - chiSquare1;

展开当前帧到参考帧投影>>>

                const float a1 = f11*u2+f21*v2+f31;
                const float b1 = f12*u2+f22*v2+f32;
                const float c1 = f13*u2+f23*v2+f33;

                const float num1 = a1*u1+b1*v1+c1;
                const float squareDist2 = num1*num1/(a1*a1+b1*b1);
                const float chiSquare2 = squareDist2*invSigmaSquare;

                if(chiSquare2 > th)
                    bIn = false;
                else
                    score += thScore - chiSquare2;

最后根据局部变量 bIn 记录下各个匹配点对是否为内点。

                vbMatchesInliers[i] = bIn;
            }
            return score;
        }

3. 分解基础矩阵的算法

根据基础矩阵$\boldsymbol{F} = \boldsymbol{K}^{-T}[\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}$ 和本征矩阵$\boldsymbol{E} = [\boldsymbol{t}]_{\times}\boldsymbol{R}$的定义，其中$\boldsymbol{K}$是相机的内参矩阵，我们可以从刚刚求解出的基础矩阵中算出本征矩阵:

$$ \boldsymbol{E} = \boldsymbol{K}^T \boldsymbol{FK} $$

$[\boldsymbol{t}]_{\times}$是由相机的平移向量$\boldsymbol{t}$构成的斜对称矩阵，记作$\boldsymbol{S}$吧，有$\boldsymbol{S}^T = -\boldsymbol{S}$。据说，对于任意的$3\times 3$的斜对称矩阵都可以分解成 $k \boldsymbol{UZU^T}$的形式，其中$\boldsymbol{U}$是一个正交矩阵，$k$为一个非零的常数。 $\boldsymbol{Z}$具有如下的形式：

$$ \boldsymbol{Z} = \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} = -\underbrace{\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{bmatrix}}_{\boldsymbol{D}_{1, 1, 0}}\underbrace{\begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix}}_{\boldsymbol{W}} $$

在对极约束下，我们忽略符号的作用，有$\boldsymbol{Z} = \boldsymbol{D}_{1,1,0}\boldsymbol{W}$。忽略尺度因子$k$的作用，有$\boldsymbol{S} = \boldsymbol{U}\boldsymbol{D}_{1,1,0}\boldsymbol{W}\boldsymbol{U}^T$。本征矩阵可以分解为$\boldsymbol{E} = \boldsymbol{U}\boldsymbol{D}_{0,0,1}(\boldsymbol{W}\boldsymbol{U}^T\boldsymbol{R})$。容易验证$\boldsymbol{W}$是一个正交矩阵，所以$(\boldsymbol{W}\boldsymbol{U}^T\boldsymbol{R})$也是一个正交矩阵，记为$\boldsymbol{V}^T$。那么$\boldsymbol{U}\boldsymbol{D}_{-k,-k,0}\boldsymbol{V}^T$就是$\boldsymbol{E}$的SVD分解。有推论：

A $3\times 3$ matrix is an essential matrix if and only if two of its singular values are equal, and the third is zero.
一个$3\times 3$的矩阵是本征矩阵的充要条件是，它有两个相等的奇异值，并且第三个奇异值为0。

上述的推导得到$\boldsymbol{E} = \boldsymbol{U}\boldsymbol{D}_{0,0,1}(\boldsymbol{W}\boldsymbol{U}^T\boldsymbol{R})$的过程中，我们忽略了符号和尺度的作用。如果本征矩阵分解为$\boldsymbol{E} = \boldsymbol{U}\boldsymbol{D}_{1,1,0}\boldsymbol{V}^T$的形式，那么$\boldsymbol{E} = \boldsymbol{SR}$有两种可能的分解：

$$ \text{(1)} \begin{cases} \boldsymbol{S} = \boldsymbol{UZU^T} \\ \boldsymbol{R} = \boldsymbol{UWV^T} \end{cases} \text{(2)} \begin{cases} \boldsymbol{S} = \boldsymbol{UZU^T} \\ \boldsymbol{R} = \boldsymbol{UW^TV^T} \end{cases} $$

因为在忽略符号的作用的情况下，$\boldsymbol{D}_{1,1,0}\boldsymbol{W}$ 与$\boldsymbol{D}_{1,1,0}\boldsymbol{W}^T$的作用一样。上式中的$\boldsymbol{R}$确定了相机的姿态矩阵。忽略尺度因子的作用时，上式中$\boldsymbol{S} = \boldsymbol{UZU^T}$确定了相机的平移向量。因为向量对自身的叉积为零，所以$\boldsymbol{S}\boldsymbol{t} = 0$，因此$\boldsymbol{t}=\boldsymbol{U}\begin{bmatrix} 0 & 0 & 1\end{bmatrix}^T = \boldsymbol{u_3}$，矩阵$\boldsymbol{U}$的最后一列。但是因为$\boldsymbol{E}$的符号不能确定，所以也不能确定$\boldsymbol{t}$符号。因此相机的位姿共有四种可能：

$$ \begin{equation}\label{f6} \begin{array}{c} \begin{bmatrix} \boldsymbol{UWV^T} | \boldsymbol{u_3} \end{bmatrix} & \begin{bmatrix} \boldsymbol{UWV^T} | -\boldsymbol{u_3} \end{bmatrix} & \begin{bmatrix} \boldsymbol{UW^TV^T} | \boldsymbol{u_3} \end{bmatrix} & \begin{bmatrix} \boldsymbol{UW^TV^T} | -\boldsymbol{u_3} \end{bmatrix} & \end{array} \end{equation} $$

在这4中可能中，只有一种解能够保证特征点位于在两个相机的前面，既深度值为正的。把各个匹配点对代进去分贝检测一遍，就可以找出正确的那个。下面我们详细分析一下函数 ReconstructF，了解具体的实现过程。

4. 分解基础矩阵的过程

下面是函数 ReconstructF 的代码片段，它有 9 个参数。其中，vbMatchesInliers 和 H21 是函数 FindFundamental() 输出的内点标记和单应矩阵。 K 为相机的内参矩阵。参数 R21, t21, vP3D 分别记录了最后输出的旋转矩阵、平移向量、特征点的三角测量结果。参数 vbTriangulated 标记着每个特征点是否成功被三角测量了。参数 minParallax 和 minTriangulated 是用来筛选解的两个阈值，表示最小视差角和成功完成三角测量的最少特征点数。

        bool Initializer::ReconstructF(std::vector<bool> &vbMatchesInliers, // 匹配点对为内点的标记
                                       cv::Mat &F21,                        // 函数 FindFundamental 输出的基础矩阵
                                       cv::Mat &K,                          // 相机的内参矩阵
                                       cv::Mat &R21,                        // 输出旋转矩阵
                                       cv::Mat &t21,                        // 输出平移向量
                                       std::vector<cv::Point3f> &vP3D,      // 对特征点进行三角测量后输出的世界坐标
                                       std::vector<bool> &vbTriangulated,   // 特征点是否成功三角测量了
                                       float minParallax,                   // 最小视差角
                                       int minTriangulated)                 // 成功完成三角测量的最少特征点数

            int N=0;
            for (size_t i = 0, iend = vbMatchesInliers.size() ; i < iend; i++)
                if (vbMatchesInliers[i])
                    N++;
            cv::Mat E21 = K.t()*F21*K;
            cv::Mat R1, R2, t;
            DecomposeE(E21,R1,R2,t);  
            cv::Mat t1=t;
            cv::Mat t2=-t;

如左侧代码片段中的10到13行所示，在函数的一开始，先遍历一遍输入参数 vbMatchesInliers 统计内点的数量，用局部变量 N 记录。

然后第14行，在基础矩阵右左两侧分别乘上相机内参矩阵及其转置，构建本征矩阵。

紧接着调用函数 DecomposeE 分解本征矩阵，得到式($\ref{f6}$)罗列的四种组合。

在接下来的代码片段中，通过调用函数 CheckRT 遍历四种组合，分别进行三角测量。该函数将返回成功进行三角测量的匹配点数量，分别用局部变量 nGoodn 来表示。我们将在三角测量中分析函数 CheckRT。

            std::vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
            std::vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
            float parallax1,parallax2, parallax3, parallax4;

            int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
            int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
            int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
            int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);

            int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));
            int nMinGood = max(static_cast<int>(0.9*N), minTriangulated);
            int nsimilar = 0;
            if(nGood1 > 0.7*maxGood) nsimilar++;
            if(nGood2 > 0.7*maxGood) nsimilar++;
            if(nGood3 > 0.7*maxGood) nsimilar++;
            if(nGood4 > 0.7*maxGood) nsimilar++;
            if (maxGood < nMinGood || nsimilar > 1)
                return false;

然后对比四种解成功进行三角测量的点数量。如果最优的组合成功三角化的数量不足 90% 或者比指定的阈值 minTriangulated 小，则认为当前帧不能估计出相机的位姿。

左侧代码片段中第29到33行在更新局部参数 nsimilar，这主要是在对比最优的组合相比于其它的组合。只有最优的组合能够显著地优于其它组合，才能认为找到了一个合理解。因为理论上式($\ref{f6}$)所描述的四个组合中只有一个是合理的。

如果确定存在一个组合与其它组合相比具有显著的优势，并且能够提供足够的成功三角测量，就进一步查看该组合下的视差值是否满足阈值 minParallax 的要求。接下来是一段重复度非常高的一串代码，其本质就是在遍历各个组合找到最优的那个，检查视差值，如果满足要求就接受之。

展开最优组合 >>>

            
            if(maxGood==nGood1)
            {
                if(parallax1 > minParallax)
                {
                    vP3D = vP3D1;
                    vbTriangulated = vbTriangulated1;

                    R1.copyTo(R21);
                    t1.copyTo(t21);
                    return true;
                }
            }else if(maxGood==nGood2)
            {
                if(parallax2 > minParallax)
                {
                    vP3D = vP3D2;
                    vbTriangulated = vbTriangulated2;

                    R2.copyTo(R21);
                    t1.copyTo(t21);
                    return true;
                }
            }else if(maxGood==nGood3)
            {
                if(parallax3 > minParallax)
                {
                    vP3D = vP3D3;
                    vbTriangulated = vbTriangulated3;

                    R1.copyTo(R21);
                    t2.copyTo(t21);
                    return true;
                }
            }else if(maxGood==nGood4)
            {
                if(parallax4 > minParallax)
                {
                    vP3D = vP3D4;
                    vbTriangulated = vbTriangulated4;

                    R2.copyTo(R21);
                    t2.copyTo(t21);
                    return true;
                }
            }
            return false;
        }

5. 完

本文中，我们先介绍八点法计算基础矩阵的方法，再分析函数 FindFundamental 及其调用的其它函数了解实现过程。然后，介绍如何从基础矩阵中分解出相机的位姿，发现它有四种解，但只有一种能够保证匹配点在两帧图像中具有正的深度值。最后分析函数 ReconstructF 了解分解相机位姿的过程。该函数主要是在调用函数 CheckRT，对匹配点进行三角测量，获得其深度值和视差值。函数 CheckRT 将放到三角测量中介绍。