单目初始化 1

qq_26697045

已于 2023-10-29 15:29:24 修改

阅读量1.4k

点赞数 3

分类专栏： SLAM 文章标签： slam 多视图几何

于 2022-04-02 10:55:45 首次发布

本文链接：https://blog.csdn.net/qq_26697045/article/details/103999740

版权

SLAM 专栏收录该内容

30 篇文章 12 订阅

订阅专栏

欢迎访问我的博客首页。

单目初始化 1

1. 对极约束与单应约束
2. 归一化
3. 基础矩阵
4. 单应矩阵
5. 使用 OpenCV 求本质矩阵和单应矩阵并分解

使用单目相机只能得到特征点的像素坐标。缺少距离信息无法知道一个像素对应的空间坐标，只能根据像素与像素的匹配关系估计相机运动，称为 2D-2D 的位姿估计。

1. 对极约束与单应约束

单目里程计可以根据对极约束和单应约束，利用像素坐标计算相机运动。单应矩阵只能用于物方点共面和平移向量为零的场景；基础矩阵较单应矩阵自由度更多，如果用在单应矩阵适用的场景，多余的自由度将由噪声决定，准确性没有单应矩阵好。因此通常同时使用这两种约束求解相机运动，再根据重投影误差挑选其中较准确的那个。

对极约束与单应约束

图 1.1 对极约束与单应约束

对极约束中的基础矩阵是点到线的映射，即从一个坐标系中的像素坐标到另一个坐标系中的极线方程的映射，详见公式 (3.4) 和公式 (3.5)。单应约束中的单应矩阵是点到点的映射，即从一个坐标系中的像素坐标到另一个坐标系中的像素坐标的映射，详见公式 (1.8)。

单应矩阵适合纯旋转、特征点离相机较远、所有特征点位于同一平面上。

1.1 对极约束

因为向量 $\overrightarrow{O_2P}$ 、 $\overrightarrow{O_1O_2}$ 、 $\overrightarrow{O_1P}$ 共面，所以它们的混合积为 0，这就是对极约束。要构造混合积为 0 的约束，首先应该求出这三个向量在同一个坐标系中的坐标表示。假设从坐标系 1 到坐标系 2 的变换是 $\vec t)$ ，p 是齐次像素坐标，K 是相机内参矩阵，则

因为 P 在坐标系 2 中的坐标是 $s_2K^{-1}p_2$ ，所以 $\overrightarrow{O_2P} = s_2K^{-1}p_2$ 。
因为 $O_1$ 在坐标系 2 中的坐标是 $RO_1 + \vec t = \vec t$ ，所以 $\overrightarrow{O_1O_2} = - \vec t$ 。
因为 $P$ 在坐标系 1 中的坐标是 $s_1K^{-1}p_1$ ，在坐标系 2 中的坐标是 $R(s_1K^{-1}p_1) + \vec t$ ，所以 $\overrightarrow O_1P = Rs_1K^{-1}p_1$ 。

这三个向量的混合积是 $\overrightarrow{O_2p} \times \overrightarrow{O_1O_2} \cdot \overrightarrow{O_1P}$ 。使用去掉常数项后的向量坐标表示混合积为 0 的约束：

$K^{-1}p_2 \times \vec t \cdot RK^{-1}p_1 = \bf 0 \tag{1.1}$

如果把 $\vec t$ 写成反实对称矩阵的形式，向量叉乘就可以写成矩阵相乘的形式。于是公式 (1.1) 就可以写成矩阵相乘的形式：

$p_2^TK^{-T} \cdot \vec t \; \hat{\;} \cdot RK^{-1}p_1 = 0 \tag{1.2}$

其中

$\vec t \; \hat{\;} = \left[ \begin{matrix} 0 & -t_3 & t_2 \\ t_3 & 0 & -t_1 \\ -t_2 & t_1 & 0 \end{matrix} \right]$

公式 (1.2) 称为对极约束。

1.2 基础矩阵与本质矩阵

假如把 $E$ 称为本质矩阵， $F$ 称为基础矩阵：

$\begin{cases} E = \vec t \hat{\quad} R \\ F = K^{-T} E K^{-1} \end{cases} \tag{1.3}$

那么公式 (1.2) 可以写成：

$\begin{cases} (K^{-1} p_2)^T (\vec t \; \hat{\;} R) (K^{-1} p_1) = n_2^{T} E n_1 = 0 \\ p_2^T (K^{-T} \vec t \; \hat{\;} R K^{-1}) p_1 = p_2^{T} F p_1 = 0 \end{cases} \tag{1.4}$

公式中的 p 是像素坐标， $K^{-1} \cdot p$ 是归一化成像平面上的坐标，E 和 F 是相机坐标系 1 到相机坐标系 2 的变换。

由于对极约束是等式为 0 的约束，所以对本质矩阵乘以任意非零常数，对极约束仍然成立。这称为本质矩阵的尺度等价性。本质矩阵包含旋转和平移共 6 个自由度，由于尺度等价性，它实际上只有 5 个自由度。虽然只有 5 个自由度，但通常使用八点法求解本质矩阵，八点法需要 8 对匹配点。

通常使用 8 对匹配的像素点计算出基础矩阵，然后根据 $K^T \cdot F \cdot K$ 得到本质矩阵，接着分解本质矩阵得到 R 和 $\vec t$ 。

1.3 单应矩阵

