SLAM ORB-SLAM2（21）基础矩阵的计算和评分

1. 前言

在 《SLAM ORB-SLAM2（20）查找基础矩阵》 中了解到 查找基础矩阵主要过程：

1. 特征点坐标归一化 Normalize

函数 Normalize 参考 《SLAM ORB-SLAM2（14）特征点坐标归一化》

2. 选择归一化之后的特征点
3. 八点法计算基础矩阵 ComputeF21
4. 评分并评优 CheckFundamental

现在来看看基础矩阵如何计算和评分

2. 基础矩阵

2.1. 对级约束

不过先来了解一下什么是对级约束
拿着相机分别在点 $O_1$ 和 $O_2$ 观测空间中一点 $P$
该点在两个视图平面上分别被投影到了点 $p_1$ 和 $p_2$

由于两次测量都是对同一个点 $P$ 的观测
所以 $O_1{p}_1$ 和 $O_2{p}_2$的延长线相交与点 $P$
即点 $P,O_1,O_2,p_1,p_2$ 都在一个平面上，这个平面被称为 极面 (epipolar plane)
连线 $O_1{O}_2$ 被称为 基线 (baseline)
基线分别与两个像平面相交与点 $e_1,e_2$ ，这两个点被称为 极点 (epipole)
极点与成像点 $p_1,p_2$ 的连线 $p_1{e}_1,p_2{e}_2$ 所在的直线 $l_1,l_2$ 被称为 极线 (epipolar line)

对级约束就是，在两个不同的位置上观测空间中同一个点，其成像一定在极线上

2.2. 推导

假设某个特征点 $P$
相对于参考帧相机坐标系的坐标为 ${\mathbf{X}}_1={\left[\begin{array}{ccc}{x}_1& y_1& z_1\end{array}\right]}^T$

成像点的齐次坐标为 ${\mathbf{x}}_1={\left[\begin{array}{ccc}{u}_1& v_1& 1\end{array}\right]}^T$

根据针孔相机模型：
$${\mathbf{x}}_1=\frac{1}{z}\mathbf{K}{\mathbf{X}}_1⟺\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\frac{1}{z}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$ 矩阵 $K$ 为相机的内参矩阵，记录了相机的 $xy$ 轴上的焦距 $f_x,f_y$ 和 光心坐标 $c_x,c_y$

此时，考虑相机的内参，将$X_1,X_2$投影到成像平面上有：
$$\{\begin{array}{l}{\mathbf{x}}_1=\frac{1}{z_1}\mathbf{K}{\mathbf{X}}_1\\ {\mathbf{x}}_2=\frac{1}{z_2}\mathbf{K}{\mathbf{X}}_2\end{array}\Rightarrow \{\begin{array}{l}{\mathbf{X}}_1=z_1{\mathbf{K}}^{-1}{\mathbf{x}}_1\\ {\mathbf{X}}_2=z_2{\mathbf{K}}^{-1}{\mathbf{x}}_2\end{array}$$
相机经过位姿变换$⟨R,t⟩$后，观测到 $P$ 点坐标为，上式导入运动关系：
$${\mathbf{X}}_2=\mathbf{R}{\mathbf{X}}_1+\mathbf{t}\Rightarrow z_2{\mathbf{K}}^{-1}{\mathbf{x}}_2=z_1\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{x}}_1+\mathbf{t}$$
两边叉乘 $t$
$$z_2{\mathbf{t}}_{\times }{\mathbf{K}}^{-1}{\mathbf{x}}_2=z_1\mathbf{t}\times \mathbf{R}{\mathbf{K}}^{-1}{\mathbf{x}}_1$$

叉乘（外积）： A X B = |A| |B| $sin\theta$， 向量形成的平行四边形的面积，也是法向量

$t=\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]\Rightarrow t^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$

反对称矩阵，它的主对角线上的元素全为0，而位于主对角线两侧对称的元素反号

A X B = $\left[\begin{array}{ccc}0& -z_a& y_a\\ z_a& 0& -x_a\\ -y_a& x_a& 0\end{array}\right]$$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]$=$\left[\begin{array}{c}{y}_a{z}_b-y_b{z}_a\\ x_b{z}_a-x_a{z}_b\\ x_a{y}_b-x_b{y}_a\end{array}\right]$

那么两个平行的向量叉乘为0

