讲解关于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(基本)矩阵，然后再讲解如何利用他们来求解 相机位姿 \color{red}{相机位姿} 相机位姿。 那么我们首先来看看什么是 Homography矩阵。
二、Homography 作用
首先我们来说一下 Homography 矩阵的作用，最简单的作用就是投影变换，如使用相机拍摄如下两幅图像：
上图中的红点标明了左右图中对应的点，Homography 就是将一幅图中的点映射到另一幅图中它的对应点的映射。也就是如果我们知道了 左图 到 右图 的 Homography 矩阵那么我们就可以把 左图 的任意像素坐标映射到 右图。
那么反过来，已知两图中对应的坐标位置，如 左图 中 红色标记 ( x 1 , y 1 ) (x_1,y_1) (x1,y1) 与 右图 中的 红色标记 ( x 2 , y 2 ) (x_2,y_2) (x2,y2) 为一组已知的对应坐标，存在多组对应时，我们是否可以求解出相应的 Homography 矩阵。答案当然是可以的，也就是本博客需要讲解的内容。
对于 H o m o g r a p h y 矩阵的由来 \color{red}{Homography 矩阵的由来} Homography矩阵的由来，大家可以参考一下本人编写的该篇博客:史上最简SLAM零基础解读(4) - 单应性Homography →公式推导与细节理解
三、公式推导
矩阵的一个重要作用是将空间中的点变换到另一个空间中。这个作用在国内的《线性代数》教学中基本没有介绍。要能形像地理解这一作用，比较直观的方法就是图像变换，图像变换的方法很多，单应性变换是其中一种方法，单应性变换会涉及到单应性矩阵。单应性变换的目标是通过给定的几个点（通常是4对点）来得到单应性矩阵。单应性矩阵为：
H
=
[
h
11
h
12
h
13
h
21
h
22
h
23
h
31
h
32
h
33
]
\color{blue} H=\left[\begin{array}{lll} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{array}\right]
H=⎣
⎡h11h21h31h12h22h32h13h23h33⎦
⎤
上面的矩阵H会将一幅图像上的一个点的坐标
a
=
(
x
,
y
,
1
)
a=(x,y,1)
a=(x,y,1) 映射成另一幅图像上的点的坐标
b
=
(
x
1
,
y
1
,
1
)
b=(x_1,y_1,1)
b=(x1,y1,1)，但
H
H
H是未知的。通常需要根据在同一平面上已知的一些点对(比如
a
,
b
a,b
a,b)来求
H
H
H。 假设已知点对
(
a
,
b
)
(a,b)
(a,b)，则有下面的公式：
b
=
H
a
T
(1)
\tag {1} \color{blue} b=Ha^T
b=HaT(1)带入
H
H
H得：
b
=
H
a
T
=
[
h
11
h
12
h
13
h
21
h
22
h
23
h
31
h
32
h
33
]
[
x
y
1
]
=
(
x
1
,
y
1
,
1
)
(2)
\tag {2} \color{blue}b=Ha^T=\left[\begin{array}{lll} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{array}\right]\left[\begin{array}{l} x\\ y \\ 1 \end{array}\right] =(x_1,y_1,1)
b=HaT=⎣
⎡h11h21h31h12h22h32h13h23h33⎦
⎤⎣
⎡xy1⎦
⎤=(x1,y1,1)(2)
进而可以得到：
{
x
1
=
h
11
x
+
h
12
y
+
h
13
y
1
=
h
21
x
+
h
22
y
+
h
23
1
=
h
31
x
+
h
32
y
+
h
33
(3)
\tag {3} \color{blue}\left\{\begin{aligned} x_{1} &=h_{11} x+h_{12} y+h_{13} \\ y_{1} &=h_{21} x+h_{22} y+h_{23} \\ 1 &=h_{31} x+h_{32} y+h_{33} \end{aligned}\right.
⎩
⎨
⎧x1y11=h11x+h12y+h13=h21x+h22y+h23=h31x+h32y+h33(3)由上面这个公式中的
1
=
h
31
x
+
h
32
y
+
h
33
1=h31x+h32y+h33
1=h31x+h32y+h33 可得到下面两个等式
{
x
1
=
x
1
1
=
h
11
x
+
h
12
y
+
h
13
h
31
x
+
h
32
y
+
h
33
y
1
=
y
1
1
=
h
21
x
+
h
22
y
+
h
23
h
31
x
+
h
32
y
+
h
33
(4)
\tag {4} \color{blue} \left\{\begin{aligned} x_{1}=\frac{x_{1}}{1}=\frac{h_{11} x+h_{12} y+h_{13}}{h_{31} x+h_{32} y+h_{33}} \\ y_{1}=\frac{y_{1}}{1}=\frac{h_{21} x+h_{22} y+h_{23}}{h_{31} x+h_{32} y+h_{33}} \end{aligned}\right.
⎩
⎨
⎧x1=1x1=h31x+h32y+h33h11x+h12y+h13y1=1y1=h31x+h32y+h33h21x+h22y+h23(4)
⇒
\Rightarrow
⇒
{
h
11
x
+
h
12
y
+
h
13
=
h
31
x
x
1
+
h
32
y
x
1
+
h
33
x
1
h
21
x
+
h
22
y
+
h
23
=
h
31
x
y
1
+
h
32
y
y
1
+
h
33
y
1
(5)
\tag {5} \color{blue}\left\{\begin{aligned} h_{11} x+h_{12} y+h_{13}=h 31 x x_{1}+h_{32} y x_{1}+h_{33} x_{1} \\ \\ h_{21} x+h_{22} y+h_{23}=h 31 x y_{1}+h_{32} y y_{1}+h_{33} y_{1} \end{aligned}\right.
⎩
⎨
⎧h11x+h12y+h13=h31xx1+h32yx1+h33x1h21x+h22y+h23=h31xy1+h32yy1+h33y1(5)
⇒
\Rightarrow
⇒
{
0
=
h
31
x
x
1
+
h
32
y
x
1
+
h
33
x
1
−
(
h
11
x
+
h
12
y
+
h
13
)
0
=
h
31
x
y
1
+
h
32
y
y
1
+
h
33
y
1
−
(
h
21
x
+
h
22
y
+
h
23
)
(6)
\tag {6} \color{blue} \left\{\begin{array}{r} 0=h_{31} x x_{1}+h_{32} y x_{1}+h_{33} x_{1}-\left(h_{11} x+h_{12} y+h_{13}\right) \\ \\ 0=h_{31} x y_{1}+h_{32} y y_{1}+h_{33} y_{1}-\left(h_{21} x+h_{22} y+h_{23}\right) \end{array}\right.
⎩
⎨
⎧0=h31xx1+h32yx1+h33x1−(h11x+h12y+h13)0=h31xy1+h32yy1+h33y1−(h21x+h22y+h23)(6)对于方程(1)可写成一个矩阵与一个向量相乘，即：
[
−
x
−
y
−
1
0
0
0
x
x
1
y
x
1
x
1
0
0
0
−
x
−
y
−
1
x
y
1
y
y
1
y
1
]
h
=
0
(7)
\tag {7} \color{blue} \left[ \begin{array}{ccccccccc} -x & -y & -1 & 0 & 0 & 0 & x x_{1} & y x_{1} & x_{1} \\ \\ 0 & 0 & 0 & -x & -y & -1 & x y_{1} & y y_{1} & y_{1} \end{array}\right ] h=0
⎣
⎡−x0−y0−100−x0−y0−1xx1xy1yx1yy1x1y1⎦
⎤h=0(7)其中，
h
=
[
h
11
,
h
12
,
h
13
,
h
21
,
h
22
,
h
23
,
h
31
,
h
32
,
h
33
]
T
h=[h11,h12,h13,h21,h22,h23,h31,h32,h33]^T
h=[h11,h12,h13,h21,h22,h23,h31,h32,h33]T，是一个9维的列向量。若令:
A
=
[
−
x
−
y
−
1
0
0
0
x
x
1
y
x
1
x
1
0
0
0
−
x
−
y
−
1
x
y
1
y
y
1
y
1
]
(8)
\tag {8} \color{blue} A=\left[\begin{array}{ccccccccc} -x & -y & -1 & 0 & 0 & 0 & x x_{1} & y x_{1} & x_{1} \\ \\ 0 & 0 & 0 & -x & -y & -1 & x y_{1} & y y_{1} & y_{1} \end{array}\right]
A=⎣
⎡−x0−y0−100−x0−y0−1xx1xy1yx1yy1x1y1⎦
⎤(8)则(7)可以记为
A
h
=
0
(9)
\tag {9} \color{blue} {Ah=0}
Ah=0(9)
这里的
A
∈
R
2
×
9
A∈R^{2×9}
A∈R2×9。这只是1对点所得到的矩阵A。究竟要多少点对才能求出
H
H
H?,由于我们是采用齐次坐标（即(x,y,1)）来表示平面上的点，所以存在一个非零的标量s，使得
b
1
=
s
H
a
T
b_1=sHa^T
b1=sHaT 与
b
=
s
H
a
T
b=sHa^T
b=sHaT都表示同一个点b。若令
s
=
1
h
33
s=\frac{1}{h_{33}}
s=h331，则
1
h
33
H
\frac{1}{h_{33}}H
h331H 为:
1
h
33
H
=
[
h
11
h
33
h
12
h
33
h
13
h
33
h
21
h
33
h
22
h
33
h
23
h
33
h
31
h
33
h
32
h
33
1
]
=
[
h
11
h
33
,
h
12
h
33
,
h
13
h
33
,
h
21
h
33
,
h
22
h
33
,
h
23
h
33
,
h
31
h
33
,
h
32
h
33
,
1
]
T
(10)
\tag{10} \color{blue} \frac{1}{h_{33}} H=\left[\begin{array}{ccc} \frac{h_{11}}{h_{33}} & \frac{h_{12}}{h_{33}} & \frac{h_{13}}{h_{33}} \\ \frac{h_{21}}{h_{33}} & \frac{h_{22}}{h_{33}} & \frac{h_{23}}{h_{33}} \\ \frac{h_{31}}{h_{33}} & \frac{h_{32}}{h_{33}} & 1 \end{array}\right]=[ \frac{h11}{h_{33}},\frac{h12}{h_{33}},\frac{h13}{h_{33}},\frac{h21}{h_{33}},\frac{h22}{h_{33}},\frac{h23}{h_{33}},\frac{h31}{h_{33}},\frac{h32}{h_{33}},1]^T
h331H=⎣
⎡h33h11h33h21h33h31h33h12h33h22h33h32h33h13h33h231⎦
⎤=[h33h11,h33h12,h33h13,h33h21,h33h22,h33h23,h33h31,h33h32,1]T(10)
从公式(9)可以看出，其实H只有8个变量（8个自由度）。因此，只需要4个点对，然后通过解线性方程组就可以求得
H
H
H。也可以多于4个点对。m个点对的公式如下(下标表示矩阵大小)：
A
(
2
m
,
9
)
H
(
9
,
1
)
=
0
(11)
\tag {11} \color{blue} A_{(2m,9)}H_{(9,1)}=0
A(2m,9)H(9,1)=0(11)如果是八个点对展开类似如下(
x
,
y
x,y
x,y与
x
′
,
y
′
x',y'
x′,y′为对应点对)：
[
0
0
0
−
x
1
−
y
1
−
1
x
1
∗
y
1
′
y
1
∗
y
1
′
y
1
′
x
1
y
1
1
0
0
0
−
x
1
∗
x
1
′
−
y
1
∗
x
1
′
−
x
1
′
0
0
0
−
x
2
−
y
2
−
1
x
2
∗
y
2
′
y
2
∗
y
2
′
y
2
′
x
2
y
2
1
0
0
0
−
x
2
∗
x
2
′
−
y
2
∗
x
2
′
−
x
2
′
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
⋯
0
0
0
−
x
7
−
y
7
−
1
x
7
∗
y
7
′
y
7
∗
y
7
′
y
7
′
x
7
y
7
1
0
0
0
−
x
7
∗
x
7
′
−
y
7
∗
x
7
′
−
x
7
′
0
0
0
−
x
8
−
y
8
−
1
x
8
∗
y
8
′
y
8
∗
y
8
′
y
8
′
x
8
y
8
1
0
0
0
−
x
8
∗
x
8
′
−
y
8
∗
x
8
′
−
x
8
′
]
[
h
1
h
1
h
3
h
4
h
5
h
6
h
7
h
8
h
9
]
=
0
(11)
\tag {11} \color{blue} \left[\begin{array}{ccccccccc} 0 & 0 & 0 & -x_{1} & -y_{1} & -1 & x_{1} * y'_{1} & y_{1} * y'_{1} & y'_{1} \\ x_{1} & y_{1} & 1 & 0 & 0 & 0 & -x_{1} * x'_{1} & -y_{1} * x'_{1} & -x'_{1}\\ 0 & 0 & 0 & -x_{2} & -y_{2} & -1 & x_{2} * y'_{2} & y_{2} * y'_{2} & y'_{2} \\ x_{2} & y_{2} & 1 & 0 & 0 & 0 & -x_{2} * x'_{2} & -y_{2} * x'_{2} & -x'_{2}\\ \cdots & \cdots & \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \\ \cdots & \cdots & \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \\ 0 & 0 & 0 & -x_{7} & -y_{7} & -1 & x_{7} * y'_{7} & y_{7} * y'_{7} & y'_{7} \\ x_{7} & y_{7} & 1 & 0 & 0 & 0 & -x_{7} * x'_{7} & -y_{7} * x'_{7} & -x'_{7}\\ 0 & 0 & 0 & -x_{8} & -y_{8} & -1 & x_{8} * y'_{8} & y_{8} * y'_{8} & y'_{8} \\ x_{8} & y_{8} & 1 & 0 & 0 & 0 & -x_{8} * x'_{8} & -y_{8} * x'_{8} & -x'_{8}\\ \end{array}\right]\left[\begin{array}{l} h_{1} \\ h_{1} \\ h_{3} \\ h_{4} \\ h_{5} \\ h_{6} \\ h_{7} \\ h_{8} \\ h_{9} \end{array}\right]=0
⎣
⎡0x10x2⋯⋯0x70x80y10y2⋯⋯0y70y80101⋯⋯0101−x10−x20⋯⋯−x70−x80−y10−y20⋯⋯−y70−y80−10−10⋯⋯−10−10x1∗y1′−x1∗x1′x2∗y2′−x2∗x2′⋯⋯x7∗y7′−x7∗x7′x8∗y8′−x8∗x8′y1∗y1′−y1∗x1′y2∗y2′−y2∗x2′⋯⋯y7∗y7′−y7∗x7′y8∗y8′−y8∗x8′y1′−x1′y2′−x2′⋯⋯y7′−x7′y8′−x8′⎦
⎤⎣
⎡h1h1h3h4h5h6h7h8h9⎦
⎤=0(11)
四、代码流程：
源码位于 src/Initializer.cc文件中
1 将当前帧和参考帧中的特征点坐标进行归一化
2 选择8个归一化之后的点对进行迭代
3 八点法计算单应矩阵矩阵
4 利用重投影误差为当次RANSAC的结果评分
5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
步骤
1
:
\color{blue}{步骤1:}
步骤1:
主要是把参考帧特征点变换到当前帧特征点的平移以及尺度变换去除。简单的说，做了归一化，再去求解 Homography 矩阵，该矩阵中只包含了旋转信息。这里的归一化，主要包含了减均值(消除平移变换)，除偏差(消除尺度缩放)。
步骤
2
:
\color{blue}{步骤2:}
步骤2:
循环迭代mMaxIterations=200次,每次选取把对匹配的特征点，
步骤
3
:
\color{blue}{步骤3:}
步骤3:
八点法计算单应矩阵矩阵，这里主要使用真接对
H
H
H 进行SVD分解，即
U
∗
Σ
∗
V
T
U∗Σ∗V^T
U∗Σ∗VT。的方式求解。该求解的方式，这篇博客就不进行讲解了，大家可以参考博客:
步骤
4
:
\color{blue}{步骤4:}
步骤4:
利用重投影误差为当次RANSAC的结果评分。使用八点法计算出来的 Homography 矩阵， 他适应于这八对特征点的转换，但是并不一定完全适用于特征点。所以我们把当前帧的所有特征点, 使用该矩阵映射到参考帧，然后从参考帧再映回来。重映射回来特征点坐标误差。
步骤
5
:
\color{blue}{步骤5:}
步骤5:
根据重映射回来特征点坐标误差进行评分，累计所有特征的评分，作为该 Homography 矩阵的评分。 这里需要注意的一个点，如果其中某些特征点分值太低，我们则认为其特征点为外点，不再线内。也就是这样的特征带点我们认为是一个离群点。同时对特征点进行线内，线外的标记
最后获得 mMaxIterations=200 个 Homography 矩阵，但是我们只选择了其中分值最高的矩阵。特征点线内，线外的标记，也是与该矩阵进行对应。
五、源码注释
主调函数为 FindHomography 函数，其内部核心函数为:
Normalize() 归一化操作
ComputeH21() 八点法计算参考帧到当前帧的H矩阵
CheckHomography() 重投影误差评分
C o m p u t e H 21 ( ) 与 C h e c k H o m o g r a p h y ( ) 在后面的博客有详细的讲解 \color{red}{ComputeH21()与} CheckHomography() 在后面的博客有详细的讲解 ComputeH21()与CheckHomography()在后面的博客有详细的讲解，所以这里简单过一下即可。
void Initializer::FindHomography()代码注释如下
/**
* @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<cv::KeyPoint> &vKeys, vector<cv::Point2f> &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<N; i++)
{
//分别累加特征点的X、Y坐标
meanX += vKeys[i].pt.x;
meanY += vKeys[i].pt.y;
}
//计算X、Y坐标的均值
meanX = meanX/N;
meanY = meanY/N;
// Step 2 计算特征点X,Y坐标离均值的平均偏离程度 meanDevX, meanDevY，注意不是标准差
float meanDevX = 0;
float meanDevY = 0;
// 将原始特征点减去均值坐标，使x坐标和y坐标均值分别为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;
float sX = 1.0/meanDevX;
float sY = 1.0/meanDevY;
// Step 3 将x坐标和y坐标分别进行尺度归一化，使得x坐标和y坐标的一阶绝对矩分别为1
// 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值（期望）
for(int i=0; i<N; i++)
{
//对，就是简单地对特征点的坐标进行进一步的缩放
vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
}
// Step 4 计算归一化矩阵：其实就是前面做的操作用矩阵变换来表示而已
// |sX 0 -meanx*sX|
// |0 sY -meany*sY|
// |0 0 1 |
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;
}
/**
* @brief 用DLT方法求解单应矩阵H
* 这里最少用4对点就能够求出来，不过这里为了统一还是使用了8对点求最小二乘解
*
* @param[in] vP1 参考帧中归一化后的特征点
* @param[in] vP2 当前帧中归一化后的特征点
* @return cv::Mat 计算的单应矩阵H
*/
cv::Mat Initializer::ComputeH21(
const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame
const vector<cv::Point2f> &vP2) //归一化后的点, in current frame
{
// 基本原理：见附件推导过程：
// |x'| | h1 h2 h3 ||x|
// |y'| = a | h4 h5 h6 ||y| 简写: x' = a H x, a为一个尺度因子
// |1 | | h7 h8 h9 ||1|
// 使用DLT(direct linear tranform)求解该模型
// x' = a H x
// ---> (x') 叉乘 (H x) = 0 (因为方向相同) (取前两行就可以推导出下面的了)
// ---> Ah = 0
// A = | 0 0 0 -x -y -1 xy' yy' y'| h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 |
// |-x -y -1 0 0 0 xx' yx' x'|
// 通过SVD求解Ah = 0，A^T*A最小特征值对应的特征向量即为解
// 其实也就是右奇异值矩阵的最后一列
//获取参与计算的特征点的数目
const int N = vP1.size();
// 构造用于计算的矩阵 A
cv::Mat A(2*N, //行，注意每一个点的数据对应两行
9, //列
CV_32F); //float数据类型
// 构造矩阵A，将每个特征点添加到矩阵A中的元素
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;
}
// 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置
cv::Mat u,w,vt;
//使用opencv提供的进行奇异值分解的函数
cv::SVDecomp(A, //输入，待进行奇异值分解的矩阵
w, //输出，奇异值矩阵
u, //输出，矩阵U
vt, //输出，矩阵V^T
cv::SVD::MODIFY_A | //输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存
cv::SVD::FULL_UV); //FULL_UV=把U和VT补充成单位正交方阵
// 返回最小奇异值所对应的右奇异向量
// 注意前面说的是右奇异值矩阵的最后一列，但是在这里因为是vt，转置后了，所以是行；由于A有9列数据，故最后一列的下标为8
return vt.row(8).reshape(0, //转换后的通道数，这里设置为0表示是与前面相同
3); //转换后的行数,对应V的最后一列
}
/**
* @brief 对给定的homography matrix打分,需要使用到卡方检验的知识
*
* @param[in] H21 从参考帧到当前帧的单应矩阵
* @param[in] H12 从当前帧到参考帧的单应矩阵
* @param[in] vbMatchesInliers 匹配好的特征点对的Inliers标记
* @param[in] sigma 方差，默认为1
* @return float 返回得分
*/
float Initializer::CheckHomography(
const cv::Mat &H21, //从参考帧到当前帧的单应矩阵
const cv::Mat &H12, //从当前帧到参考帧的单应矩阵
vector<bool> &vbMatchesInliers, //匹配好的特征点对的Inliers标记
float sigma) //估计误差
{
// 说明：在已值n维观测数据误差服从N(0，sigma）的高斯分布时
// 其误差加权最小二乘结果为 sum_error = SUM(e(i)^T * Q^(-1) * e(i))
// 其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵
// 误差加权最小二次结果越小，说明观测数据精度越高
// 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高
// 算法目标： 检查单应变换矩阵
// 检查方式：通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差
// 算法流程
// input: 单应性矩阵 H21, H12, 匹配点集 mvKeys1
// do:
// for p1(i), p2(i) in mvKeys:
// error_i1 = ||p2(i) - H21 * p1(i)||2
// error_i2 = ||p1(i) - H12 * p2(i)||2
//
// w1 = 1 / sigma / sigma
// w2 = 1 / sigma / sigma
//
// if error1 < th
// score += th - error_i1 * w1
// if error2 < th
// score += th - error_i2 * w2
//
// if error_1i > th or error_2i > th
// p1(i), p2(i) are inner points
// vbMatchesInliers(i) = true
// else
// p1(i), p2(i) are outliers
// vbMatchesInliers(i) = false
// end
// end
// output: score, inliers
// 特点匹配个数
const int N = mvMatches12.size();
// Step 1 获取从参考帧到当前帧的单应矩阵的各个元素
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);
// 给特征点对的Inliers标记预分配空间
vbMatchesInliers.resize(N);
// 初始化score值
float score = 0;
// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值
const float th = 5.991;
//信息矩阵，方差平方的倒数
const float invSigmaSquare = 1.0/(sigma * sigma);
// Step 2 通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权重投影误差
// H21 表示从img1 到 img2的变换矩阵
// H12 表示从img2 到 img1的变换矩阵
for(int i = 0; i < N; i++)
{
// 一开始都默认为Inlier
bool bIn = true;
// Step 2.1 提取参考帧和当前帧之间的特征匹配点对
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;
// Step 2.2 计算 img2 到 img1 的重投影误差
// x1 = H12*x2
// 将图像2中的特征点通过单应变换投影到图像1中
// |u1| |h11inv h12inv h13inv||u2| |u2in1|
// |v1| = |h21inv h22inv h23inv||v2| = |v2in1| * w2in1inv
// |1 | |h31inv h32inv h33inv||1 | | 1 |
// 计算投影归一化坐标
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;
// 计算重投影误差 = ||p1(i) - H12 * p2(i)||2
const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);
const float chiSquare1 = squareDist1 * invSigmaSquare;
// Step 2.3 用阈值标记离群点，内点的话累加得分
if(chiSquare1>th)
bIn = false;
else
// 误差越大，得分越低
score += th - chiSquare1;
// 计算从img1 到 img2 的投影变换误差
// x1in2 = H21*x1
// 将图像2中的特征点通过单应变换投影到图像1中
// |u2| |h11 h12 h13||u1| |u1in2|
// |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv
// |1 | |h31 h32 h33||1 | | 1 |
// 计算投影归一化坐标
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;
// Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier point
if(bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
}
return score;
}
/**
* @brief 计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分
* 原理参考Multiple view geometry in computer vision P109 算法4.4
* Step 1 将当前帧和参考帧中的特征点坐标进行归一化
* Step 2 选择8个归一化之后的点对进行迭代
* Step 3 八点法计算单应矩阵矩阵
* Step 4 利用重投影误差为当次RANSAC的结果评分
* Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
*
* @param[in & out] vbMatchesInliers 标记是否是外点
* @param[in & out] score 计算单应矩阵的得分
* @param[in & out] H21 单应矩阵结果
*/
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
// Number of putative matches
//匹配的特征点对总数
const int N = mvMatches12.size();
// Normalize coordinates
// Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换
// 具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2
// 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值
// 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标
//归一化后的参考帧1和当前帧2中的特征点坐标
vector<cv::Point2f> vPn1, vPn2;
// 记录各自的归一化矩阵
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
//这里求的逆在后面的代码中要用到，辅助进行原始尺度的恢复
cv::Mat T2inv = T2.inv();
// Best Results variables
// 记录最佳评分
score = 0.0;
// 取得历史最佳评分时,特征点对的inliers标记
vbMatchesInliers = vector<bool>(N,false);
// Iteration variables
//某次迭代中，参考帧的特征点坐标
vector<cv::Point2f> vPn1i(8);
//某次迭代中，当前帧的特征点坐标
vector<cv::Point2f> vPn2i(8);
//以及计算出来的单应矩阵、及其逆矩阵
cv::Mat H21i, H12i;
// 每次RANSAC记录Inliers与得分
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
//下面进行每次的RANSAC迭代
for(int it=0; it<mMaxIterations; it++)
{
// Select a minimum set
// Step 2 选择8个归一化之后的点对进行迭代
for(size_t j=0; j<8; j++)
{
//从mvSets中获取当前次迭代的某个特征点对的索引信息
int idx = mvSets[it][j];
// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标
// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标
vPn1i[j] = vPn1[mvMatches12[idx].first]; //first存储在参考帧1中的特征点索引
vPn2i[j] = vPn2[mvMatches12[idx].second]; //second存储在参考帧1中的特征点索引
}//读取8对特征点的归一化之后的坐标
// Step 3 八点法计算单应矩阵
// 利用生成的8个归一化特征点对, 调用函数 Initializer::ComputeH21() 使用八点法计算单应矩阵
// 关于为什么计算之前要对特征点进行归一化，后面又恢复这个矩阵的尺度？
// 可以在《计算机视觉中的多视图几何》这本书中P193页中找到答案
// 书中这里说,8点算法成功的关键是在构造解的方称之前应对输入的数据认真进行适当的归一化
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
// 单应矩阵原理：X2=H21*X1，其中X1,X2 为归一化后的特征点
// 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2 得到:T2 * mvKeys2 = Hn * T1 * mvKeys1
// 进一步得到:mvKeys2 = T2.inv * Hn * T1 * mvKeys1
H21i = T2inv*Hn*T1;
//然后计算逆
H12i = H21i.inv();
// Step 4 利用重投影误差为当次RANSAC的结果评分
currentScore = CheckHomography(H21i, H12i, //输入，单应矩阵的计算结果
vbCurrentInliers, //输出，特征点对的Inliers标记
mSigma); //TODO 测量误差，在Initializer类对象构造的时候，由外部给定的
// Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
if(currentScore>score)
{
//如果当前的结果得分更高，那么就更新最优计算结果
H21 = H21i.clone();
//保存匹配好的特征点对的Inliers标记
vbMatchesInliers = vbCurrentInliers;
//更新历史最优评分
score = currentScore;
}
}
}
六、结语
该片博客主要讲解了 Homography 的求解过程，主要流程如下:
1、特征归一化
2、随机选取八个点
3、八点法求解H矩阵
4、计算重投影误差
5、选择最好的H矩阵，并且标记在线内的特征点
下一篇博客我们再来看看 Fundamental 基本矩阵的求解方式。另外，在后续的博客中会对ComputeH21() 与
CheckHomography() 进行专门的讲解。
本文内容来自计算机视觉life ORB-SLAM2 课程课件
1597
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