如图 1.1，假设 $p_1$ 和 $p_2$ 是坐标系 1 和坐标系 2 中的像素坐标， $\vec t)$ 是从坐标系 1 到坐标系 2 的变换；同时假设平面 $I$ 的单位法向量在坐标系 1 中的坐标是 $\vec n$ ，P 是平面 $I$ 上的点在坐标系 1 中的坐标，坐标系 1 的原点 $O_1$ 到平面 $I$ 的距离是 d。则

$\vec n^T \cdot P = d \tag{1.5}$

由坐标变换关系知

$s_2 \cdot p_2 = K \cdot (R \cdot P + \vec t) \tag{1.6}$

为了构造 $p_1$ 与 $p_2$ 的方程，把公式 (1.5) 带入 (1.6)，即

$\begin{aligned} s_2 \cdot p_2 &= K \cdot (R \cdot P + \vec t \cdot \frac{\vec n^T \cdot P}{d}) \\ &= K \cdot (R + \frac{\vec t \cdot \vec n^T}{d}) \cdot P \\ &= s_1 \cdot K \cdot (R + \frac{\vec t \cdot \vec n^T}{d}) \cdot K^{-1} \cdot p_1 \end{aligned} \tag{1.7}$

忽略尺度，公式 (1.7) 可以写成

$\begin{aligned} p_2 &= K \cdot (d \cdot R + \vec t \cdot \vec n^T) \cdot K^{-1} \cdot p_1\\ &= K \cdot A \cdot K^{-1} \cdot p_1 \\ &= H \cdot p_1 \end{aligned} \tag{1.8}$

其中，H 称为单应矩阵

$\left\{\begin{aligned} A &= d \cdot R + \vec t \cdot \vec n^T \\ H &= K \cdot A \cdot K^{-1} \end{aligned}\right. \tag{1.9}$

可以看出，矩阵 A 与矩阵 H 的关系 $K^{-1} \cdot H \cdot K$ 类似与本质矩阵 E 与基础矩阵 F 的关系 $K^T \cdot F \cdot K$ 。通常先使用像素坐标求得单应矩阵 H，再求得矩阵 A，最后从 A 中分解出 $(\vec n, d, R, \vec t)$ 。

单应约束要求所有空间点在同一个平面上。如图 1.1，左右两个像素坐标系间的单应变换矩阵 H 是根据位于平面 $I$ 上的若干个空间点 $P^i$ 在这两个像素坐标系中的像素坐标 $p_1^i$ 与 $p_2^i$ 计算出来的。假如有一个空间点 $P^{'}$ 不在平面 $I$ 上，它在像素坐标系 1 上的投影是 $p_1$ ，且 H 是像素坐标系 1 到像素坐标系 2 的单应变换矩阵，则 $H p_1 = p_2$ ，而事实上 $P^{'}$ 在像素坐标系 2 上的投影是 $p_2'$ 。

1.4 单应矩阵用于图像拼接

单应矩阵可以把一个像素坐标系中的坐标映射到另一个像素坐标系中，因此它可以用于图像拼接，如图 1.2。

图像拼接

图 1.2 图像拼接

虽然 image1 和 image2 上的物方点不一定在一个平面上，但不影响计算单应矩阵：假设有 100 个匹配点，使用 RANSAC 之类的算法可能找出 10 对适合计算单应矩阵的匹配点计算出单应矩阵，拼接时根据该单应矩阵和插值算法把一幅图像上的像素投影到另一幅图像的像素坐标系上即可。因此单应矩阵用于图像拼接时，不要求所有物方点在同一个平面上。

2. 归一化

根据《计算机视觉中的多视图几何》算法 3.2 和算法 10.1，使用 DLT 计算单应矩阵和基础矩阵前，需要先对两组特征点进行归一化，最后对求得的矩阵解除归一化。例如求解单应矩阵的算法如下：

对每个坐标系中的特征点，计算其像素坐标的均值 (meanX, meanY) 和平均差 (meanDevX, meanDevY)。然后让每个特征点除以平均差得到归一化后的特征点 $(\frac{x_i}{meanDevX}, \frac{y_i}{meanDevY})$ ，同时得到归一化变换矩阵 T

$\left[\begin{matrix} \frac{1}{meanDevX} & 0 & -\frac{meanX}{meanDevX} \\ \\ 0 & \frac{1}{meanDevY} & -\frac{meanY}{meanDevY} \\ \\ 0 & 0 & 1 \end{matrix}\right]$

使用 DLT 算法利用归一化后的特征点计算出归一化的单应矩阵 $\widetilde{H}$ 和基础矩阵 $\widetilde{F}$ 。当 F 和 H 是从坐标系 1 到坐标系 2 的变换时，解除归一化的公式是：

$\begin{cases} H &= T_2^{-1} \cdot \widetilde{H} \cdot T_1 \\ F &= T_2^{T} \cdot \widetilde{F} \cdot T_1 \end{cases} \tag{2.1}$

下面是 ORB_SLAM2 中归一化的代码。

void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T) {
    float meanX = 0;
    float meanY = 0;
    const int N = vKeys.size();
    vNormalizedPoints.resize(N);

    // 1. 均值。
    for (int i = 0; i < N; i++)
    {
        meanX += vKeys[i].pt.x;
        meanY += vKeys[i].pt.y;
    }
    meanX = meanX / N;
    meanY = meanY / N;

    // 2. 平均差。meanDev = sum|X - u| / n。
    float meanDevX = 0;
    float meanDevY = 0;
    for (int i = 0; i < N; i++)
    {
        vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
        vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
        meanDevX += fabs(vNormalizedPoints[i].x);
        meanDevY += fabs(vNormalizedPoints[i].y);
    }
    meanDevX = meanDevX / N;
    meanDevY = meanDevY / N;

    // 3. 坐标除以平均差。
    float sX = 1.0 / meanDevX;
    float sY = 1.0 / meanDevY;
    for (int i = 0; i < N; i++)
    {
        vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
        vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
    }

    // 4. 归一化矩阵。
    T = cv::Mat::eye(3, 3, CV_32F);
    T.at<float>(0, 0) = sX;
    T.at<float>(1, 1) = sY;
    T.at<float>(0, 2) = -meanX * sX;
    T.at<float>(1, 2) = -meanY * sY;
}