两边点乘 ${\left({\mathbf{K}}^{-1}{\mathbf{x}}_2\right)}^T$
$$z_2{\left({\mathbf{K}}^{-1}{\mathbf{x}}_2\right)}^T\mathbf{t}\times {\mathbf{K}}^{-1}{\mathbf{x}}_2=z_1{\left({\mathbf{K}}^{-1}{\mathbf{x}}_2\right)}^T\mathbf{t}\times \mathbf{R}{\mathbf{K}}^{-1}{\mathbf{x}}_1$$
关注式子的左侧的左边，${\mathbf{t}}_{\times }{\mathbf{K}}^{-1}{\mathbf{x}}_2$的含义就是向量 $t$ 与 ${\mathbf{K}}^{-1}{\mathbf{x}}_2$ 的叉乘
将得到一个同时与这两个向量垂直的向量，该向量与 ${\mathbf{K}}^{-1}{\mathbf{x}}_2$ 的点积为零
所以上式可以写成如下的形式：
$$z_1{\left({\mathbf{K}}^{-1}{\mathbf{x}}_2\right)}^T\mathbf{t}\times \mathbf{R}{\mathbf{K}}^{-1}{\mathbf{x}}_1=0$$

上式就是一个等式为零的约束，所以其中$z$的取值实际上没有什么作用，只要非零就行，那么
$${{\mathbf{x}}_2}^T{\mathbf{K}}^{-T}\mathbf{t}\times \mathbf{R}{\mathbf{K}}^{-1}{\mathbf{x}}_1=0$$

把矩阵 ${\mathbf{K}}^{-T}{\mathbf{t}}_{\times }\mathbf{R}{\mathbf{K}}^{-1}$ 称为 基础矩阵，用符号 $\mathbf{F}$ 表示，那么得到映射关系：
$${{\mathbf{x}}_2}^T\mathbf{F}\mathbf{x}=0$$

2.3. 计算原理

根据两帧图像之间的映射关系：${{\mathbf{x}}_2}^T\mathbf{F}\mathbf{x}=0$
代入求解：
$$\begin{gathered} \left[\begin{array}{ccc}{u}_2& v_2& 1\end{array}\right]\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0\Rightarrow \\ {u}_2{u}_1{f}_{11}+u_2{v}_1{f}_{12}+u_2{f}_{13}+v_2{u}_1{f}_{21}+v_2{v}_1{f}_{22}+v_2{f}_{23}+u_1{f}_{31}+v_1{f}_{32}+f_{33}=0 \end{gathered}$$

