讲解关于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=⎝
⎛x0y01⎠
⎞ p1=⎝
⎛x1y11⎠
⎞那么必然存在
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=p0TEp1=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−TEK1−1) uoTFv1=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=⎝
⎛xiyi1⎠
⎞ pi′=⎝
⎛xi′yi′1⎠
⎞那么带入
v
o
T
F
v
1
=
0
v_o^TFv_1=0
voTFv1=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]⎣
⎡f1f4f7f2f5f8f3f6f9⎦
⎤⎣
⎡xiyi1⎦
⎤=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′+f7b=f2∗x′+f5∗y′+f8c=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
[abc]⎣
⎡xiyi1⎦
⎤=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
f1xixi′+f2yixi′+f3xi′+f4xiyi′+f5yiyi′+f6yi′+f7xi+f8yi+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
[xixi′yixi′xi′xiyi′yiyi′yi′xiyi1]⎣
⎡f1f1f3f4f5f6f7f8f9⎦
⎤=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
⎣
⎡x1x1′x2x2′⋯⋯x7x7′x8x8′y1x1′y2x2′⋯⋯y7x7′y8x8′x1′x2′⋯⋯x7′x8′x1y1′x2y2′⋯⋯x7y7′x8y8′y1y1′y2y2′⋯⋯y7y7′y8y8′y1′y2′⋯⋯y7′y8′x1x2⋯⋯x7x8y1y2⋯⋯y7y811⋯⋯11⎦
⎤⎣
⎡f1f1f3f4f5f6f7f8f9⎦
⎤=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
关注
打赏
![]()
查看更多评论
- 史上最全slam从零开始-总目录
- (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解
- 目标检测00-00：mmdetection(Foveabox为例)-目录-史上最新无死角讲解
- 风格迁移1-00：Liquid Warping GAN(Impersonator)-目录-史上最新无死角讲解
- 姿态估计1-00：FSA-Net(头部姿态估算)-目录-史上最新无死角讲解
- 姿态估计0-00：DenseFusion(6D姿态估计)-目录-史上最新无死角讲解
- 行人检测0-00：LFFD-目录-史上最新无死角解读
- 风格迁移0-00：stylegan-目录-史上最全
- 人脸识别0-00：insightFace-目录-史上最全
- (01)ORB-SLAM2源码无死角解析-(65) BA优化(g2o)→闭环线程:Optimizer::OptimizeEssentialGraph→本质图优化