3. 基础矩阵

3.1 DLT 求基础矩阵

通常先求基础矩阵，然后根据 $K^T \cdot F \cdot K$ 得到本质矩阵。基础矩阵和本质矩阵都是 3 阶方阵，包含 9 个未知数。由于它们具有尺度等价性，所以只有 8 个自由度。最直接的方法就是用 9 个变量表示基础矩阵或本质矩阵，再使用 8 对匹配点的像素坐标或归一化平面坐标，按公式 (1.4) 构造 8 个方程，方程组的解就是基础矩阵或本质矩阵。这称为直接线性变换法(DLT)。

我们使用 9 个未知数表示从坐标 1 变换到坐标 2 的基础矩阵或本质矩阵。根据公式 (1.4) 有

$\left[ \begin{matrix} u_2 & v_2 & 1 \end{matrix} \right] \cdot \left[ \begin{matrix} e_1 & e_2 & e_3 \\ e_4 & e_5 & e_6 \\ e_7 & e_8 & e_9 \end{matrix} \right] \cdot \left[ \begin{matrix} u_1 \\ v_1 \\ 1 \end{matrix} \right] = 0 \tag{3.1}$

如果记 ${\bf \vec e} = [e_1, e_2, e_3, e_4, e_5, e_6, e_7, e_8, e_9]^T$ ，那么

$\left[ \begin{matrix} u_2 u_1 & u_2 v_1 & u_2 & v_2 u_1 & v_2 v_1 & v_2 & u_1 & v_1 & 1 \end{matrix} \right] \cdot {\bf \vec e} = 0 \tag{3.2}$

公式 (3.2) 是使用 1 对匹配点构造的一个方程。使用 8 对匹配点将构造一个方程组

$\left[ \begin{matrix} u^1_2 u^1_1 & u^1_2 v^1_1 & u^1_2 & v^1_2 u^1_1 & v^1_2 v^1_1 & v^1_2 & u^1_1 & v^1_1 & 1 \\ \\ u^2_2 u^2_1 & u^2_2 v^2_1 & u^2_2 & v^2_2 u^2_1 & v^2_2 v^2_1 & v^2_2 & u^2_1 & v^2_1 & 1 \\ \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ \\ u^8_2 u^8_1 & u^8_2 v^8_1 & u^8_2 & v^8_2 u^8_1 & v^8_2 v^8_1 & v^8_2 & u^8_1 & v^8_1 & 1 \end{matrix} \right] \cdot {\bf \vec e} = \vec 0 \tag{3.3}$

求得该齐次线性方程组的一个特解即得从坐标系 1 到坐标系 2 的基础矩阵或本质矩阵。

基础矩阵和本质矩阵都是秩为 2 的 3 阶方阵。更确切地说，它们有两个相等的非 0 奇异值和一个 0 奇异值。求解齐次线性方程组 (3.3) 得到的矩阵不一定满足这个性质。

ORB_SLAM2 为了保证基础矩阵有一个 0 奇异值，使用的调整方法是：对求解的矩阵 M 奇异值分解 $\cdot \Lambda \cdot V^T$ ， $\Lambda$ 是一个 3 阶对角矩阵 $diag(k_1, k_2, k_3)$ ，让 $k_3 =0$ ，再通过 $\cdot \Lambda \cdot V^T$ 得到 M。

如果要保证有两个相等的非 0 奇异值和一个 0 奇异值，可以把 $\Lambda$ 中的奇异值从大到小排序为 $k_1 \geq k_2 \geq k_3$ ，再令 $\Lambda = diag(\frac{k_1 + k_2}{2}, \frac{k_1 + k_2}{2}, 0)$ ，由 $\cdot \Lambda \cdot V^T$ 得到 M。

下面是 ORB_SLAM2 中求解基础矩阵的代码。

cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) {
    const int N = vP1.size(); // N = 8。
    cv::Mat A(N, 9, CV_32F);  // A.shape = (8, 9)。

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

    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<float>(2) = 0;

    return u * cv::Mat::diag(w) * vt;
}

至此求得基础矩阵 F。如果使用了归一化，还要根据公式 (2.1) 对 F 解除归一化。

3.2 选择基础矩阵

匹配点通常比较多，只使用 8 对点容易产生偶然误差。通常的做法是，在匹配点对中随机选取 N 组，每组包含 8 对匹配点，得到 N 个基础矩阵。再为每个基础矩阵打分，根据分数选取最好的基础矩阵。下面是 ORB_SLAM2 中为基础矩阵打分的代码，该函数选出分数最高的基础矩阵和它的分数，然后用该分数与单应矩阵的分数比较，以确定从基础矩阵恢复位姿还是从单应矩阵恢复位姿。

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

    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);

    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;

        // Reprojection error in second image: l2 = F21x1 = (a2, b2, c2)。参考《视觉 SLAM 十四讲》公式 (7.10)。
        const float a2 = f11 * u1 + f12 * v1 + f13;
        const float b2 = f21 * u1 + f22 * v1 + f23;
        const float c2 = f31 * u1 + f32 * v1 + f33;
        // 误差：点 (u2, v2) 到直线 a2x + b2y + c2 = 0 的距离的平方。
        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;

        // Reprojection error in second image: l1 = x2tF21 = (a1, b1, c1)。
        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;

        if (bIn)
            vbMatchesInliers[i] = true;
        else
            vbMatchesInliers[i] = false;
    }

    return score;
}

