(01)ORB-SLAM2源码无死角解析-(17) 单目初始化Initializer→八点发求解Fundamental矩阵

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

在上一篇博客中，我们讲解了 Homography 矩阵的求解过程，该篇博客主要对Fundamental 矩阵进行分析，再讲解之前，请大家详细阅读该篇博客 史上最简SLAM零基础解读(2) - 对极约束→Essential矩阵、Fundamental矩阵推导 通过该篇博客，我们可以了解到如果世界坐标 P P P 再两个相机成像平面 I 0 I_0 I0​, I 1 I_1 I1​，图像坐标分别为 p 0 = ( x 0 y 0 1 )             p 1 = ( x 1 y 1 1 ) \color{blue} p_0=\begin{pmatrix} x_0\\ y_0\\ 1\\ \end{pmatrix} ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~p_1=\begin{pmatrix} x_1\\ y_1\\ 1\\ \end{pmatrix} p0​=⎝ ⎛​x0​y0​1​⎠ ⎞​           p1​=⎝ ⎛​x1​y1​1​⎠ ⎞​那么必然存在 p 0 ⋅ E p 1 = p 0 T E p 1 = 0 \color{blue}p_0 \cdot Ep_1=p_0^T Ep_1=0 p0​⋅Ep1​=p0T​Ep1​=0 其中 E = t × R = t ∧ R \color{blue} E=t\times R=t ^{\wedge} R E=t×R=t∧R上式 t t t 是光心 O 1 O_1 O1​ 相对于 O 0 O_0 O0​ 的平移 , E E E 我们称为本质或者本征矩阵(Essential)， E p 1 Ep1 Ep1 为极线方程。 总的来说就是平面 I 0 I_0 I0​ 中的任意一点，一定在平面 I 1 I_1 I1​ 的极线 E p 1 Ep_1 Ep1​上。另外我们还进一步推导出来基本矩阵： F = ( K 0 − T E K 1 − 1 )            u o T F v 1 = 0 \color{blue} F=(K_0^{- T} EK_1^{-1}) ~~~~~~~~~~ \color{blue} u_o^TFv_1=0 F=(K0−T​EK1−1​)          uoT​Fv1​=0其上的 注意，其上的 p 0 , p 1 p_0,p_1 p0​,p1​ 是图像坐标， v 0 , v 1 v_0,v_1 v0​,v1​ 是像素坐标。我们源码中的特征点肯定是像素坐标，所以我们需要求解基本矩阵。但是上述公式中其为一对特征点建立的 欠定方程。我们需要需要把对点对齐进行约束，这里假设特征点对(像素坐标)如下: p i = ( x i y i 1 )             p i ′ = ( x i ′ y i ′ 1 ) \color{blue} p_{i}=\begin{pmatrix} x_{i}\\ y_{i}\\ 1\\ \end{pmatrix} ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~p'_{i}=\begin{pmatrix} x'_{i}\\ y'_{i}\\ 1\\ \end{pmatrix} pi​=⎝ ⎛​xi​yi​1​⎠ ⎞​           pi′​=⎝ ⎛​xi′​yi′​1​⎠ ⎞​那么带入 v o T F v 1 = 0 v_o^TFv_1=0 voT​Fv1​=0 ，令 v 0 = p i ′ , v 1 = p i v_0=p'_i, v_1=p_i v0​=pi′​,v1​=pi​，然后展开如下： [ x i ′ y i ′ 1 ] [ f 1 f 2 f 3 f 4 f 5 f 6 f 7 f 8 f 9 ] [ x i y i 1 ] = 0 \color{blue} \left[\begin{array}{lll} x'_{i} & y'_{i} & 1 \end{array}\right]\left[\begin{array}{lll} f_{1} & f_{2} & f_{3} \\ f_{4} & f_{5} & f_{6} \\ f_{7} & f_{8} & f_{9} \end{array}\right]\left[\begin{array}{c} x_{i} \\ y_{i} \\ 1 \end{array}\right]=0 [xi′​​yi′​​1​]⎣ ⎡​f1​f4​f7​​f2​f5​f8​​f3​f6​f9​​⎦ ⎤​⎣ ⎡​xi​yi​1​⎦ ⎤​=0为了方便得 a = f 1 ∗ x ′ + f 4 ∗ y ′ + f 7 b = f 2 ∗ x ′ + f 5 ∗ y ′ + f 8 c = f 3 ∗ x ′ + f 6 ∗ y ′ + f 9 \color{blue} \begin{array}{l} a=f_{1} * x'+f_{4} * y'+f_{7} \\ b=f_{2} * x'+f_{5} * y'+f_{8} \\ c=f_{3} * x'+f_{6} * y'+f_{9} \\ \end{array} a=f1​∗x′+f4​∗y′+f7​b=f2​∗x′+f5​∗y′+f8​c=f3​∗x′+f6​∗y′+f9​​那么上面的矩阵可以转换为： [ a b c ] [ x i y i 1 ] = 0 \color{blue} \left[\begin{array}{lll} a & b & c \end{array}\right]\left[\begin{array}{c} x_{i} \\ y_{i} \\ 1 \end{array}\right]=0 [a​b​c​]⎣ ⎡​xi​yi​1​⎦ ⎤​=0

