2D-2D对极几何运动估计和三角测量法
本讲关注基于特征点方式的视觉里程计算法。我们将介绍什么是特征点，如何提取和匹配特征点，以及如何根据配对的特征点估计相机运动。
前端也称为视觉里程计(VO)。它根据相邻图像的信息，估计出粗略的相机运动，给后端提供较好的初始值。VO的实现方法，按是否需要提取特征，分为特征点法的前端以及不提特征的直接法前端。
总结一下:
视觉里程计(VO)首先需要找到相邻帧图像数据的对应点，也就是特征点。大部分使用的是ORB特征。ORB特征主要包括两个部分，一个是FAST关键点，一个是BRIEF描述子。等找到两幅图像的特征点之后，需要进行特征点匹配，有暴力匹配法和快速近似最近邻(FLANN)算法等等。匹配后的特征点对，会用来进行机器人的运动估计。运动估计分为三种，这一篇文章主要讲的是2D-2D的对极几何，适用于单目摄像头，只知道像素坐标的场景。对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步:根据配对点的像素位置，求出本质矩阵，基础矩阵，和单应矩阵。求出这些矩阵之后，通过SVD等方法求出旋转矩阵和平移矩阵
R
,
t
R,t
R,t。最后，我们需要用相机的运动估计特征点的空间位置，采用的是三角测量法。
一、 特征点法
首先，从图像中选取比较有代表性的点。这些点在相机视角发生少量变化后会保持不变，所以我们会在各个图像中找到相同的点。然后，在这些点的基础上，讨论相机位姿估计问题，以及这些点的定位问题。在经典SLAM模型中，把它们称为路标。而在视觉SLAM中，路标则是指图像特征(Features)。 也就是关键点的匹配，估计相机姿态。
特征点是图像里一些特别的地方。相比于朴素的角点，这些人工设计的特征点能够拥有如下的性质:
- 可重复性: 相同的"区域"可以在不同的图像中找到。
- 可区别性: 不同的"区域"有不同的表达。
- 高效率性: 同一图像中，特征点的数量应远小于像素的数量。
- 本地性: 特征仅与一小片图像区域相关。
特征点由关键点(Key-point)和描述子(Descriptor)两部分组成。关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。描述子通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。描述子是按照"外观相似的特征应该有相似的描述子"的原则设计的。
ORB(Oriented FAST and Rotated BRIEF)特征则是目前看来非常具有代表性的实时图像特征。它改进了FAST检测子不具有方向性的问题，并采用速度极快的二进制描述子BRIEF，使整个图像特征提取的环节大大加速。根据作者在论文中的测试，在同一幅图像中同时提取约1000个特征点的情况下，ORB约要花费15.3ms，SURF约花费217.3ms，SIFT约花费5228.7ms。由此可以看出ORB在保持了特征子具有旋转，尺度不变性的同时，速度方面提升明显，对于实时性要求很高的SLAM来说是一个很好的选择。具体可以看这个博客。
特征匹配是视觉SLAM中极为关键的一步，宽泛地说，特征匹配解决了SLAM中的数据关联问题(data association)，即确定当前看到的路标与之前看到的路标之间的对应关系。
考虑两个时刻的图像。如果在图像 I t I_t It中提取到特征点 x t m , m = 1 , 2 , . . . , M x_t^m, \quad m=1,2,...,M xtm,m=1,2,...,M，在图像 I t + 1 I_{t+1} It+1中提取到特征点 x t + 1 n , n = 1 , 2 , . . . , N x^n_{t+1}, \quad n=1,2,...,N xt+1n,n=1,2,...,N。如何寻找这两个集合元素的对应关系呢？最简单的特征匹配方法就是暴力匹配(Brute-Force Matcher)。 也就是对每一个特征点 x t m x_t^m xtm，与所有的 x t + 1 n x_{t+1}^n xt+1n测量描述子的距离，然后排序，取最近的一个点作为匹配点。描述子距离表示了两个特征之间的相似程度，不过在实际运用中还可以取不同的距离度量范数。对于浮点类型的描述子，使用欧氏距离进行度量即可。而对于二进制的描述子(比如BRIEF这样的)，我们往往使用汉明距离(Hamming distance) 做为度量——两个二进制串之间的汉明距离，指的是它们不同位数的个数。
然而，当特征点数量很大时，暴力匹配法的运算量将变得很大，特别是当我们想要匹配一个帧和一张地图的时候。这不符合我们在SLAM中的实时性需求。此时快速近似最近邻(FLANN)算法更加适合于匹配点数量极多的情况。
二、估计相机运动
接下来，根据匹配的点对，估计相机的运动。
- 当相机为单目时，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何来解决。
- 当相机为双目、RGB-D时，或者我们通过某种方法得到了距离信息，那问题就是根据两组3D点估计运动。该问题通常用ICP来解决。
- 如果我们有3D点和它们在相机的投影位置，也能估计相机的运动。该问题通过PnP求解。
1. 2D-2D:对极几何
我们希望求取两帧图像 I 1 , I 2 I_1,I_2 I1,I2之间的运动，设第一帧到第二帧的运动为 R , t R,t R,t。两个相机中心分别为 0 1 , 0 2 0_1,0_2 01,02。现在，考虑 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被称为基线(Baseline)。称极平面与两个像平面 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 P的空间位置，以及相机的运动。
在第一帧的坐标系下,设
P
P
P的空间位置为:
P
=
[
X
,
Y
,
Z
]
T
P=[X,Y,Z]^T
P=[X,Y,Z]T
根据针孔相机模型，我们知道两个像素点
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_1p_1 = KP, \quad s_2p_2=K(RP+t)
s1p1=KP,s2p2=K(RP+t)
这里
K
K
K为相机内参矩阵，
R
,
t
R,t
R,t为两个坐标系的相机运动。也可以写成:
p
1
=
K
P
,
p
2
=
K
(
R
P
+
t
)
p_1 = KP, \quad p_2=K(RP+t)
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=R x_1 + t
x2=Rx1+t
两边同时左乘
t
∧
t^{\land}
t∧。相当于两侧同时与
t
t
t做外积:
t
∧
x
2
=
t
∧
R
x
1
t^{\land}x_2 = t^{\land} Rx_1
t∧x2=t∧Rx1
然后，两侧同时左乘
x
2
T
x_2^T
x2T:
x
2
T
t
∧
x
2
=
x
2
T
t
∧
R
x
1
x_2^Tt^{\land}x_2 = x^T_2t^{\land}Rx_1
x2Tt∧x2=x2Tt∧Rx1
其中，
t
∧
x
2
t^{\land}x_2
t∧x2是一个与
t
t
t和
x
2
x_2
x2都垂直的向量。把它再和
x
2
x_2
x2做内积时候，將得到0。
x
2
T
t
∧
R
x
1
=
0
x_2^T t^{\land}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
p_2^TK^{-T}t^{\land}RK^{-1}p_1=0
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^{\land}R, \quad F=K^{-T}EK^{-1}, \quad x_2^TEx_1 = p_2^TFp_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。
(1)本质矩阵
本质矩阵为: E = t ∧ R E=t^{\land}R E=t∧R。他是一个3x3的矩阵，内有9个未知数。
- 本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对 E E E乘以任意非零常数后，对极约束依然满足。我们把这件事情称为 E E E在不同尺度下是等价的。
- 根据 E = t ∧ R E=t^{\land}R E=t∧R,可以知道本质矩阵 E E E的奇异值必定是 [ σ , σ , 0 ] T [\sigma,\sigma, 0]^T [σ,σ,0]T的形式。这称为本质矩阵的内在性质。
- 另一方面，由于平移和旋转各有三个自由度，故 t ∧ R t^{\land}R t∧R共有六个自由度。但由于尺度等价性，故 E E E实际上有五个自由度。
E
E
E具有五个自由度的事实， 表明我们最少可以用五对点来求解
E
E
E。但是，
E
E
E的内在性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用八对点来估计
E
E
E——这就是经典的八点法(Eight-point-algorithm)，具体如下:
考虑一对匹配点，它们的归一化坐标为:
x
1
=
[
u
1
,
v
1
,
1
]
T
,
x
2
=
[
u
2
,
v
2
,
1
]
T
x_1=[u_1,v_1,1]^T, x_2=[u_2,v_2,1]^T
x1=[u1,v1,1]T,x2=[u2,v2,1]T。根据对极约束，有:
(
u
1
,
v
1
,
1
)
(
e
1
e
2
e
3
e
4
e
5
e
6
e
7
e
8
e
9
)
(
u
2
v
2
1
)
=
0
(u_1, v_1, 1)\bigg( \begin{matrix} e_1 ~ e_2 ~ e_3 \\ e_4 ~ e_5 ~ e_6 \\ e_7 ~ e_8 ~ e_9 \end{matrix} \bigg) \bigg( \begin{matrix} u_2 \\ v_2 \\ 1 \end{matrix}\biggl) = 0
(u1,v1,1)(e1 e2 e3e4 e5 e6e7 e8 e9)(u2v21)=0
把矩阵
E
E
E展开，写成向量的形式:
e
=
[
e
1
,
e
2
,
e
3
,
e
4
,
e
5
,
e
6
,
e
7
,
e
8
,
e
9
]
T
e=[e_1, e_2, e_3, e_4, e_5, e_6, e_7, e_8, e_9]^T
e=[e1,e2,e3,e4,e5,e6,e7,e8,e9]T
那么对极约束可以写成与
e
e
e有关的线性形式:
[
u
1
u
2
,
u
1
v
2
,
u
1
,
v
1
u
2
,
v
1
,
u
2
,
v
2
,
1
]
⋅
e
=
0
[u_1u_2, u_1v_2, u_1, v_1u_2, v_1, u_2, v_2 ,1] \cdot e=0
[u1u2,u1v2,u1,v1u2,v1,u2,v2,1]⋅e=0
同理，对于其它点对也有相同的表示。我们把所有点都放到一个方程中，变成线性方程组( u i , v i u^i,v^i ui,vi表示第 i i i个特征点，以此类推):
这八个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为8 × 9。 e e e位于该矩阵的零空间中。如果系数矩阵是满秩的(即秩为8)，那么它的零空间维数为1，也就是 e e e构成一条线。这与 e e e的尺度等价性是一致的。如果八对匹配点组成的矩阵满足秩 为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^T
E=UΣVT
其中
U
,
V
U,V
U,V为正交阵，
Σ
\Sigma
Σ为奇异值矩阵。根据
E
E
E的内在性质，我们知道
Σ
=
d
i
a
g
(
σ
,
σ
,
0
)
\Sigma=diag(\sigma, \sigma,0)
Σ=diag(σ,σ,0)。在SVD分解中，对于任意一个
E
E
E，存在两个可能的
t
,
R
t, R
t,R与它对应:
t
1
∧
=
U
R
z
(
π
2
)
Σ
U
T
,
R
1
=
U
R
z
(
π
2
)
T
V
T
t
2
∧
=
U
R
z
(
−
π
2
)
Σ
U
T
,
R
2
=
U
R
z
(
π
2
)
T
V
T
t_1^{\land}=UR_{z(\frac{\pi}{2})} \Sigma U^T, \quad R_1 = U R_{z(\frac{\pi}{2})}^TV^T \\ t_2^{\land}=UR_{z(-\frac{\pi}{2})} \Sigma U^T, \quad R_2 = U R_{z(\frac{\pi}{2})}^TV^T
t1∧=URz(2π)ΣUT,R1=URz(2π)TVTt2∧=URz(−2π)ΣUT,R2=URz(2π)TVT
其中
R
z
(
π
2
)
R_{z(\frac{\pi}{2})}
Rz(2π)表示沿着Z轴旋转90度得到的旋转矩阵。同时，由于
−
E
-E
−E和
E
E
E等价，所以对任意一个
t
t
t取负号，也会得到哦它能够样的结果。因此，从
E
E
E分解到
t
,
R
t, R
t,R时候，一共存在四个可能的解:
上图形象地显示了分解本质矩阵得到的四个解。我们已知空间点在相机(蓝色线)上的投影(红点)，想要求解相机的运动。在保持红点不变的情况下，可以画出四种可能的情况，不过幸运的是，只有第一种解中， P P P在两个相机中都具有正的深度。因此，只要把任意一点代入四种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。
如果利用 E E E的内在性质，那么它只有五个自由度。所以最小可以通过五对点来求解相机运动。 然而这种做法形式复杂，从工程实现角度考虑，由于平时通常会有几十对乃至上百对的匹配点，从八对减至五对意义并不明显。
剩下的问题还有一个: 根据线性方程解出的
E
E
E，可能不满足
E
E
E的内在性质----它的奇异值不一定为
σ
,
σ
,
0
\sigma, \sigma, 0
σ,σ,0的形式。这时，在做SVD时候，我们会刻意地把
Σ
\Sigma
Σ矩阵调整称为上面的样子。通常的做法是，对八点法求得的
E
E
E进行SVD分解后，会得到奇异值矩阵
Σ
=
d
i
a
g
(
σ
1
,
σ
2
,
σ
3
)
\Sigma = diag(\sigma_1, \sigma_2, \sigma_3)
Σ=diag(σ1,σ2,σ3)，不妨设
σ
1
≥
σ
2
≥
σ
3
\sigma_1 \geq \sigma_2 \geq \sigma_3
σ1≥σ2≥σ3，取:
E
=
U
d
i
a
g
(
σ
1
+
σ
2
2
,
σ
1
+
σ
2
2
,
0
)
V
T
E= U diag(\frac{\sigma_1 + \sigma_2}{2}, \frac{\sigma_1 + \sigma_2}{2}, 0)V^T
E=Udiag(2σ1+σ2,2σ1+σ2,0)VT
这相当于把求出来的矩阵投影到了
E
E
E所在的流形上。当然，更简单的做法是將奇异值矩阵取成
d
i
a
g
(
1
,
1
,
0
)
diag(1,1,0)
diag(1,1,0)，因为
E
E
E具有尺度等价性，这样做也是合理的。
(1)单应矩阵
除了基本矩阵和本质矩阵，我们还有一种称为单应矩阵(Homography)H 的东西，它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上(比如墙，地面等)，则可以通过单应性来进行运动估计。 这种情况在无人机携带的俯视相机，或扫地机携带的顶视相机中比较常见。由于之前没提到过单应，稍微介绍一下。
单应矩阵通常描述处于共同平面上的一些点，在两张图像之间的变换关系。考虑在图像 I 1 I_1 I1和 I 2 I_2 I2有一对匹配好的特征点 p 1 p_1 p1和 p 2 p_2 p2。这些特征点落在某个平面上。设这个平面满足方程:
n
T
P
+
d
=
0
n^TP+d=0
nTP+d=0
稍加整理，得:
−
n
T
P
d
=
1
-\frac{n^TP}{d}=1
−dnTP=1
然后，回顾开头式子
s
1
p
1
=
K
P
,
s
2
p
2
=
K
(
R
P
+
t
)
s_1p_1=KP, \quad s_2p_2=K(RP+t)
s1p1=KP,s2p2=K(RP+t)，得到:
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 &= K(RP+t) \\ & = K(RP + t \cdot (-\frac{n^TP}{d})) \\ & = K(R - \frac{tn^T}{d})P \\ & = K(R - \frac{tn^T}{d})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
p_2 = Hp_1
p2=Hp1
它的定义与旋转、平移以及平面的参数有关。与基础矩阵
F
F
F类似，单应矩阵
H
H
H也是一个3×3的矩阵，求解时的思路也和
F
F
F类似，同样地可以先根据匹配点计算
H
H
H，然后将它分解以计算旋转和平移。把上式展开，得
(
u
1
,
v
1
,
1
)
=
(
h
1
h
2
h
3
h
4
h
5
h
6
h
7
h
8
h
9
)
(
u
2
v
2
1
)
(u_1, v_1, 1)=\bigg( \begin{matrix} h_1 ~ h_2 ~ h_3 \\ h_4 ~ h_5 ~ h_6 \\ h_7 ~ h_8 ~ h_9 \end{matrix} \bigg) \bigg( \begin{matrix} u_2 \\ v_2 \\ 1 \end{matrix}\biggl)
(u1,v1,1)=(h1 h2 h3h4 h5 h6h7 h8 h9)(u2v21)
请注意这里的等号是在非零因子下成立的。们在实际处理中，通常乘以一个非零因子使得
h
9
=
1
h_9=1
h9=1(在它取非零值时)。然后根据第三行，去掉这个非零因子，于是:
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
u_2 = \frac{h_1u_1 + h_2v_1 + h_3}{h_7u_1+h_8v_1+h_9} \\ \quad \\ v_2 = \frac{h_4u_1 + h_5v_1 + h_6}{h_7u_1+h_8v_1+h_9} \\
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
+
h
8
v
1
+
h
9
=
u
2
h
4
u
1
+
h
5
v
1
+
h
6
−
h
7
u
1
+
h
8
v
1
+
h
9
=
v
2
h_1u_1 + h_2v_1 + h_3 - h_7u_1+h_8v_1+h_9 = u_2 \\ \quad \\ h_4u_1 + h_5v_1 + h_6 - h_7u_1+h_8v_1+h_9 = v_2
h1u1+h2v1+h3−h7u1+h8v1+h9=u2h4u1+h5v1+h6−h7u1+h8v1+h9=v2
这样一组匹配点对就可以构造出两项约束(事实上有三个约束，但是因为线性相关，只取前两个)，于是自由度为8的单应矩阵可以通过4对匹配特征点算出(注意:这些特征点不能有三点共线的情况)，即求解以下的线性方程组(当
h
9
=
0
h_9=0
h9=0时，右侧为零):
这种做法把
H
H
H矩阵看成了向量，通过解该向量的线性方程来恢复
H
H
H，又称直接线性变换法(Direct Linear Transform)。与本质矩阵相似，求出单应矩阵以后需要对其进行分解，才可以得到相应的旋转矩阵
R
R
R和平移向量
t
t
t。分解的方法包括数值法与解析法。与本质矩阵的分解类似，单应矩阵的分解同样会返回四组旋转矩阵与平移向量，并且同时可以计算出它们分别对应的场景点所在平面的法向量。如果已知成像的地图点的深度全为正值(即在相机前方)，则又可以排除两组解。最后仅剩两组解，这时需要通过更多的先验信息进行判断。通常我们可以通过假设已知场景平面的法向量来解决，如场景平面与相机平面平行，那么法向量
n
n
n的理论值为
1
T
1^T
1T。
单应性在SLAM中具重要意义。当特征点共面，或者相机发生纯旋转的时候，基础矩阵的自由度下降，这就出现了所谓的退化(degenerate)。 现实中的数据总包含一些噪声，这时候如果我们继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。 为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵
F
F
F和单应矩阵
H
H
H，选择重投影误差比较小的那个作为最终的运动估计矩阵。
2. 三角测量
对极几何约束估计相机运动的方法存在局限性。在得到运动之后，下一步我们需要用相机的运动估计特征点的空间位置。 在单目SLAM中，仅通过单张图像无法获得像素的深度信息，我们需要通过 三角测量(Triangulation)或(三角化) 的方法来估计地图点的深度。
三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离。在SLAM中，我们主要用三角化来估计像素点的距离。
按照对极几何的定义，设
x
1
,
x
2
x_1, x_2
x1,x2为两个特征点的归一化坐标。那么它们满足:
s
1
x
1
=
s
2
R
x
2
+
t
s_1x_1 = s_2 R x_2 + t
s1x1=s2Rx2+t
我们现在已经知道了
R
,
t
R, t
R,t，想要求解两个特征点的深度
s
1
,
s
2
s_1, s_2
s1,s2。当然这两个深度是可以分开求的，比方说先来看
s
2
s_2
s2。如果我要算
s
2
s_2
s2，那么先对上式两侧左乘一个
x
1
∧
x_1^{\land}
x1∧，得到:
s
1
x
1
∧
x
1
=
0
=
s
2
x
1
∧
R
x
2
+
x
1
∧
t
s_1x_1^{\land}x_1 = 0 = s_2x_1^{\land}Rx_2 + x_1^{\land}t
s1x1∧x1=0=s2x1∧Rx2+x1∧t
式左侧为零，右侧可看成 s 2 s_2 s2的一个方程，可以根据它直接求得 s 2 s_2 s2。所以我们就得到了两个帧下的点的深度，确定了它们的空间坐标。当然，由于噪声的存在，我们估得的 R , t R,t R,t，不一定精确使式 s 1 x 1 = s 2 R x 2 + t s_1x_1 = s_2 R x_2 + t s1x1=s2Rx2+t为零，所以更常见的做法求 最小二乘解而不是零解。
三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈的上三角测量。因此，纯旋转是无法使用三角测量的，因为对极约束将永远满足。在平移存在的情况下，我们还要关心三角测量的不确定性， 这会引出一个三角测量的矛盾。
当平移很小时，像素上的不确定性将导致较大的深度不确定性。 也就是说，如果特征点运动一个像素 δ x \delta x δx 使得视线角变化了一个角度 δ θ \delta \theta δθ， 那么测量到深度值将有 δ d \delta d δd的变化。 从几何关系可以看到， 当 t t t较大时， δ d \delta d δd将明显变小，这说明平移较大时，在同样的相机分辨率下，三角化测量将更精确。对该过程的定量分析可以使用正弦定理得到，但我们这里先考虑定性分析。
因此，要增加三角化的精度，其一是提高特征点的提取精度，也就是提高图像分辨率——但这会导致图像变大，提高计算成本。另一方式是使平移量增大。 但是，平移量增大，会导致图像的外观发生明显的变化，如箱子原先被挡住的侧面显示出来了，比如反射光发生变化了，等等。外观变化会使得特征提取与匹配变得困难。 总而言之，在增大平移，会导致匹配失效。而平移太小，则三角化精度不够——这就是三角化的矛盾。
虽然三角化的深度估计存在局限性，但只要我们愿意，也能够定量地计算每个特征点的位置及不确定性。 所以，如果假设特征点服从高斯分布，并且对它不断地进行观测，在信息正确的情况下，我们就能够期望它的方差会不断减小乃至收敛。这就得到了一个滤波器，称为深度滤波器(Depth Filter)。
1388
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