假如 $p_1$ 在平面 $I_2$ 的投影是 $p_1'$ ，一般用投影点 $p_1'$ 到极线 $l_2$ 的距离的相反数作为基础矩阵的分数。因此，为了计算基础矩阵的分数，首先应该求直线 $l_2$ 在坐标系 2 中的方程。

$F p_1$ 是一个 $\times 1$ 的向量，我们假设该向量是 $a_2, b_2, c_2]^T$ 。 $a_2$ 、 $b_2$ 和 $c_2$ 可以确定一条直线 $a_2 x + b_2 y + c_2 = 0$ ，即 $F p_1$ 可以确定一条直线，我们把这条直线叫做 $l$ 。因为 $p_2^T F p_1 = 0$ ，所以 $p_2$ 在这条直线上。

点 $O_1$ 在坐标系 $O_2$ 中的像素坐标是 $\frac{1}{s}Kt$ ，其中 s 是点 $O_1$ 在坐标系 $O_2$ 中的 Z 坐标，K 是相机内参矩阵，t 是从坐标系 $O_1$ 变换到坐标系 $O_2$ 的平移向量。因为 $(\frac{1}{s} K t)^T F p_1 = \frac{1}{s} t^T K^T F p_1 = \frac{1}{s} t^T K^T K^{-T} t \hat{\;} R K^{-1} p_1 = \frac{1}{s} t^T t \hat{\;} R K^{-1} p_1 = 0$ ，所以极点 $e_2$ 也在直线 $l$ 上。

因为 $p_2$ 和 $e_2$ 都在直线 $l$ 上，所以 $l$ 就是极线 $l_2$ 。因此右极线 $l_2$ 在坐标系 $O_2$ 中的方程

$l_2 = F p_1 \tag{3.4}$

同理，可以推出左极线 $l_1$ 在坐标系 $O_1$ 中的方程

$l_1 = p_2^T F \tag{3.5}$

从这两个公式可以看出，基础矩阵是从像素坐标到极线方程的变换，即，基础矩阵与一个坐标系中的像素坐标相乘，得到的是另一个坐标系中的极线方程。

3.3 分解本质矩阵

求得基础矩阵后，根据 $K^T \cdot F \cdot K$ 得到本质矩阵。首先介绍几个推导公式需要用到的特殊矩阵的性质：

本质矩阵有两个相等的非零奇异值和一个零奇异值。
旋转矩阵是正交矩阵，即 $\cdot M^T = E$ 。
实反对称矩阵满足 $M^T = -M$ ，且存在 n 阶正交矩阵 A，使得 $\cdot Z \cdot A^T$ 。其中

我们用 W 表示绕 Z 轴旋转 90° 的旋转矩阵，即

$\left[\begin{matrix} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 1 \end{matrix} \right]$

则