f 1 x i x i ′ + f 2 y i x i ′ + f 3 x i ′ + f 4 x i y i ′ + f 5 y i y i ′ + f 6 y i ′ + f 7 x i + f 8 y i + f 9 = 0 \color{blue} f_{1}x_{i}x_{i}'+f_{2}y_{i}x_{i}'+f_{3}x_{i}'+f_{4}x_{i}y_{i}'+f_{5}y_{i}y_{i}'+f_{6}y_{i}'+f_{7}x_{i}+f_{8}y_{i}+f_{9}=0 f1​xi​xi′​+f2​yi​xi′​+f3​xi′​+f4​xi​yi′​+f5​yi​yi′​+f6​yi′​+f7​xi​+f8​yi​+f9​=0 转化为矩阵形式： [ x i x i ′ y i x i ′ x i ′ x i y i ′ y i y i ′ y i ′ x i y i 1 ] [ f 1 f 1 f 3 f 4 f 5 f 6 f 7 f 8 f 9 ] = 0 \color{blue} \left[\begin{array}{ccccccccc} x_{i}x_{i}' & y_{i}x_{i}' & x_{i}'& x_{i}y_{i}' & y_{i}y_{i}' & y_{i}' & x_{i} & y_{i} & 1\\ \end{array}\right]\left[\begin{array}{l} f_{1} \\ f_{1} \\ f_{3} \\ f_{4} \\ f_{5} \\ f_{6} \\ f_{7} \\ f_{8} \\ f_{9} \end{array}\right]=0 [xi​xi′​​yi​xi′​​xi′​​xi​yi′​​yi​yi′​​yi′​​xi​​yi​​1​]⎣ ⎡​f1​f1​f3​f4​f5​f6​f7​f8​f9​​⎦ ⎤​=0 其上为任意一点对组成的方程，如果写成八个点对的公式图下： [ x 1 x 1 ′ y 1 x 1 ′ x 1 ′ x 1 y 1 ′ y 1 y 1 ′ y 1 ′ x 1 y 1 1 x 2 x 2 ′ y 2 x 2 ′ x 2 ′ x 2 y 2 ′ y 2 y 2 ′ y 2 ′ x 2 y 2 1 ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ x 7 x 7 ′ y 7 x 7 ′ x 7 ′ x 7 y 7 ′ y 7 y 7 ′ y 7 ′ x 7 y 7 1 x 8 x 8 ′ y 8 x 8 ′ x 8 ′ x 8 y 8 ′ y 8 y 8 ′ y 8 ′ x 8 y 8 1 ] [ f 1 f 1 f 3 f 4 f 5 f 6 f 7 f 8 f 9 ] = 0 \color{blue} \left[\begin{array}{ccccccccc} x_{1}x_{1}' & y_{1}x_{1}' & x_{1}'& x_{1}y_{1}' & y_{1}y_{1}' & y_{1}' & x_{1} & y_{1} & 1\\ x_{2}x_{2}' & y_{2}x_{2}' & x_{2}'& x_{2}y_{2}' & y_{2}y_{2}' & y_{2}' & x_{2} & y_{2} & 1\\ \cdots & \cdots & \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \\ \cdots & \cdots & \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \\ x_{7}x_{7}' & y_{7}x_{7}' & x_{7}'& x_{7}y_{7}' & y_{7}y_{7}' & y_{7}' & x_{7} & y_{7} & 1\\ x_{8}x_{8}' & y_{8}x_{8}' & x_{8}'& x_{8}y_{8}' & y_{8}y_{8}' & y_{8}' & x_{8} & y_{8} & 1\\ \end{array}\right]\left[\begin{array}{l} f_{1} \\ f_{1} \\ f_{3} \\ f_{4} \\ f_{5} \\ f_{6} \\ f_{7} \\ f_{8} \\ f_{9} \end{array}\right]=0 ⎣ ⎡​x1​x1′​x2​x2′​⋯⋯x7​x7′​x8​x8′​​y1​x1′​y2​x2′​⋯⋯y7​x7′​y8​x8′​​x1′​x2′​⋯⋯x7′​x8′​​x1​y1′​x2​y2′​⋯⋯x7​y7′​x8​y8′​​y1​y1′​y2​y2′​⋯⋯y7​y7′​y8​y8′​​y1′​y2′​⋯⋯y7′​y8′​​x1​x2​⋯⋯x7​x8​​y1​y2​⋯⋯y7​y8​​11⋯⋯11​​⎦ ⎤​⎣ ⎡​f1​f1​f3​f4​f5​f6​f7​f8​f9​​⎦ ⎤​=0

 