记 $\mathbf{f}={\left[\begin{array}{ccccccccc}{f}_{11}& f_{12}& f_{13}& f_{21}& f_{22}& f_{23}& f_{31}& f_{32}& f_{33}\end{array}\right]}^T$ ，上式可以写成向量点乘的形式：
$$\left[\begin{array}{ccccccccc}{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{array}\right]\mathbf{f}=0$$

假有 $m$ 对匹配点，根据上式可以写出 $m$ 个约束，可以写成 $\mathbf{A}\mathbf{f}=0$ 的矩阵形式，如下：
$$\mathbf{A}\mathbf{f}=\left[\begin{array}{ccccccccc}{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_{m,2}{u}_{m,1}& u_{m,2}{v}_{m,1}& u_{m,2}& v_{m,2}{u}_{m,1}& v_{m,2}{v}_{m,1}& v_{m,2}& u_{m,1}& v_{m,1}& 1\end{array}\right]\mathbf{f}=0$$
对矩阵 $A$ 进行奇异值分解，得到的 右奇异矩阵的最后一列 就是 $\mathbf{f}$ 的 最优解
能够最小化 $\Vert \mathbf{A}\mathbf{f}\Vert /\Vert \mathbf{f}\Vert$的解，使 $\mathbf{A}\mathbf{f}$ 尽可能的接近 0

为何右奇异矩阵的最后一列为单应矩阵的最优解 参考 《SLAM ORB-SLAM2（16）奇异值分解》 的 求解超定方程

至少需要8个点才能求得基础矩阵，这也就是所谓的八点法

3. ComputeF21

用 直接线性法 DLT 方法求解 基础矩阵 $F$

按上述公式填充数据于系数矩阵 $A$

/**
 * @brief 基础矩阵计算求解函数
 * @param vP1  参考帧归一化坐标
 * @param vP2  当前帧归一化坐标
 * @return cv::Mat 计算的基础矩阵F
 */
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
{
    /* 获取坐标数量 */
    const int N = vP1.size();

    /* 定义系数矩阵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;
    }

    /* 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt为右正交矩阵V的转置T */
    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);

    /* 对初步得来的基础矩阵进行第2次奇异值分解 */
    cv::SVDecomp(Fpre, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

    /* 基础矩阵的秩为2，强制将第3个奇异值设置为0 */
    w.at<float>(2) = 0;

    /* 返回重组满足秩约束的基础矩阵 */
    return u * cv::Mat::diag(w) * vt;
}

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

秩：矩阵$A$ 的 非零特征值 的 个数
奇异值分解 参考 《SLAM ORB-SLAM2（16）奇异值分解》

4. CheckFundamental

经过计算，得到基础矩阵，那么根据基础矩阵计算重投影误差

这里返回的单应矩阵是归一化之后的，还不能直接使用， 需要恢复归一化之前的矩阵才行
在 《SLAM ORB-SLAM2（20）查找基础矩阵》 的 5. 八点法计算基础矩阵 中已经恢复了：

cv::Mat Fn = ComputeF21(vPn1i, vPn2i); /* 八点法计算基础矩阵 */
F21i = T2t * Fn * T1;                  /* 根据归一化的矩阵恢复 基础矩阵 F21 */

在第一步已经进行特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2

又因为 $x$₂^T$F$₂₁$x$₁=0
根据基础矩阵约束得到：(T2 * mvKeys2)^T * Fn * T1 * mvKeys1 = 0
进一步得到 : mvKeys2^T * T2t * Fn * T1 * mvKeys1 = 0

/**
 * @brief 基础矩阵评分函数
 * @param F21  参考帧到当前帧的基础矩阵
 * @param vbMatchesInliers  特征点对的Inlier标记
 * @param sigma  标准差
 * @return score 得分
 */
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);

获取从参考帧到当前帧的基础矩阵中的各个元素

    /* 预分配特征点对Inliers标记的空间 */
    vbMatchesInliers.resize(N);

    /* 初始化单应矩阵得分 */
    float score = 0;

    /* 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
       自由度为1的卡方分布在显著性水平为0.05时对应的临界阈值为3.841
       自由度为2的卡方分布在显著性水平为0.05时对应的临界阈值为5.991 */
    const float th = 3.841;
    const float thScore = 5.991;

    /* 方差平方的倒数，作为信息矩阵 协方差的逆 */
    const float invSigmaSquare = 1.0 / (sigma * sigma);

根据传入的sigma值计算协方差矩阵

    /* 通过F矩阵，进行参考帧和当前帧之间的双向投影，并计算出加权重投影误差 */
    for (int i = 0; i < N; i++)
    {
        /* 开始都默认为Inlier */
        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)
        /* 计算 参考帧 上的点在 当前帧 上投影得到的极线 l2 = F21 * p1 = (a2, b2, c2) */
        const float a2 = f11 * u1 + f12 * v1 + f13;
        const float b2 = f21 * u1 + f22 * v1 + f23;
        const float c2 = f31 * u1 + f32 * v1 + f33;

        /* 计算 当前帧的特征点 到 投影得到的极线 l2 的距离平方 d^2 = ((ax+by+c) / sqrt(a^2 + b^2))^2 */
        const float num2 = a2 * u2 + b2 * v2 + c2;
        const float squareDist1 = num2 * num2 / (a2 * a2 + b2 * b2);

        /* 转换参考帧投影至当前帧的误差 */
        const float chiSquare1 = squareDist1 * invSigmaSquare;

        /* 阀值用th，而累加得分用thScore，确保与单应基础评分标准一致 */
        /* 用阈值标记离群点，内点的话累加得分 */
        if (chiSquare1 > th)
            bIn = false;
        else
            /* 累计得分。误差越大，得分越低 */
            score += thScore - chiSquare1;

        // Reprojection error in second image
        // l1 =x2tF21=(a1,b1,c1)
        /* 计算 当前帧 上的点在 参考帧 上投影得到的极线 l1 = p2 * F21 = (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;

        /* 计算 参考帧的特征点 到 投影得到的极线 l1 的距离平方 d^2 = ((ax+by+c) / sqrt(a^2 + b^2))^2 */
        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;
}

根据对极约束的几何意义，它能够将一幅图像中的某个点投影到另一幅图像的极线上

对每个匹配的特征点，计算双向投影误差（特征点 到 投影得到的极线 的距离 $d=$ $\frac{ax+by+c}{\sqrt{a^2+b^2}}$ 的 平方），并获取对应得分和更新内点标记，具体如下：文章来源地址https://www.toymoban.com/news/detail-837561.html

1. 获取匹配点的像素坐标
2. 计算参考帧上的点 $x_1$ 在当前帧上投影得到的极线 $l_2$，再计算参考帧到当前帧的带权重的重投影误差，同时累积当前得分并进行内点判断
3. 计算当前帧上的点 $x_2$ 在参考帧上投影得到的极线 $l_1$，再计算当前帧到参考帧的带权重的重投影误差，同时累积当前得分并进行内点判断
4. 记录当前匹配点的内点标记

到了这里，关于SLAM ORB-SLAM2（21）基础矩阵的计算和评分的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！