$\left\{ \begin{matrix} Z &= diag(1, 1, 0) \cdot W &= \left[\begin{matrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix} \right] \\ \\ -Z &= diag(1, 1, 0) \cdot W^T &= \left[\begin{matrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix} \right] \end{matrix}\right. \tag{3.6}$

假设 $\vec t$ 是平移向量。因为 $\vec t \hat{\;}$ 是反对称矩阵，根据性质 3 知 $\vec t \hat{\;} = U \cdot Z \cdot U^T$ ，于是本质矩阵

$\vec t \hat{\;} \cdot R = U \cdot Z \cdot U^T \cdot R \tag{3.7}$

对 E 进行奇异值分解

$\cdot \Lambda \cdot V^T \tag{3.8}$

为了让公式 (3.7) 与公式 (3.8) 中的本质矩阵尺度相等，我们对公式 (3.7) 两边同时乘以非零常数 $k$ 或 $- k$ ，其中 $\pm k$ 是公式 (3.8) 中 $\Lambda$ 内的非零奇异值。由公式 (3.6) 知 $\cdot Z = \Lambda \cdot W$ ， $\cdot Z = \Lambda \cdot W^T$ 。于是公式 (3.7) 可以写成

$\left\{\begin{matrix} U \cdot (k \cdot Z) \cdot U^T \cdot R = U \cdot \Lambda \cdot W \cdot U^T \cdot R \\ U \cdot (-k \cdot Z) \cdot U^T \cdot R = U \cdot \Lambda \cdot W^T \cdot U^T \cdot R \end{matrix}\right. \tag{3.9}$

由公式 (3.8) 和 (3.9) 可知

$\left\{\begin{matrix} W \cdot U^T \cdot R &= V^T \\ W^T \cdot U^T \cdot R &= V^T \end{matrix}\right.$

所以

$\left\{\begin{matrix} R &= U \cdot W^T \cdot V^T \\ R &= U \cdot W \cdot V^T \end{matrix}\right. \tag{3.10}$

因为 $\vec t \hat{\;} \cdot \vec t = 0$ ，所以由公式 (3.7) 知 $\cdot Z \cdot U^T \cdot \vec t = 0$ 。所以

$\left\{\begin{matrix} \frac{1}{k} \cdot U \cdot \Lambda \cdot W \cdot U^T \cdot \vec t &= \frac{1}{k} \cdot U \cdot \Lambda \cdot (U \cdot W^T)^T \cdot \vec t = 0 \\ \\ -\frac{1}{k} \cdot U \cdot \Lambda \cdot W^T \cdot U^T \cdot \vec t &= -\frac{1}{k} \cdot U \cdot \Lambda \cdot (U \cdot W)^T \cdot \vec t = 0 \end{matrix}\right. \tag{3.11}$

对于公式 (3.11) 的第一个式子，忽略尺度 k 得 $\cdot \Lambda \cdot (U \cdot W^T)^T \cdot \vec t = 0$ ，所以 $\vec t = (U \cdot W^T).col(2)$ 。而

$\cdot W^T = \left[\begin{matrix} u_0 & u_1 & u_2 \end{matrix} \right] \cdot \left[\begin{matrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{matrix} \right] = \left[\begin{matrix} -u_1 & u_0 & u_2 \end{matrix} \right]$

所以 $\vec t = u_2 = U.col(2)$ ，这是 $\vec t \hat{\;} \cdot \vec t = 0$ 的一个特解。使用公式 (3.11) 的第二个式子也能同样推导出这个结论。忽略尺度时， $\vec t \hat{\;} \cdot \vec t = 0$ 就有两个异号的解

$\vec t = \left\{\begin{matrix} U.col(2) \\ -U.col(2) \end{matrix}\right. \tag{3.12}$

由公式 (3.10) 和 (3.12) 可以得到四组解。下面是 ORB_SLAM2 中分解本质矩阵的代码：

void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t) {
    cv::Mat u, w, vt;
    cv::SVD::compute(E, w, u, vt);

    u.col(2).copyTo(t);
    t = t / cv::norm(t);

    cv::Mat W(3, 3, CV_32F, cv::Scalar(0));
    W.at<float>(0, 1) = -1;
    W.at<float>(1, 0) = 1;
    W.at<float>(2, 2) = 1;

    R1 = u * W * vt;
    if (cv::determinant(R1) < 0)
        R1 = -R1;

    R2 = u * W.t() * vt;
    if (cv::determinant(R2) < 0)
        R2 = -R2;
}

3.4 验证

由公式 (3.10) 和公式 (3.12) 可知，分解本质矩阵会得到两个 R 和两个 $\vec t$ ，共 4 个位姿。这 4 个位姿如图 3.1 所示，只有图 3.1 的 (1) 代表两个相机坐标系的深度值都为正。所以通过三角测量求得深度值就可以从 4 个位姿中选出唯一一个合理的位姿。

图 3.1 分解本质矩阵得到的 4 个解

下面是 ORB_SLAM2 中对每个 $\vec t)$ 的评价方法。

int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,
                         const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,
                         const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax) {
    // Calibration parameters
    const float fx = K.at<float>(0, 0);
    const float fy = K.at<float>(1, 1);
    const float cx = K.at<float>(0, 2);
    const float cy = K.at<float>(1, 2);

    vbGood = vector<bool>(vKeys1.size(), false);
    vP3D.resize(vKeys1.size());

    vector<float> vCosParallax;
    vCosParallax.reserve(vKeys1.size());

    // 相机 1 的投影矩阵。Camera 1 Projection Matrix K[I|0]
    cv::Mat P1(3, 4, CV_32F, cv::Scalar(0));
    K.copyTo(P1.rowRange(0, 3).colRange(0, 3));
    // 相机 2 的投影矩阵。Camera 2 Projection Matrix K[R|t]
    cv::Mat P2(3, 4, CV_32F);
    R.copyTo(P2.rowRange(0, 3).colRange(0, 3));
    t.copyTo(P2.rowRange(0, 3).col(3));
    P2 = K * P2;

    // 相机 1 坐标系的原点在相机 1 坐标系中的坐标。
    cv::Mat O1 = cv::Mat::zeros(3, 1, CV_32F);
    // 相机 2 坐标系的原点在相机 1 坐标系中的坐标。
    cv::Mat O2 = -R.t() * t;

    int nGood = 0;
    for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) {
        if (!vbMatchesInliers[i])
            continue;

        // 利用一对匹配的特征点进行三角测量，并验证得到的这个三维点的三个坐标值是否是 NaN 值。
        const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
        const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
        cv::Mat p3dC1;
        Triangulate(kp1, kp2, P1, P2, p3dC1);
        if (!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2))) {
            vbGood[vMatches12[i].first] = false;
            continue;
        }

        // 检查从两个相机坐标系原点到三维点的两个向量夹角的余弦值。Check parallax
        cv::Mat normal1 = p3dC1 - O1;
        float dist1 = cv::norm(normal1);
        cv::Mat normal2 = p3dC1 - O2;
        float dist2 = cv::norm(normal2);
        float cosParallax = normal1.dot(normal2) / (dist1 * dist2); // 余弦值。

        // 检查在相机坐标系 1 中的深度值。Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
        if (p3dC1.at<float>(2) <= 0 && cosParallax < 0.99998)
            continue;

        // 检查在相机坐标系 2 中的深度值。Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
        cv::Mat p3dC2 = R * p3dC1 + t;
        if (p3dC2.at<float>(2) <= 0 && cosParallax < 0.99998)
            continue;

        // 检查在像素坐标系 1 中的坐标误差。Check reprojection error in first image
        float im1x, im1y;
        float invZ1 = 1.0 / p3dC1.at<float>(2);
        im1x = fx * p3dC1.at<float>(0) * invZ1 + cx;
        im1y = fy * p3dC1.at<float>(1) * invZ1 + cy;
        float squareError1 = (im1x - kp1.pt.x) * (im1x - kp1.pt.x) + (im1y - kp1.pt.y) * (im1y - kp1.pt.y);
        if (squareError1 > th2)
            continue;

        // 检查在像素坐标系 2 中的坐标误差。Check reprojection error in second image
        float im2x, im2y;
        float invZ2 = 1.0 / p3dC2.at<float>(2);
        im2x = fx * p3dC2.at<float>(0) * invZ2 + cx;
        im2y = fy * p3dC2.at<float>(1) * invZ2 + cy;
        float squareError2 = (im2x - kp2.pt.x) * (im2x - kp2.pt.x) + (im2y - kp2.pt.y) * (im2y - kp2.pt.y);
        if (squareError2 > th2)
            continue;

        // 收集三角测量的结果。
        vCosParallax.push_back(cosParallax);
        vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0), p3dC1.at<float>(1), p3dC1.at<float>(2));
        nGood++;

        // 平移足够大(夹角足够大)时vbGood[i] = true。
        if (cosParallax < 0.99998)
            vbGood[vMatches12[i].first] = true;
    }

    if (nGood > 0) {
        // 余弦值要足够小(O1P 与 O2P 的夹角要足够大)。
        sort(vCosParallax.begin(), vCosParallax.end());
        // 如果三角测量的结果小于 50 个，只要求最大的夹角大于阈值。如果三角测量的结果大于 50 个，从第 51 个开始夹角都要大于阈值。
        size_t idx = min(50, int(vCosParallax.size() - 1));
        parallax = acos(vCosParallax[idx]) * 180 / CV_PI;
    }
    else
        parallax = 0;

    return nGood;
}