二、代码流程

代码的主要逻辑如下：

 * Step 1 将当前帧和参考帧中的特征点坐标进行归一化
 * Step 2 选择8个归一化之后的点对进行迭代
 * Step 3 八点法计算基础矩阵矩阵
 * Step 4 利用重投影误差为当次RANSAC的结果评分
 * Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记

其与上一篇博客求解 Homography 矩阵的流程基本一致，所以这里就不再进行重复了。

 

三、源码注释

主调函数为 FindFundamental函数，其内部核心函数为:

Normalize()  归一化操作
ComputeF21() 八点法计算计算F矩阵
CheckFundamental() 重投影误差评分

C o m p u t e F 21 ( ) 与 C h e c k F u n d a m e n t a l ( ) 在后面的博客有详细的介绍 \color{red}ComputeF21() 与 CheckFundamental() 在后面的博客有详细的介绍 ComputeF21()与CheckFundamental()在后面的博客有详细的介绍 ，所以大家这里随便看一下，不用太在意 。

void Initializer::FindFundamental(） 函数具体实现如下：

/**
 * @brief 归一化特征点到同一尺度，作为后续normalize DLT的输入
 *  [x' y' 1]' = T * [x y 1]' 
 *  归一化后x', y'的均值为0，sum(abs(x_i'-0))=1，sum(abs((y_i'-0))=1
 *
 *  为什么要归一化？
 *  在相似变换之后(点在不同的坐标系下),他们的单应性矩阵是不相同的
 *  如果图像存在噪声,使得点的坐标发生了变化,那么它的单应性矩阵也会发生变化
 *  我们采取的方法是将点的坐标放到同一坐标系下,并将缩放尺度也进行统一 
 *  对同一幅图像的坐标进行相同的变换,不同图像进行不同变换
 *  缩放尺度是为了让噪声对于图像的影响在一个数量级上
 * 
 *  Step 1 计算特征点X,Y坐标的均值 
 *  Step 2 计算特征点X,Y坐标离均值的平均偏离程度
 *  Step 3 将x坐标和y坐标分别进行尺度归一化，使得x坐标和y坐标的一阶绝对矩分别为1 
 *  Step 4 计算归一化矩阵：其实就是前面做的操作用矩阵变换来表示而已
 * 
 * @param[in] vKeys                               待归一化的特征点
 * @param[in & out] vNormalizedPoints             特征点归一化后的坐标
 * @param[in & out] T                             归一化特征点的变换矩阵
 */
void Initializer::Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T)                           //将特征点归一化的矩阵
{
    // 归一化的是这些点在x方向和在y方向上的一阶绝对矩（随机变量的期望）。

    // Step 1 计算特征点X,Y坐标的均值 meanX, meanY
    float meanX = 0;
    float meanY = 0;

	//获取特征点的数量
    const int N = vKeys.size();

	//设置用来存储归一后特征点的向量大小，和归一化前保持一致
    vNormalizedPoints.resize(N);

	//开始遍历所有的特征点
    for(int i=0; i