视觉SLAM理论到实践系列文章
下面是《视觉SLAM十四讲》学习笔记的系列记录的总链接，本人发表这个系列的文章链接均收录于此
视觉SLAM理论到实践系列文章链接
下面是专栏地址：
视觉SLAM理论到实践专栏
文章目录
前言
高翔博士的《视觉SLAM14讲》学习笔记的系列记录
视觉SLAM理论到实践系列（六）——特征点法视觉里程计（2）
2D-2D 对极几何
特征匹配之后，得到了特征点之间的对应关系
- 如果只有两个单目图像，得到2D-2D间的关系 ——对极几何
- 如果匹配的是帧和地图，得到3D-2D间的关系 ——PnP
- 如果匹配的是RGB-D图，得到3D-3D间的关系 ——ICP
对极约束
以下图为例，我们希望求取两帧图像 I 1 , I 2 I_1,I_2 I1,I2 之间的运动，设第一帧到第二帧的运动为 R , t R,t R,t。两个相机中心分别为 O 1 , O 2 O_1,O_2 O1,O2。现在，考虑 I 1 I_1 I1 中有一个特征点 p 1 p_1 p1，它在 I 2 I_2 I2 中对应着特征点 p 2 p_2 p2。我们知道两者是通过特征匹配得到的。如果匹配正确，说明它们确实是同一个空间点在两个成像平面上的投影。
这里需要一些术语来描述它们之间的几何关系。
首先，连线 O 1 p 1 → \overrightarrow{O_1p_1} O1p1 和连线在 O 2 p 2 → \overrightarrow{O_2p_2} O2p2 三维空间中会相交于点 P P P。这时 O 1 , O 2 , P O_1,O_2,P O1,O2,P三个点可以确定一个平面，称为极平面(Epipolar plane)。
O 1 O 2 O_1O_2 O1O2 连线与像平面 I 1 , I 2 I_1,I_2 I1,I2 的交点分别为 e 1 , e 2 e_1,e_2 e1,e2。 e 1 , e 2 e_1,e_2 e1,e2 称为极点(Epipoles)， O 1 O 2 O_1O_2 O1O2 被称为基线。
我们称极平面与两个像平面 I 1 , I 2 I_1,I_2 I1,I2之间的相交线 l 1 , l 2 l_1,l_2 l1,l2为极线(Epipolar line)。
从第一帧的角度看，射线 O 1 p 1 → \overrightarrow{O_1p_1} O1p1 是某个像素可能出现的空间位置——因为该射线上的所有点都会投影到同一个像素点。
同时，如果不知道 P P P 的位置，那么当我们在第二幅图像上看时，连线 e 2 p 2 → \overrightarrow{e_2p_2} e2p2（也就是第二幅图像中的极线)就是 P P P 可能出现的投影的位置，也就是射线 O 1 p 1 → \overrightarrow{O_1p_1} O1p1在第二个相机中的投影。
现在，由于我们通过特征点匹配确定了 p 2 p_2 p2的像素位置，所以能够推断P的空间位置，以及相机的运动。
要提醒读者的是，这多亏了正确的特征匹配。
如果没有特征匹配，我们就没法确定 p 2 p_2 p2到底在极线的哪个位置。那时，就必须在极线上搜索以获得正确的匹配，这将在第12讲中提到。
在第一帧的坐标系下，设
P
P
P 的空间位置为
P
=
[
X
,
Y
,
Z
]
T
.
\boldsymbol{P}=[X,Y,Z]^{\mathrm{T}}.
P=[X,Y,Z]T.
根据第 5 讲介绍的针孔相机模型，我们知道两个像素点
p
1
,
p
2
p_1,p_2
p1,p2 的像素位置为
s
1
p
1
=
K
P
,
s
2
p
2
=
K
(
R
P
+
t
)
.
s_{1}\boldsymbol{p}_{1}=\boldsymbol{K}\boldsymbol{P},\quad s_{2}\boldsymbol{p}_{2}=\boldsymbol{K}\left(\boldsymbol{R}\boldsymbol{P}+\boldsymbol{t}\right).
s1p1=KP,s2p2=K(RP+t).
这里
K
K
K 为相机内参矩阵，
R
,
t
R,t
R,t 为两个坐标系的相机运动。具体来说，这里计算的是
R
21
R_{21}
R21 和
t
21
t_{21}
t21,因为它们把第一个坐标系下的坐标转换到第二个坐标系下。
如果我们愿意，也可以把它们写成李代数形式。
有时，我们会使用齐次坐标表示像素点。
在使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如，
s
1
p
1
s_1\boldsymbol{p}_1
s1p1 和
p
1
\boldsymbol{p}_1
p1 成投影关系，它们在齐次坐标的意义下是相等的。我们称这种相等关系为尺度意义下相等( equal up to a scale)，记作：
s
p
≃
p
.
s\boldsymbol{p}\simeq\boldsymbol{p}.
sp≃p.
那么，上述两个投影关系可写为
p 1 ≃ K P , p 2 ≃ K ( R P + t ) . p_{1}\simeq KP,\quad p_{2}\simeq K\left(RP+t\right). p1≃KP,p2≃K(RP+t).
现在，取：
x
1
=
K
−
1
p
1
,
x
2
=
K
−
1
p
2
.
x_{1}=K^{-1}p_{1},\quad x_{2}=K^{-1}p_{2}.
x1=K−1p1,x2=K−1p2.
这里的
x
1
,
x
2
x_1,x_2
x1,x2 是两个像素点的归一化平面上的坐标。代人上式，得
x
2
≃
R
x
1
+
t
.
x_{2}\simeq Rx_{1}+t.
x2≃Rx1+t.
两边同时左乘
t
∧
t^\mathrm{\wedge}
t∧，这相当于两侧同时与
t
t
t 做外积：
t
∧
x
2
≃
t
∧
R
x
1
.
t^{\wedge}x_{2}\simeq t^{\wedge}Rx_{1}.
t∧x2≃t∧Rx1.
然后，两侧同时左乘
x
2
T
x_2^\mathrm{T}
x2T：
x
2
T
t
∧
x
2
≃
x
2
T
t
∧
R
x
1
.
\quad\quad\quad\boldsymbol{x_2^\mathrm{T}t^\mathrm{\wedge}x_2}\simeq\boldsymbol{x_2^\mathrm{T}t^\mathrm{\wedge}Rx_1}.
x2Tt∧x2≃x2Tt∧Rx1.
观察等式左侧，
t
∧
x
2
t^\wedge x_2
t∧x2 是一个与
t
t
t 和
x
2
x_{2}
x2 都垂直的向量。它再和
x
2
x_{2}
x2做内积时，将得到 0。由于等式左侧严格为零，乘以任意非零常数之后也为零，于是我们可以把
≃
\simeq
≃ 写成通常的等号。
因此， 有
x
2
T
t
∧
R
x
1
=
0
\boldsymbol{x_2^\mathrm{T}t^\mathrm{\wedge}Rx_1}=0
x2Tt∧Rx1=0
重新代入
p
1
,
p
2
p_1,p_2
p1,p2， 有
p
2
T
K
−
T
t
∧
R
K
−
1
p
1
=
0.
\begin{gathered} p_{2}^{T}K^{-T}t^{\wedge}RK^{-1}p_{1}=0. \end{gathered}
p2TK−Tt∧RK−1p1=0.
这两个式子都称为对极约束，它以形式简洁著名。
它的几何意义是 O 1 , P , O 2 O_1,P,O_2 O1,P,O2 三者共面。对极约束中同时包含了平移和旋转。
我们把中间部分记作两个矩阵： 基础矩阵( Fundamental Matrix )
F
F
F 和本质矩阵( Essential Matrix)
E
E
E, 于是可以进一步简化对极约束：
E
=
t
∧
R
,
F
=
K
−
T
E
K
−
1
,
x
2
T
E
x
1
=
p
2
T
F
p
1
=
0.
E=t^{\wedge}\boldsymbol{R},\quad\boldsymbol{F}=\boldsymbol{K}^{-\mathrm{T}}\boldsymbol{E}\boldsymbol{K}^{-1},\quad\boldsymbol{x}_{2}^{\mathrm{T}}\boldsymbol{E}\boldsymbol{x}_{1}=\boldsymbol{p}_{2}^{\mathrm{T}}\boldsymbol{F}\boldsymbol{p}_{1}=0.
E=t∧R,F=K−TEK−1,x2TEx1=p2TFp1=0.
对极约束简洁地给出了两个匹配点的空间位置关系。
于是，相机位姿估计问题变为以下两步：
-
根据配对点的像素位置求出 E E E 或者 F F F。
-
根据 E E E 或者 F F F 求出 R , t R,t R,t
由于 E E E 和 F F F 只相差了相机内参，而内参在 SLAM 中通常是已知的，所以实践中往往使用形式更简单的 E E E。我们以 E E E为例，介绍上面两个问题如何求解。
本质矩阵
根据定义，本质矩阵 E = t ∧ R E=t^\wedge R E=t∧R。它是一个 3 × 3 3\times3 3×3 的矩阵，内有 9个未知数。那么，是不是任意一个 3 × 3 3\times3 3×3 的矩阵都可以被当成本质矩阵呢？ 从 E E E 的构造方式上看，有以下值得注意的地方：
- 本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对 E E E 乘以任意非零常数后，对极约束依然满足。我们把这件事情称为 E E E在不同尺度下是等价的。
- 根据 E = t ∧ R E=t^\wedge R E=t∧R, 可以证明 [ 3 ] ^{[3]} [3], 本质矩阵 E E E 的奇异值必定是 [ σ , σ , 0 ] T [\sigma,\sigma,0]^{\mathrm{T}} [σ,σ,0]T 的形式。这称为本质矩阵的内在性质。
- 另外，由于平移和旋转各有 3 个自由度，故 t ∧ R t^{\wedge}R t∧R 共有 6 个自由度。但由于尺度等价性， 故 E E E 实际上有 5 个自由度。
E E E 具有 5 个自由度的事实，表明我们最少可以用 5 对点来求解 E E E。
但是， E E E 的内在性质是一种非线性性质，在估计时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用 8 对点来估计 E E E——这就是经典的八点法( Eight-point-algorithm ) [ 48 , 49 ] ^{[48,49]} [48,49]。
八点法只利用了 E E E 的线性性质， 因此可以在线性代数框架下求解。下面我们来看八点法是如何工作的。
考虑一对匹配点,它们的归一化坐标为
x
1
=
[
u
1
,
v
1
,
1
]
T
,
x
2
=
[
u
2
,
v
2
,
1
]
T
\boldsymbol{x}_1=[u_1,v_1,1]^\mathrm{T},\boldsymbol{x}_2=[u_2,v_2,1]^\mathrm{T}
x1=[u1,v1,1]T,x2=[u2,v2,1]T，根据对极约束，有
(
u
2
,
v
2
,
1
)
(
e
1
e
2
e
3
e
4
e
5
e
6
e
7
e
8
e
9
)
(
u
1
v
1
1
)
=
0.
\left(u_2,v_2,1\right)\begin{pmatrix}e_1&e_2&e_3\\\\e_4&e_5&e_6\\\\e_7&e_8&e_9\end{pmatrix}\begin{pmatrix}u_1\\\\v_1\\\\1\end{pmatrix}=0.
(u2,v2,1)
e1e4e7e2e5e8e3e6e9
u1v11
=0.
把矩阵
E
E
E 展开，写成向量的形式：
e
=
[
e
1
,
e
2
,
e
3
,
e
4
,
e
5
,
e
6
,
e
7
,
e
8
,
e
9
]
T
,
\boldsymbol{e}=\left[e_{1},e_{2},e_{3},e_{4},e_{5},e_{6},e_{7},e_{8},e_{9}\right]^{\mathrm{T}},
e=[e1,e2,e3,e4,e5,e6,e7,e8,e9]T,
那么，对极约束可以写成与
e
e
e 有关的线性形式：
[
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
]
⋅
e
=
0.
[u_2u_1,u_2v_1,u_2,v_2u_1,v_2v_1,v_2,u_1,v_1,1]\cdot\boldsymbol{e}=0.
[u2u1,u2v1,u2,v2u1,v2v1,v2,u1,v1,1]⋅e=0.
同理，对于其他点对也有相同的表示。我们把所有点都放到一个方程中，变成线性方程组
(
u
i
,
v
i
(u^i,v^i
(ui,vi 表示第
i
i
i 个特征点，依此类推 ):
(
u
2
1
u
1
1
u
2
1
v
1
1
u
2
1
v
2
1
u
1
1
v
2
1
v
1
1
v
2
1
u
1
1
v
1
1
1
u
2
2
u
1
2
u
2
2
v
1
2
u
2
2
v
2
2
u
1
2
v
2
2
v
1
2
v
1
2
u
1
2
v
1
2
1
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
u
2
8
u
1
8
u
2
8
v
1
8
u
2
8
v
2
8
u
1
8
v
2
8
v
1
8
v
2
8
u
1
8
v
1
8
1
)
(
e
1
e
2
e
3
e
4
e
5
e
5
e
6
e
7
e
8
e
9
)
=
0.
\begin{pmatrix} u_2^1u_1^1&u_2^1v_1^1&u_2^1&v_2^1u_1^1&v_2^1v_1^1&v_2^1&u_1^1&v_1^1&1\\ u_2^2u_1^2&u_2^2v_1^2&u_2^2&v_2^2u_1^2&v_2^2v_1^2&v_1^2&u_1^2&v_1^2&1\\ \vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots\\ u_2^8u_1^8&u_2^8v_1^8&u_2^8&v_2^8u_1^8&v_2^8v_1^8&v_2^8&u_1^8&v_1^8&1 \end{pmatrix}\begin{pmatrix}e_1\\e_2\\e_3\\e_4\\e_5\\e_5\\e_6\\e_7\\e_8\\e_9\end{pmatrix}=0.
u21u11u22u12⋮u28u18u21v11u22v12⋮u28v18u21u22⋮u28v21u11v22u12⋮v28u18v21v11v22v12⋮v28v18v21v12⋮v28u11u12⋮u18v11v12⋮v18111
e1e2e3e4e5e5e6e7e8e9
=0.
这 8 个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为
8
×
9
8\times9
8×9。
e e e 位于该矩阵的零空间中。
如果系数矩阵是满秩的(即秩为8)，那么它的零空间维数为1，也就是 e e e 构成一条线。这与 e e e的尺度等价性是一致的。如果 8 对匹配点组成的矩阵满足秩为 8 的条件，那么 E E E 的各元素就可由上述方程解得。
接下来的问题是如何根据已经估得的本质矩阵 E E E, 恢复出相机的运动 R , t R,t R,t。这个过程是由奇异值分解(SVD)得到的。
设
E
E
E 的SVD为
E
=
U
Σ
V
T
E=U\Sigma V^{\mathrm{T}}
E=UΣVT
其中
U
,
V
U,V
U,V 为正交阵，
Σ
\Sigma
Σ 为奇异值矩阵。根据
E
E
E 的内在性质，我们知道$\Sigma=
d
i
a
g
diag
diag( \sigma, \sigma, 0) $。
在 SVD 分解中，对于任意一个
E
E
E, 存在两个可能的
t
,
R
t,R
t,R 与它对应：
t
1
∧
=
U
R
Z
(
π
2
)
Σ
U
T
,
R
1
=
U
R
Z
T
(
π
2
)
V
T
t
2
∧
=
U
R
Z
(
−
π
2
)
Σ
U
T
,
R
2
=
U
R
Z
T
(
−
π
2
)
V
T
.
\begin{aligned} t_1^{\wedge} &=\boldsymbol{UR}_{Z}(\frac{\pi}{2})\boldsymbol{\Sigma U}^{\mathrm{T}},\quad\boldsymbol{R}_{1}=\boldsymbol{UR}_{Z}^{\mathrm{T}}(\frac{\pi}{2})\boldsymbol{V}^{\mathrm{T}} \\\\ t_2^{\wedge} &=\boldsymbol{UR}_{Z}(-\frac{\pi}{2})\boldsymbol{\Sigma}\boldsymbol{U}^{\mathrm{T}},\quad\boldsymbol{R}_{2}=\boldsymbol{U}\boldsymbol{R}_{Z}^{\mathrm{T}}(-\frac{\pi}{2})\boldsymbol{V}^{\mathrm{T}}. \end{aligned}
t1∧t2∧=URZ(2π)ΣUT,R1=URZT(2π)VT=URZ(−2π)ΣUT,R2=URZT(−2π)VT.
其中，
R
Z
(
π
2
)
R_Z(\frac\pi2)
RZ(2π) 表示沿
Z
Z
Z 轴旋转 90° 得到旋转矩阵。
同时，由于 − E - E −E 和 E E E 等价，所以对任意一个 t t t 取负号，也会得到同样的结果。因此，从 E E E 分解到 t , R t,R t,R 时，一共存在 4 个可能的解。
在保持红色点不变的情况下，可以画出4种可能的情况，不过只有第一种解中 P P P 在两个相机中都具有正的深度。
如果利用 E E E 的内在性质，那么它只有 5 个自由度。所以最少可以通过 5 对点来求解相机运动 [ 50 , 51 ] ^{[50,51]} [50,51]。
但需要利用E的非线性性质，原理较复杂
然而这种做法形式复杂，从工程实现角度考虑，由于平时通常会有几十对乃至上百对的匹配点，从8 对减至 5 对意义并不明显。为保持简单，我们这里就只介绍基本的八点法。
剩下的一个问题是： 根据线性方程解出的 E E E, 可能不满足 E E E 的内在性质——它的奇异值不一定为 σ , σ , 0 \sigma,\sigma,0 σ,σ,0 的形式。这时，我们会刻意地把 Σ \Sigma Σ 矩阵调整成上面的样子。
通常的做法是，对八点法求得的
E
E
E 进行 SVD,会得到奇异值矩阵 $\Sigma= \mathrm{diag}( \sigma_1, \sigma_2, \sigma_3) $, 不妨设
σ
1
⩾
σ
2
⩾
σ
3
\sigma_1\geqslant\sigma_2\geqslant\sigma_3
σ1⩾σ2⩾σ3。取：
E
=
U
d
i
a
g
(
σ
1
+
σ
2
2
,
σ
1
+
σ
2
2
,
0
)
V
T
.
\boldsymbol{E}=\boldsymbol{U}\mathrm{diag}(\frac{\sigma_{1}+\sigma_{2}}{2},\frac{\sigma_{1}+\sigma_{2}}{2},0)\boldsymbol{V}^{\mathrm{T}}.
E=Udiag(2σ1+σ2,2σ1+σ2,0)VT.
这相当于是把求出来的矩阵投影到了
E
E
E 所在的流形上。
当然，更简单的做法是将奇异值矩阵取成 $ \mathrm{diag}( 1, 1, 0) $，因为 E E E 具有尺度等价性，所以这样做也是合理的。
八点法的讨论
- 用于单目SLAM的初始化
- 尺度不确定性：归一化 t 或特征点的平均深度
- 纯旋转问题：t=0 时无法求解
- 多于八对点时：最小二乘
- 有外点时：RANSAC
单应矩阵
除了基本矩阵和本质矩阵，二视图几何中还存在另一种常见的矩阵：单应矩阵( Homography ) H H H，它描述了两个平面之间的映射关系。
若场景中的特征点都落在同一平面上(比如墙、地面等)，则可以通过单应性进行运动估计。
这种情况在无人机携带的俯视相机或扫地机携带的顶视相机中比较常见。
单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系。
设图像
I
1
I_{1}
I1 和
I
2
I_{2}
I2 有一对匹配好的特征点
p
1
p_1
p1 和
p
2
p_2
p2。这个特征点落在平面
P
P
P 上，设这个平面满足方程：
n
T
P
+
d
=
0.
n^\mathrm{T}P+d=0.
nTP+d=0.
整理得
−
n
T
P
d
=
1.
-\frac{n^{\mathrm{T}}P}{d}=1.
−dnTP=1.
由于有
p
2
≃
K
(
R
P
+
t
)
≃
K
(
R
P
+
t
⋅
(
−
n
T
P
d
)
)
≃
K
(
R
−
t
n
T
d
)
P
≃
K
(
R
−
t
n
T
d
)
K
−
1
p
1
.
\begin{aligned} p_2& \simeq K\left(RP+t\right) \\ &\simeq\boldsymbol{K}\left(\boldsymbol{R}\boldsymbol{P}+\boldsymbol{t}\cdot(-\frac{n^{\mathrm{T}}\boldsymbol{P}}{d})\right) \\ &\simeq K\left(R-\frac{tn^{\mathrm{T}}}{d}\right)P \\ &\simeq K\left(R-\frac{tn^{\mathrm{T}}}{d}\right)K^{-1}p_{1}. \end{aligned}
p2≃K(RP+t)≃K(RP+t⋅(−dnTP))≃K(R−dtnT)P≃K(R−dtnT)K−1p1.
于是,我们得到了一个直接描述图像坐标
p
1
p_1
p1 和
p
2
p_2
p2 之间的变换，把中间这部分记为
H
H
H，于是：
p
2
≃
H
p
1
.
\boldsymbol{p}_{2}\simeq\boldsymbol{H}\boldsymbol{p}_{1}.
p2≃Hp1.
它的定义与旋转、平移及平面的参数有关。
与基础矩阵 F F F 类似，单应矩阵 H H H 也是一个 3 × 3 3\times3 3×3 的矩阵，求解时的思路和 F F F 类似，同样可以先根据匹配点计算 H H H，然后将它分解以计算旋转和平移。
把上式展开，得
(
u
2
v
2
1
)
≃
(
h
1
h
2
h
3
h
4
h
5
h
6
h
7
h
8
h
9
)
(
u
1
v
1
1
)
.
\begin{pmatrix}u_2\\v_2\\1\end{pmatrix}\simeq\begin{pmatrix}h_1&h_2&h_3\\h_4&h_5&h_6\\h_7&h_8&h_9\end{pmatrix}\begin{pmatrix}u_1\\v_1\\1\end{pmatrix}.
u2v21
≃
h1h4h7h2h5h8h3h6h9
u1v11
.
注意，这里的等号依然是
≃
\simeq
≃ 而不是普通的等号，所以
H
H
H 矩阵也可以乘以任意非零常数。
我们在实际处理中可以令
h
9
=
1
h_9=1
h9=1 (在它取非零值时)。然后根据第 3 行，去掉这个非零因子，于是有
u
2
=
h
1
u
1
+
h
2
v
1
+
h
3
h
7
u
1
+
h
8
v
1
+
h
9
v
2
=
h
4
u
1
+
h
5
v
1
+
h
6
h
7
u
1
+
h
8
v
1
+
h
9
.
\begin{gathered} 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{gathered}
u2=h7u1+h8v1+h9h1u1+h2v1+h3v2=h7u1+h8v1+h9h4u1+h5v1+h6.
整理得
h 1 u 1 + h 2 v 1 + h 3 − h 7 u 1 u 2 − h 8 v 1 u 2 = u 2 h 4 u 1 + h 5 v 1 + h 6 − h 7 u 1 v 2 − h 8 v 1 v 2 = v 2 . h_{1}u_{1}+h_{2}v_{1}+h_{3}-h_{7}u_{1}u_{2}-h_{8}v_{1}u_{2}=u_{2}\\\\ h_{4}u_{1}+h_{5}v_{1}+h_{6}-h_{7}u_{1}v_{2}-h_{8}v_{1}v_{2}=v_{2}. h1u1+h2v1+h3−h7u1u2−h8v1u2=u2h4u1+h5v1+h6−h7u1v2−h8v1v2=v2.
这样一组匹配点对就可以构造出两项约束(事实上有三个约束，但是因为线性相关，只取前两个)，于是自由度为 8 的单应矩阵可以通过 4 对匹配特征点算出 (在非退化的情况下，即这些特征点不能有三点共线的情况 ),
即求解以下的线性方程组(当
h
9
=
0
h_{9}=0
h9=0 时，右侧为零 ):
(
u
1
1
v
1
1
1
0
0
0
−
u
1
1
u
2
1
−
v
1
1
u
2
1
0
0
0
u
1
1
v
1
1
1
−
u
1
1
v
2
1
−
v
1
1
v
2
1
u
1
2
v
1
2
1
0
0
0
−
u
1
2
u
2
2
−
v
1
2
u
2
2
0
0
0
u
1
2
v
1
2
1
−
u
1
2
v
2
2
−
v
2
2
v
2
2
u
1
3
v
1
3
1
0
0
0
−
u
1
3
u
2
3
−
v
1
3
u
2
3
0
0
0
u
1
3
v
1
3
1
−
u
1
3
v
2
3
−
v
1
3
v
2
3
u
1
4
v
1
4
1
0
0
0
−
u
1
4
u
2
4
−
v
1
4
u
2
4
0
0
0
u
1
4
v
1
4
1
−
u
1
4
v
2
4
−
v
1
4
v
2
4
)
(
h
1
h
2
h
3
h
4
h
5
h
6
h
7
h
8
)
=
(
u
2
1
v
2
1
u
2
2
v
2
2
u
2
3
v
2
3
u
2
4
v
2
4
)
\begin{pmatrix}u_1^1&v_1^1&1&0&0&0&-u_1^1u_2^1&-v_1^1u_2^1\\0&0&0&u_1^1&v_1^1&1&-u_1^1v_2^1&-v_1^1v_2^1\\u_1^2&v_1^2&1&0&0&0&-u_1^2u_2^2&-v_1^2u_2^2\\0&0&0&u_1^2&v_1^2&1&-u_1^2v_2^2&-v_2^2v_2^2\\u_1^3&v_1^3&1&0&0&0&-u_1^3u_2^3&-v_1^3u_2^3\\0&0&0&u_1^3&v_1^3&1&-u_1^3v_2^3&-v_1^3v_2^3\\u_1^4&v_1^4&1&0&0&0&-u_1^4u_2^4&-v_1^4u_2^4\\0&0&0&u_1^4&v_1^4&1&-u_1^4v_2^4&-v_1^4v_2^4\end{pmatrix}\begin{pmatrix}h_1\\h_2\\h_3\\h_4\\h_5\\h_6\\h_7\\h_8\end{pmatrix}=\begin{pmatrix}u_2^1\\v_2^1\\u_2^2\\v_2^2\\u_2^3\\v_2^3\\u_2^4\\v_2^4\end{pmatrix}
u110u120u130u140v110v120v130v140101010100u110u120u130u140v110v120v130v1401010101−u11u21−u11v21−u12u22−u12v22−u13u23−u13v23−u14u24−u14v24−v11u21−v11v21−v12u22−v22v22−v13u23−v13v23−v14u24−v14v24
h1h2h3h4h5h6h7h8
=
u21v21u22v22u23v23u24v24
这种做法把
H
H
H 矩阵看成了向量，通过解该向量的线性方程来恢复
H
H
H,又称直接线性变换法( Direct Linear Transform, DLT)。
与本质矩阵相似，求出单应矩阵以后需要对其进行分解，才可以得到相应的旋转矩阵 R R R 和平移向量 t t t。分解的方法包括数值法 [ 52 , 53 ] ^{[52,53]} [52,53] 与解析法 [ 54 ] ^{[54]} [54]。
与本质矩阵的分解类似，单应矩阵的分解同样会返回 4 组旋转矩阵与平移向量，同时，可以计算出它们分别对应的场景点所在平面的法向量。如果已知成像的地图点的深度全为正值(即在相机前方)，则又可以排除两组解。最后仅剩两组解，这时需要通过更多的先验信息进行判断。
通常，我们可以通过假设已知场景平面的法向量来解决，如场景平面与相机平面平行，那么法向量 n n n 的理论值为 1 T \boldsymbol{1}^T 1T。
单应性在 SLAM 中具有重要意义。
当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化(degenerate)。现实中的数据总包含一些噪声，这时如果继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。
为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵 F F F 和单应矩阵 H H H, 选择重投影误差比较小的那个作为最终的运动估计矩阵。
代码实践
见slambook2的第7章代码 pose_estimation_2d2d.cpp
讨论
尺度不确定性
对 t t t 长度的归一化，直接导致了单目视觉的尺度不确定性。例如，程序中输出的 t t t 第一维约为 0.822。这个 0.822 究竟是指 0.822 米还是 0.822 厘米，我们是没法确定的。因为对 t t t 乘以任意比例常数后，对极约束依然是成立的。换言之，在单目 SLAM 中，对轨迹和地图同时缩放任意倍数，我们得到的图像依然是一样的。这在第 2 讲中就已经向读者介绍过了。
在单目视觉中，我们对两张图像的 t t t 归一化相当于固定了尺度。虽然我们不知道它的实际长度是多少，但我们以这时的 t t t 为单位1，计算相机运动和特征点的 3D 位置。这被称为单目 SLAM 的初始化。在初始化之后，就可以用 3D-2D 计算相机运动了。初始化之后的轨迹和地图的单位， 就是初始化时固定的尺度。因此，单目 SLAM 有一步不可避免的初始化。初始化的两张图像必须有一定程度的平移，而后的轨迹和地图都将以此步的平移为单位。
除了对 t t t 进行归一化，另一种方法是令初始化时所有的特征点平均深度为 1, 也可以固定一个尺度。相比于令 t t t 长度为 1 的做法，把特征点深度归一化可以控制场景的规模大小，使计算在数值上更稳定。
初始化的纯旋转问题
从 E E E 分解到 R , t R,t R,t 的过程中，如果相机发生的是纯旋转，导致 t t t 为零，那么，得到的 E E E 也将为零，这将导致我们无从求解 R R R。
不过，此时我们可以依靠 H H H 求取旋转，但仅有旋转时，我们无法用三角测量估计特征点的空间位置(这将在下文提到), 于是，另一个结论是，单目初始化不能只有纯旋转，必须要有一定程度的平移。
如果没有平移，单目将无法初始化。在实践中，如果初始化时平移太小，会使得位姿求解与三角化结果不稳定，从而导致失败。
相对地，如果把相机左右移动而不是原地旋转，就容易让单目 SLAM 初始化。
因而，有经验的 SLAM 研究人员，在单目 SLAM 情况下经常选择让相机进行左右平移以顺利地进行初始化。
多于8对点的情况
当给定的点数多于8 对时(例如，例程找到了 79 对匹配),我们可以计算一个最小二乘解。
回忆式 (7.13) 中线性化后的对极约束，我们把左侧的系数矩阵记为
A
:
A:
A:
A
e
=
0.
Ae=\mathbf{0}.
Ae=0.
对于八点法，
A
A
A的大小为
8
×
9
8\times9
8×9。如果给定的匹配点多于8，则该方程构成一个超定方程，即不一定存在
e
e
e 使得上式成立。因此，可以通过最小化一个二次型来求：
min
e
∥
A
e
∥
2
2
=
min
e
e
T
A
T
A
e
.
\min_{\boldsymbol{e}}\left\|\boldsymbol{A}\boldsymbol{e}\right\|_{2}^{2}=\min_{\boldsymbol{e}}e^{\mathrm{T}}\boldsymbol{A}^{\mathrm{T}}\boldsymbol{A}\boldsymbol{e}.
emin∥Ae∥22=emineTATAe.
于是就求出了在最小二乘意义下的
E
E
E 矩阵。不过，当可能存在误匹配的情况时，我们会更倾向于使用随机采样一致性( Random Sample Concensus, RANSAC )来求，而不是最小二乘。
RANSAC 是一种通用的做法，适用于很多带错误数据的情况，可以处理带有错误匹配的数据。
扫一扫
5883
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