代码中要求余弦值小于 0.99998，其实就是要求位姿变换中的平移向量不为 0，因为平移向量为 0 时从单应矩阵恢复出的位姿更准确。该函数对 n 个匹配点进行三角测量，并用 nGood 记录三角测量成功的个数。三角测量成功的标准是：

测量得出的三维点 P(x, y, z) 的深度值 z 在两个坐标系中都要是正值且 $\overrightarrow {O_1P}$ 与 $\overrightarrow {O_2P}$ 的夹角要大于 0。
两个像素坐标系中的投影误差都要小于阈值。

四个位姿将会得到 4 个 nGood 和 4 个 $\overrightarrow {O_1P}$ 与 $\overrightarrow {O_2P}$ 夹角的角度。假设最大的 nGood 是 nGood1，对应的夹角是 $\alpha_1$ 。从该本质矩阵恢复出相机位姿成功的标准是：

nGood1 须明显大于其它三个，且大于一个设定的阈值。
$\alpha_1$ 须大于阈值。

4. 单应矩阵

如图 4.1，如果两台相机所拍摄的场景为同一个平面，则两台相机之间的关系称为共面点成像（Planar Homography）；如果两台相机拍摄的是同一个场景，但两台相机之间只有旋转角度的不同，没有任何位移，则这两台相机之间的关系称为单应（Homography）。

单应矩阵

图 4.1 单应矩阵适用的两种情况

这两种情况分别是物方点共面和平移向量为零，相比普通场景的约束性更强，因此应该使用自由度更少的单应矩阵。如果使用基础矩阵，多余的自由度将由噪声决定，误差会更大。从另一个角度来看，当平移向量为零时，由 $\vec t \hat{\quad} R$ 知本质矩阵和基础矩阵都为零，因此不能利用基础矩阵求解位姿。

4.1 DLT 求单应矩阵

和求基础矩阵一样，我们用 9 个未知数表示单应矩阵。假设单应矩阵 H 是从坐标系 1 到坐标系 2 的变换。根据公式 (1.8)，因为 $p_2 = H p_1$ ，所以 $p_2 \hat{\;} H p_1 = \vec 0$ ，即

$\left[\begin{matrix} 0 & -1 & v_2 \\ 1 & 0 & -u_2 \\ -v_2 & u_2 & 0 \end{matrix} \right] \cdot \left[\begin{matrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{matrix} \right] \cdot \left[\begin{matrix} u_1 \\ v_1 \\ 1 \end{matrix} \right] = \left[\begin{matrix} 0 \\ 0 \\ 0 \end{matrix} \right] \tag{4.1}$

展开上式得到三个等式

$\left\{\begin{matrix} 0 (h_1 u_1 + h_2 v_1 + h3) - 1 (h_4 u_1 + h_5 v_1 + h_6) + v_2 (h_7 u_1 + h_8 v_1 + h_9) &= 0 \\ 1 (h_1 u_1 + h_2 v_1 + h3) - 0 (h_4 u_1 + h_5 v_1 + h_6) - u_2 (h_7 u_1 + h_8 v_1 + h_9) &= 0 \\ -v_2 (h_1 u_1 + h_2 v_1 + h3) + u_2 (h_4 u_1 + h_5 v_1 + h_6) + 0 (h_7 u_1 + h_8 v_1 + h_9) &= 0 \end{matrix}\right. \tag{4.2}$

公式 (4.2) 中的三个式子中，任意一个式子可以用其它两个式子的线性组合得到，所以我们只取前两个式子。同时记 ${\bf \vec h} = [h_1, h_2, h_3, h_4, h_5, h_6, h_7, h_8, h_9]^T$ 。得到

$\left[\begin{matrix} 0 & 0 & 0 & -u_1 & -v_1 & -1 & v_2 u_1 & v_2 v_1 & v_2 \\ u_1 & v_1 & 1 & 0 & 0 & 0 & -u_2 u_1 & -u_2 v_1 & -u_2 \end{matrix} \right] \cdot \vec h = \vec 0 \tag{4.3}$

使用一对匹配点可以得到公式 (4.3) 所示的两个方程组。使用 8 对匹配点将得到 16 个方程组，求解这样的方程组就可以得到单应矩阵 H。下面是 ORB_SLAM2 中求解单应矩阵的代码。

cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) {
    const int N = vP1.size();    // N = 8。
    cv::Mat A(2 * N, 9, CV_32F); // A.shape = (16, 9)。

    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>(2 * i, 0) = 0.0;
        A.at<float>(2 * i, 1) = 0.0;
        A.at<float>(2 * i, 2) = 0.0;
        A.at<float>(2 * i, 3) = -u1;
        A.at<float>(2 * i, 4) = -v1;
        A.at<float>(2 * i, 5) = -1;
        A.at<float>(2 * i, 6) = v2 * u1;
        A.at<float>(2 * i, 7) = v2 * v1;
        A.at<float>(2 * i, 8) = v2;

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

    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); // shape = (3, 3)。
}

至此求得单应矩阵 H。如果使用了归一化，还要根据公式 (2.1) 对 H 解除归一化。

4.2 选择单应矩阵

我们在 $p_2 = H p_1$ 左侧乘个非零系数 k，使两边尺度相等，则有

$\left[\begin{matrix} u_2 \\ v_2 \\ 1 \end{matrix} \right] = \left[\begin{matrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{matrix} \right] \cdot \left[\begin{matrix} u_1 \\ v_1 \\ 1 \end{matrix} \right]$

把上式展开成三个等式

$\left\{\begin{aligned} h_1 u_1 + h_2 v_1 + h_3 &= k u_2 \\ h_4 u_1 + h_5 v_1 + h_6 &= k v_2 \\ h_7 u_1 + h_8 v_1 + h_9 &= k \end{aligned}\right.$

得到

$\left\{\begin{aligned} 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{aligned}\right. \tag{4.4}$

为单应矩阵打分时，使用公式 (4.4) 计算两个坐标系中的投影误差，并用误差的相反数作为单应矩阵的分数，这样的分数越大越好。ORB_SLAM2 中为单应矩阵打分的代码是

float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma) {
    const int N = mvMatches12.size();

    const float h11 = H21.at<float>(0, 0);
    const float h12 = H21.at<float>(0, 1);
    const float h13 = H21.at<float>(0, 2);
    const float h21 = H21.at<float>(1, 0);
    const float h22 = H21.at<float>(1, 1);
    const float h23 = H21.at<float>(1, 2);
    const float h31 = H21.at<float>(2, 0);
    const float h32 = H21.at<float>(2, 1);
    const float h33 = H21.at<float>(2, 2);

    const float h11inv = H12.at<float>(0, 0);
    const float h12inv = H12.at<float>(0, 1);
    const float h13inv = H12.at<float>(0, 2);
    const float h21inv = H12.at<float>(1, 0);
    const float h22inv = H12.at<float>(1, 1);
    const float h23inv = H12.at<float>(1, 2);
    const float h31inv = H12.at<float>(2, 0);
    const float h32inv = H12.at<float>(2, 1);
    const float h33inv = H12.at<float>(2, 2);

    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;

        // Reprojection error in first image: x2in1 = H12 * x2。参考《视觉 SLAM 十四讲》公式 (7.19)。
        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;
        // 误差：点 (u1, v1) 到点 (u2in1, v2in1) 的欧氏距离的平方。
        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; // score 越大越好。

        // Reprojection error in second image: x1in2 = H21 * x1。参考《视觉 SLAM 十四讲》公式 (7.19)。
        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;

        if (bIn)
            vbMatchesInliers[i] = true;
        else
            vbMatchesInliers[i] = false;
    }
    return score;
}

4.3 分解单应矩阵

下面是如何从公式 (1.9) 的矩阵 A 中分解出 $(\vec n, d, R, \vec t)$ 。对 A 奇异值分解

$\cdot \Lambda \cdot V^T \tag{4.5}$

于是

$\begin{aligned} \Lambda &= U^T \cdot A \cdot V = U^T \cdot (d \cdot R + \vec t \cdot \vec n^T) \cdot V \\ &= d \cdot (U^T \cdot R \cdot V) + (U^T \cdot \vec t) \cdot (V^T \cdot \vec n)^T \end{aligned} \tag{4.6}$

公式 (1.9) 的第一行和公式 (4.6) 的第二行具有相似的形式。由附录 7 可知， $\Lambda$ 可以分解成 $\Lambda = d' \cdot R' + \vec t' \cdot \vec n'^T$ 。用 $(\vec n, d, R, \vec t)$ 表示 $(\vec n', d', R', \vec t')$ ，求得后者即可得到前者。我们令

$\left\{\begin{aligned} d' &= d \cdot s \\ s &= |U^T| \cdot |V| \\ R' &= s \cdot U^T \cdot R \cdot V \\ \vec t' &= U^T \cdot \vec t \\ \vec n' &= V^T \cdot \vec n \end{aligned}\right. \tag{4.7}$

即

$\Lambda = (d \cdot |U^T| \cdot |V|) \cdot (|U^T| \cdot |V| \cdot U^T \cdot R \cdot V) + (U^T \cdot \vec t) \cdot (V^T \cdot \vec n)^T \tag{4.8}$

因为 $|U^T| = \pm 1$ ， $\pm 1$ ，所以在 $d^{'}$ 和 $R^{'}$ 中都加了 $s$ 。 $s^2 = 1$ 即保证了公式 (4.8) 与原式 (4.6) 等价，又保证了旋转矩阵 $R^{'}$ 的行列式为 1。然后按附录 7 求得 $(\vec n', d', R', \vec t')$ ，就可以根据公式 (4.7) 求得 $(\vec n, d, R, \vec t)$ 。下面是 ORB_SLAM2 中分解单应矩阵的代码：

bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
                               cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, 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++;

    // We recover 8 motion hypotheses using the method of Faugeras et al.
    // Motion and structure from motion in a piecewise planar environment.
    // International Journal of Pattern Recognition and Artificial Intelligence, 1988

    cv::Mat invK = K.inv();
    cv::Mat A = invK * H21 * K;

    cv::Mat U, w, Vt, V;
    cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);
    V = Vt.t();

    float s = cv::determinant(U) * cv::determinant(Vt);

    float d1 = w.at<float>(0);
    float d2 = w.at<float>(1);
    float d3 = w.at<float>(2);

    // 保证 d1 >= d2 >= d3。
    if (d1 / d2 < 1.00001 || d2 / d3 < 1.00001) {
        return false;
    }

    // 1. 计算出 8 个位姿和法向量。
    vector<cv::Mat> vR, vt, vn;
    vR.reserve(8);
    vt.reserve(8);
    vn.reserve(8);

    // n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
    float aux1 = sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3));
    float aux3 = sqrt((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3));
    float x1[] = {aux1, aux1, -aux1, -aux1};
    float x3[] = {aux3, -aux3, aux3, -aux3};

    // 1.1 计算出 4 个位姿。case d'=d2
    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};

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

    // 1.2 计算出 4 个位姿。case d'=-d2
    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};

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

    // 2. 根据三角测量结果计算分数，选择最高分对应的位姿和三角测量结果。
    int bestGood = 0;
    int secondBestGood = 0;
    int bestSolutionIdx = -1;
    float bestParallax = -1;
    vector<cv::Point3f> bestP3D;
    vector<bool> 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;
}

4.4 验证

验证的方法和分解本质矩阵相同。

5. 使用 OpenCV 求本质矩阵和单应矩阵并分解

下面使用 OpenCV，先求出本质矩阵和单应矩阵再求解位姿。OpenCV 计算的本质矩阵、单应矩阵、恢复出的位姿，都是从坐标系 1 到坐标系 2 的。

void E_matrix(const vector<Point2d> pts1, const vector<Point2d> pts2, const Mat& K) {
	Mat essential_matrix = cv::findEssentialMat(pts1, pts2, K, cv::RANSAC, 0.999, 1.0);
	Mat R, t;
	cv::recoverPose(essential_matrix, pts1, pts2, K, R, t);
	cout << "R:" << endl << R << endl;
	cout << "t:" << endl << t << endl << endl;
}

void H_matrix(const vector<Point2d> pts1, const vector<Point2d> pts2, const Mat& K) {
	Mat homography_matrix = cv::findHomography(pts1, pts2, cv::RANSAC);
	vector<Mat> R, t, n;
	cv::decomposeHomographyMat(homography_matrix, K, R, t, n);
	for (int i = 0; i < R.size(); i++) {
		cout << "R:" << endl << R[i] << endl;
		cout << "t:" << endl << t[i] << endl;
		cout << "n:" << endl << n[i] << endl << endl;
	}
}

在 OpenCV 中，函数 findEssentialMat 的匹配点对至少为 5。匹配点对等于 5 时，得出的本质矩阵是 $\times 3$ 的矩阵；匹配点对大于 5 时，得出的本质矩阵是 $\times 3$ 的矩阵。而函数 recoverPose 要求本质矩阵是 $\times 3$ 的矩阵，所以使用 OpenCV 通过本质矩阵求解位姿至少需要 6 对匹配点。函数 recoverPose 还通过三角化检查正深度，计算出唯一一个合理的解。

函数 findHomography 要求参数至少有 4 对点。函数 decomposeHomographyMat 会返回 4 组旋转矩阵、平移向量和法向量。

#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

int main() {
	Mat K = (Mat_<double>(3, 3) << 521.0, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
	// Rpts1 + t = pts2。
	vector<Point2d> pts1;
	pts1.push_back(cam2pix(Point3d(-4, 2, 1), K));
	pts1.push_back(cam2pix(Point3d(1, 2, 3), K));
	pts1.push_back(cam2pix(Point3d(1, 3, 2), K));
	pts1.push_back(cam2pix(Point3d(2, 1, 1), K));
	pts1.push_back(cam2pix(Point3d(-1, 4, 2), K));
	pts1.push_back(cam2pix(Point3d(7, 0, 3), K));
	vector<Point2d> pts2;
	pts2.push_back(cam2pix(Point3d(2, 3, 1), K));
	pts2.push_back(cam2pix(Point3d(2, -2, 3), K));
	pts2.push_back(cam2pix(Point3d(3, -2, 2), K));
	pts2.push_back(cam2pix(Point3d(1, -3, 1), K));
	pts2.push_back(cam2pix(Point3d(4, 0, 2), K));
	pts2.push_back(cam2pix(Point3d(0, -8, 3), K));
	// pose
	E_matrix(pts1, pts2, K);
	H_matrix(pts1, pts2, K);
}