视觉SLAM十四讲笔记-7-2
视觉SLAM十四讲笔记-7-2
文章目录
- 视觉SLAM十四讲笔记-7-2
- 估计相机运动
-
- 7.3 2D-2D：对极几何
-
- 7.3.1 对极约束
- 7.3.2 本质矩阵
- 7.3.3 单应矩阵
- 7.4 实践：对极约束求解相机运动
-
- 尺度不确定性
- 初始化的纯旋转问题
- 多于8点的情况
估计相机运动
上一节中讲了如何匹配点对，接下来要根据匹配好的点对估计相机的运动。
1.当相机为单目相机时，只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题可以用对极几何来解决。
2.当相机为双目、RGB-D时，或者通过某种方法得到了距离信息，那么问题就是根据两组3D点估计运动。该问题一般用ICP来解决。
3.如果一组为3D，一组为2D，即得到了一些3D点和它们在相机的投影位置，也能估计相机的运动。该问题一般通过PnP来求解。
7.3 2D-2D：对极几何
7.3.1 对极约束
假设从两幅图像中得到了一对配对好的特征点，如果有若干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出两帧之间的相机运动。那么需要多少对这样的匹配点，以及匹配点之间的有什么样的几何关系尼?
如下图所示：
希望求取两幅图像 I 1 I_1 I1, I 2 I_2 I2 之间的运动，设第一帧到第二帧的运动为 R , t R,t R,t。两个相机中心为 O 1 O_1 O1, O 2 O_2 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_1 O1， O 2 O_2 O2， P P P三个点可以确定一个平面，称为极平面(Epipolar plane)。 O 1 O 2 O_1O_2 O1O2连线与像平面 I 1 I_1 I1， I 2 I_2 I2的交点分别为 e 1 e_1 e1, e 2 e_2 e2。 e 1 e_1 e1, e 2 e_2 e2称为极点(Epipoles)， O 1 O 2 O_1O_2 O1O2被称为基线。称极平面与两个像平面 I 1 I_1 I1， I 2 I_2 I2之间的相交线为极线(Epipolar line)。
从第一帧的角度看，射线 O 1 P 1 → \overrightarrow{O_1P_1} O1P1 是某个像素可能出现的空间位置—因为该射线上的所有点都会投影到同一个像素点。同时，如果不知道点 P P P 的位置，那么当从第二幅图像上看时，连线 e 2 p 2 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 2 p_2 p2 到底在极线的哪个位置。)
下面从代数角度来分析这里的几何关系。
在第一帧的坐标系下，设 P P P 的空间位置为：
P = [ X , Y , Z ] T P=[X,Y,Z]^T P=[X,Y,Z]T
根据第五章中针孔相机模型，知道两个像素点 p 1 p_1 p1， p 2 p_2 p2 的像素位置为：
s 1 p 1 = K P ， s 2 p 2 = K ( R P + t ) s_1p_1 = KP，s_2p_2 = K(RP+t) 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_1p_1 s1p1 和 p 1 p_1 p1 成投影关系，它们在齐次坐标的意义下是相等的，称这种相等关系为尺度意义下相等，记作：
s p ≃ p sp\simeq p sp≃p
那么，上述两个投影关系可以写为：
p 1 ≃ K P ， p 2 ≃ K ( R P + t ) p_1 \simeq KP，p_2 \simeq 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，x_2= K^{-1}p_2 x1=K−1p1，x2=K−1p2
这里的 x 1 x_1 x1 和 x 2 x_2 x2 是两个像素点在归一化平面上的坐标。代入上式，得：
x 2 ≃ R x 1 + t x_2 \simeq Rx_1+t x2≃Rx1+t
两边同时左乘 t ∧ t^\wedge t∧。回忆 t ∧ t^\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^T x2T：
x 2 T t ∧ x 2 ≃ x 2 T t ∧ R x 1 x_2^Tt^\wedge x_2 \simeq x_2^T t^\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 x_2^T t^\wedge Rx_1 = 0 x2Tt∧Rx1=0
重新代入 p 1 p_1 p1， p 2 p_2 p2，有：
p 2 T K − T t ∧ R K − 1 p 1 = 0 p_2^TK^{-T}t^\wedge RK^{-1}p_1 = 0 p2TK−Tt∧RK−1p1=0
上述两个式子都称为对极约束。
它的集合意义是 O 1 O_1 O1， O 2 O_2 O2， P P P 三者共面。对极约束中同时包含了旋转和平移。把中间部分记作两个矩阵：基础矩阵(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 = 0 ， p 2 T F p 1 = 0 E = t^\wedge R，F = K^{-T} EK^{-1}，x_2^T Ex_1 = 0，p_2^TFp_1 = 0 E=t∧R，F=K−TEK−1，x2TEx1=0，p2TFp1=0
对极约束简洁地给出了两个匹配点的空间位置关系。于是相机位姿估计问题变为以下两步：
1.根据配对点的像素位置求出 E E E 或者 F F F。
2.根据 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 为例，介绍上面两个问题如何求解。
7.3.2 本质矩阵
本质矩阵 E = t ∧ R E = t^\wedge R E=t∧R，它是一个 3 * 3的矩阵，，内部有 9 个未知数。是不是任意一个 3 * 3的矩阵都可以当成本质矩阵尼?从 E E E 的构造方式来看，有以下值得注意的地方：
1.本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对 E E E 乘以任意非零常数后，对极约束依然满足。把这件事情称为 E E E 在不同尺度下是等价的。
2.根据 E = t ∧ R E = t^\wedge R E=t∧R，可以证明，本质矩阵 E E E 的奇异值必定是 [ σ , σ , 0 ] T \left [ \sigma ,\sigma ,0\right ] ^T [σ,σ,0]T 的形式。这称为本质矩阵的内在性质。(这里后续再看)
3.另外，由于平移和旋转各有 3 个自由度，故 E = t ∧ R E = t^\wedge R E=t∧R 共有 6 个自由度。但由于尺度等价性，故 E = t ∧ R E = t^\wedge R E=t∧R 实际上有 5 个自由度。
E = t ∧ R E = t^\wedge R E=t∧R 具有 5 个自由度的事实，表明至少可以用 5 对点来求解 E E E。但是， E E E 的内在性质是一种非线性性质，在估计时带来麻烦，因此，也可以只考虑它的尺度等价性，使用 8 对点来估计 E E E — 这就是经典的八点法 (Eight-point-algorithm)。八点法只利用了 E E E 的线性性质，因此可以在线性代数框架下求解。
考虑一对匹配点，它们的归一化坐标为 x 1 = [ u 1 , v 1 , 1 ] T x_1 = [u_1,v_1,1]^T x1=[u1,v1,1]T， x 2 = [ u 2 , v 2 , 1 ] T x_2 = [u_2,v_2,1]^T x2=[u2,v2,1]T。根据对极约束，有：
x 2 T E x 1 = [ 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 ] = [ u 2 , v 2 , 1 ] [ u 1 e 1 + v 1 e 2 + e 3 u 1 e 4 + v 1 e 5 + e 6 u 1 e 7 + v 1 e 8 + e 9 ] = 0 \begin{gathered} x_{2}^{T} E x_{1}=\left[u_{2}, v_{2}, 1\right]\left[\begin{array}{ccc} e_{1} & e_{2} & e_{3} \\ e_{4} & e_{5} & e_{6} \\ e_{7} & e_{8} & e_{9} \end{array}\right]\left[\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right] \\ =\left[u_{2}, v_{2}, 1\right]\left[\begin{array}{c} u_{1} e_{1}+v_{1} e_{2}+e_{3} \\ u_{1} e_{4}+v_{1} e_{5}+e_{6} \\ u_{1} e_{7}+v_{1} e_{8}+e_{9} \end{array}\right]=0 \end{gathered} x2TEx1=[u2,v2,1]⎣⎡e1e4e7e2e5e8e3e6e9⎦⎤⎣⎡u1v11⎦⎤=[u2,v2,1]⎣⎡u1e1+v1e2+e3u1e4+v1e5+e6u1e7+v1e8+e9⎦⎤=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 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 \left[\begin{array}{lllllllll} 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 \end{array}\right].e = 0 [u2u1u2v1u2v2u1v2v1v2u1v11].e=0
同理，对于其他点也有类似的表示。把所有点都放到一个方程中，变成线性方程组( u i , v i u^i,v^i ui,vi表示第 i i i 个特征点，以此类推：)
( u 1 1 u 2 1 u 1 1 v 2 1 u 1 1 v 1 1 u 2 1 v 1 1 v 2 1 v 1 1 u 2 1 v 2 1 1 u 1 2 u 2 2 u 1 2 v 2 2 u 1 2 v 1 2 u 2 2 v 1 2 v 2 2 v 1 2 u 2 2 v 2 2 1 ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ u 1 8 u 2 8 u 1 8 v 2 8 u 1 8 v 1 8 u 2 8 v 1 8 v 2 8 v 1 8 u 2 8 v 2 8 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) = 0 \left(\begin{array}{ccccccccc} u_{1}^{1} u_{2}^{1} & u_{1}^{1} v_{2}^{1} & u_{1}^{1} & v_{1}^{1} u_{2}^{1} & v_{1}^{1} v_{2}^{1} & v_{1}^{1} & u_{2}^{1} & v_{2}^{1} & 1 \\ u_{1}^{2} u_{2}^{2} & u_{1}^{2} v_{2}^{2} & u_{1}^{2} & v_{1}^{2} u_{2}^{2} & v_{1}^{2} v_{2}^{2} & v_{1}^{2} & u_{2}^{2} & v_{2}^{2} & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ u_{1}^{8} u_{2}^{8} & u_{1}^{8} v_{2}^{8} & u_{1}^{8} & v_{1}^{8} u_{2}^{8} & v_{1}^{8} v_{2}^{8} & v_{1}^{8} & u_{2}^{8} & v_{2}^{8} & 1 \end{array}\right)\left(\begin{array}{c} e_{1} \\ e_{2} \\ e_{3} \\ e_{4} \\ e_{5} \\ e_{6} \\ e_{7} \\ e_{8} \\ e_{9} \end{array}\right)=0 ⎝⎜⎜⎜⎛u11u21u12u22⋮u18u28u11v21u12v22⋮u18v28u11u12⋮u18v11u21v12u22⋮v18u28v11v21v12v22⋮v18v28v11v12⋮v18u21u22⋮u28v21v22⋮v28111⎠⎟⎟⎟⎞⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛e1e2e3e4e5e6e7e8e9⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞=0
这 8 个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为 8 ∗ 9 8 * 9 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^T E=UΣVT
其中 U U U， V V 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 ⊤ , R 1 = U R Z ⊤ ( π 2 ) V ⊤ t 2 ∧ = U R Z ( − π 2 ) Σ U ⊤ , R 2 = U R Z ⊤ ( − π 2 ) V ⊤ \begin{aligned} &t_{1}^{\wedge}=U R_{Z}\left(\frac{\pi}{2}\right) \Sigma U^{\top}, R_{1}=U R_{Z}^{\top}\left(\frac{\pi}{2}\right) V^{\top} \\ &t_{2}^{\wedge}=U R_{Z}\left(-\frac{\pi}{2}\right) \Sigma U^{\top}, R_{2}=U R_{Z}^{\top}\left(-\frac{\pi}{2}\right) V^{\top} \end{aligned} t1∧=URZ(2π)ΣU⊤,R1=URZ⊤(2π)V⊤t2∧=URZ(−2π)ΣU⊤,R2=URZ⊤(−2π)V⊤
其中， R Z ( π 2 ) R_{Z}\left(\frac{\pi}{2}\right) RZ(2π)表示沿 Z Z Z 轴旋转 9 0 ∘ 90^{\circ} 90∘ 得到旋转矩阵。同时，由于 − E -E −E 和 E E E 等价，所以对于任意一个 t t t 取负号，也会得到同样的结果。因此，从 E E E 分解到 t , R t,R t,R 时，一共有 4 个可能的解。
下图形象地展示了分解本质矩阵得到了 4 个解。
图片来源：link
不过幸运的是，只有第一种解中 P P P 在两个相机中都具有正的深度。因此，只需要把任意一点代入 4 个解中，检测该点在两个相机中的深度，就可以确定哪个解是正确的了。
虽然 E = t ∧ R E = t^\wedge R E=t∧R 具有 5 个自由度的事实，表明至少可以用 5 对点来求解 E E E。但是这种做法形式复杂。从工程角度考虑，通常还是直接用 8 点法。
7.3.3 单应矩阵
除了基本矩阵和本质矩阵，二视图几何中存在还存在另一种常见的矩阵：单应矩阵(Homography) 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^TP+d = 0 nTP+d=0
稍加整理，得：
− n T P d = 1 -\frac{n^TP}{d} = 1 −dnTP=1
则：
p 2 ≃ K ( R P + t ) ≃ K ( R P + t . ( − n T P d ) ) ≃ ( R − − t n T d ) P ≃ K ( R − − t n T d ) K − 1 p 1 p_2 \simeq K(RP+t)\simeq K(RP + t.(-\frac{n^TP}{d} )) \simeq (R - -\frac{tn^T}{d})P\simeq K (R - -\frac{tn^T}{d}) K^{-1}p_1 p2≃K(RP+t)≃K(RP+t.(−dnTP))≃(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 \simeq Hp_1 p2≃Hp1
它的定义与旋转、平移及平面的参数有关。与基础矩阵 F F F，单应矩阵 H H H 也是一个 3 * 3 的矩阵，求解思路与 F F F 矩阵类似。同样先根据匹配点计算 H H H，然后将它分解以计算旋转和平移。
将上式展开，得：
p 2 = ( u 2 v 2 1 ) ≃ H p 1 = ( h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ) ( u 1 v 1 1 ) p_{2}=\left(\begin{array}{c} u_{2} \\ v_{2} \\ 1 \end{array}\right) \simeq H p_{1}=\left(\begin{array}{ccc} h_{1} & h_{2} & h_{3} \\ h_{4} & h_{5} & h_{6} \\ h_{7} & h_{8} & h_{9} \end{array}\right)\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right) p2=⎝⎛u2v21⎠⎞≃Hp1=⎝⎛h1h4h7h2h5h8h3h6h9⎠⎞⎝⎛u1v11⎠⎞
在实际处理时令 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 q + 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{aligned} &u_{2}=\frac{h_{1} u_{1}+h_{2} v_{1}+h_{3}}{h_{7} u_{1}+h_{8} v_{q}+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{aligned} u2=h7u1+h8vq+h9h1u1+h2v1+h3v2=h7u1+h8v1+h9h4u1+h5v1+h6
整理得：
u 2 = h 1 u 1 + h 2 v 1 + h 3 h 7 u 1 + h 8 v 1 + 1 u 2 = h 1 u 1 + h 2 v 1 + h 3 − h 7 u 1 u 2 − h 8 v 1 u 2 v 2 = h 4 u 1 + h 5 v 1 + h 6 h 7 u 1 + h 8 v 1 + 1 v 2 = h 4 u 1 + h 5 v 1 + h 6 − h 7 u 1 v 2 − h 8 v 1 v 2 \begin{gathered} u_{2}=\frac{h_{1} u_{1}+h_{2} v_{1}+h_{3}}{h_{7} u_{1}+h_{8} v_{1}+1} \\ u_{2}=h_{1} u_{1}+h_{2} v_{1}+h_{3}-h_{7} u_{1} u_{2}-h_{8} v_{1} u_{2} \\ v_{2}=\frac{h_{4} u_{1}+h_{5} v_{1}+h_{6}}{h_{7} u_{1}+h_{8} v_{1}+1} \\ v_{2}=h_{4} u_{1}+h_{5} v_{1}+h_{6}-h_{7} u_{1} v_{2}-h_{8} v_{1} v_{2} \end{gathered} u2=h7u1+h8v1+1h1u1+h2v1+h3u2=h1u1+h2v1+h3−h7u1u2−h8v1u2v2=h7u1+h8v1+1h4u1+h5v1+h6v2=h4u1+h5v1+h6−h7u1v2−h8v1v2
这样一组匹配点对就可以构造出两项约束，于是自由度为 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 2 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 1 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 1 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 ) \left(\begin{array}{cccccccc} 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}^{2} \\ 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_{1}^{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}^{1} 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} \end{array}\right)\left(\begin{array}{c} h_{1} \\ h_{2} \\ h_{3} \\ h_{4} \\ h_{5} \\ h_{6} \\ h_{7} \\ h_{8} \end{array}\right)=\left(\begin{array}{c} 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{array}\right) ⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛u110u120u130u140v110v120v130v140101010100u110u120u130u140v110v120v130v1401010101−u11u21−u11v21−u12u22−u12v22−u13u23−u13v23−u11u24−u14v24−v11u21−v11v22−v12u22−v12v22−v13u23−v13v23−v14u24−v14v24⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛h1h2h3h4h5h6h7h8⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛u21v21u22v22u23v23u24v24⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
这种做法把单应矩阵 H H H 当作向量，通过解出这个向量的线性方程来恢复出 H H H，又称作直接线性变换法(Direct Linear Transform, DLT)。本质矩阵 E E E 类似，求出单应矩阵以后需要对其进行奇异值分解，才可以得到相应的旋转矩阵 R R R 和平移向量 t t t 。分解的方法包括数值法与解析法。与本质矩阵的分解类似，单应矩阵 H H H 的分解同样会返回四组旋转矩阵和平移向量，同时，可以计算出它们分别对应的场景点所在平面的法向量。如果已知成像的地图点的深度全为正值（也就是在相机的前方），则又可以排除两组解。最后仅剩的两组解，需要通过更多的先验信息来判断。通常，可以通过假设已知场景平面的法向量来解决，如场景平面与相机平面平行，那么法向量 n n n 的理论值为 1 T 1^T 1T。
单应性在SLAM中具有重要意义。当特征点共面或者相机发生纯旋转时，基础矩阵 F F F 的自由度会下降，这就出现了退化 degenerate 现象。现实中的数据总是包含一些噪声，这时如果继续使用八点法求解基础矩阵 F F F ，矩阵中多余出来的自由度将会主要由噪声决定。为了避免退化现象造成的影响，通常会同时估计出基础矩阵 F F F 和单应矩阵 H H H，然后选择重投影误差比较小的那个作为最终的运动估计矩阵。
7.4 实践：对极约束求解相机运动
mkdir pose_estimation_2d2d
cd pose_estimation_2d2d
code .
//launch.json
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "g++ - 生成和调试活动文件",
"type": "cppdbg",
"request":"launch",
"program":"${workspaceFolder}/build/pose_estimation_2d2d",
"args": [],
"stopAtEntry": false,
"cwd": "${workspaceFolder}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"setupCommands": [
{
"description": "为 gdb 启动整齐打印",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
],
"preLaunchTask": "Build",
"miDebuggerPath": "/usr/bin/gdb"
}
]
}
//tasks.json
{
"version": "2.0.0",
"options":{
"cwd": "${workspaceFolder}/build" //指明在哪个文件夹下做下面这些指令
},
"tasks": [
{
"type": "shell",
"label": "cmake", //label就是这个task的名字,这个task的名字叫cmake
"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
"args":[
".."
]
},
{
"label": "make", //这个task的名字叫make
"group": {
"kind": "build",
"isDefault": true
},
"command": "make", //这个task要执行的任务是make
"args": [
]
},
{
"label": "Build",
"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
"dependsOn":[ //这个label依赖于上面两个label
"cmake",
"make"
]
}
]
}
#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)
project(ORBCV)
#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
set(CMAKE_CXX_FLAGS "-std=c++11")
#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)
#此工程要调用opencv库，因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(pose_estimation_2d2d pose_estimation_2d2d.cpp)
#链接OpenCV库
target_link_libraries(pose_estimation_2d2d ${OpenCV_LIBS})
#include detector = FeatureDetector::create ( "ORB" );
// Ptr descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> match;
//BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1, descriptors_2, match);
//-- 第四步:匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
for (int i = 0; i < descriptors_1.rows; i++) {
if (match[i].distance <= max(2 * min_dist, 30.0)) {
matches.push_back(match[i]);
}
}
}
//估计两张图像间运动
void pose_estimation_2d2d(
std::vector<KeyPoint> keypoints_1,
std::vector<KeyPoint> keypoints_2,
std::vector<DMatch> matches,
Mat &R, Mat &t
)
{
//相机内参，TUM Freiburg2
Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
//---把匹配点转换为vector的形式
vector<Point2f> points1;
vector<Point2f> points2;
for(int i=0; i< (int)matches.size(); ++i)
{
points1.push_back(keypoints_1[matches[i].queryIdx].pt);
points2.push_back(keypoints_2[matches[i].trainIdx].pt);
}
//---计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT); //采用8点法求解F = K(-T) * E * K(-1)
cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;//输出基础矩阵
//---计算本质矩阵
Point2d principal_point(325.1,249.7); //相机光心，TUM dataset标定值
double focal_length = 521; //相机焦距，TUM dataset标定值
Mat essential_matrix;
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
cout << "essential_matrix is " << endl << essential_matrix << endl;
//---计算单应矩阵
//---但是本例中场景不是平面，单应矩阵意义不大
Mat homography_matrix;
homography_matrix = findHomography(points1, points2, RANSAC, 3);
cout << "homography_matrix is " << endl << homography_matrix << endl;
//---从本质矩阵中恢复旋转和平移矩阵
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
cout << "R is" << endl << R << endl;
cout << "t is" << endl << t << endl;
}
运行结果：
[ INFO:0] Initialize OpenCL runtime...
-- Max dist : 95.000000
-- Min dist : 7.000000
一共找到了81组匹配点
fundamental_matrix is
[5.435453065936354e-06, 0.0001366043242989653, -0.02140890086948144;
-0.0001321142229824715, 2.339475702778067e-05, -0.006332906454396007;
0.02107630352202796, -0.003666833952953114, 0.9999999999999999]
essential_matrix is
[0.01724015832721706, 0.328054335794133, 0.0473747783144249;
-0.3243229585962962, 0.03292958445202408, -0.6262554366073018;
-0.005885857752320116, 0.6253830041920333, 0.0153167864909267]
homography_matrix is
[0.9131751791807657, -0.1092435315823255, 29.95860009984688;
0.02223560352312278, 0.9826008005062553, 6.508910839573075;
-0.0001001560381022834, 0.000103777943639824, 0.9999999999999999]
R is
[0.9985534106102478, -0.05339308467584758, 0.006345444621108698;
0.05321959721496264, 0.9982715997131746, 0.02492965459802003;
-0.007665548311697523, -0.02455588961730239, 0.9996690690694516]
t is
[-0.8829934995085544;
-0.05539655431450562;
0.4661048182498402]
t^R
[-0.02438126572381045, -0.4639388908753606, -0.06699805400667856;
0.4586619266358499, -0.04656946493536188, 0.8856589319599302;
0.008323859859529846, -0.8844251262060034, -0.0216612071874423]
epipolar constraint = [0.004334754136721797]
epipolar constraint = [-0.0002809243685121809]
epipolar constraint = [-0.001438247945977744]
epipolar constraint = [0.0003269033947393973]
epipolar constraint = [-0.0003553231638489529]
epipolar constraint = [0.001284545296795364]
epipolar constraint = [0.0007111119070243033]
epipolar constraint = [0.0005809963024551446]
epipolar constraint = [-0.0004569505410570683]
epipolar constraint = [-0.0001985091674428126]
epipolar constraint = [0.0009954466629851014]
epipolar constraint = [0.004183444398105557]
epipolar constraint = [0.0003301500278483499]
epipolar constraint = [0.000433468422895756]
epipolar constraint = [0.002166463717508241]
epipolar constraint = [0.0008612142972820036]
epipolar constraint = [0.006260134367574832]
epipolar constraint = [0.007343864270669354]
epipolar constraint = [0.0006997299583792263]
epipolar constraint = [-0.0002735148772005716]
epipolar constraint = [-5.272337012321437e-07]
epipolar constraint = [0.0007372565015377822]
epipolar constraint = [-0.0006697357792934122]
epipolar constraint = [0.003123484301720714]
epipolar constraint = [-0.001231690598807428]
epipolar constraint = [0.0002668695748940936]
epipolar constraint = [0.004005543462373876]
epipolar constraint = [0.0003056705066544624]
epipolar constraint = [-0.003108414268527718]
epipolar constraint = [-1.915351944714594e-06]
epipolar constraint = [0.0006933459945522302]
epipolar constraint = [-1.20842520190817e-05]
epipolar constraint = [0.001931054288970224]
epipolar constraint = [-0.001327411567348349]
epipolar constraint = [-0.001158918062891631]
epipolar constraint = [-0.001858904428330923]
epipolar constraint = [0.0001556314587936695]
epipolar constraint = [-0.002373723544446836]
epipolar constraint = [0.004805889122330598]
epipolar constraint = [-0.0009747832347852675]
epipolar constraint = [0.0006571155616519331]
epipolar constraint = [0.002337204394122716]
epipolar constraint = [0.004765516758509947]
epipolar constraint = [-0.001750870050808317]
epipolar constraint = [0.001092859392827487]
epipolar constraint = [0.0006769492505509164]
epipolar constraint = [0.0003307429644405918]
epipolar constraint = [0.001876564994448115]
epipolar constraint = [0.001832354276950641]
epipolar constraint = [7.744615696386736e-07]
epipolar constraint = [-0.0007964236477854686]
epipolar constraint = [0.0001236409258935089]
epipolar constraint = [1.386843227244028e-06]
epipolar constraint = [0.0001450355326295741]
epipolar constraint = [0.00109731208246224]
epipolar constraint = [-0.002227053499071974]
epipolar constraint = [0.002240603253167585]
epipolar constraint = [0.0002359213388878484]
epipolar constraint = [0.003035338951631217]
epipolar constraint = [0.002650788007581513]
epipolar constraint = [-0.0002311129052587763]
epipolar constraint = [0.003158726652554816]
epipolar constraint = [0.00318443380492206]
epipolar constraint = [-0.0003030993775385848]
epipolar constraint = [-0.004151424678597325]
epipolar constraint = [0.000962908812924268]
epipolar constraint = [0.00166825104315349]
epipolar constraint = [-0.001953002815440141]
epipolar constraint = [-0.0024677215713114]
epipolar constraint = [0.0008218799177506786]
epipolar constraint = [1.36931688559278e-06]
epipolar constraint = [-0.001891259735584946]
epipolar constraint = [-0.0001304333417016107]
epipolar constraint = [0.0002565573533065552]
epipolar constraint = [0.006010380518284446]
epipolar constraint = [-0.0004837802950156331]
epipolar constraint = [0.00547529124350349]
epipolar constraint = [0.001722398975726524]
epipolar constraint = [-0.0004945848903547823]
epipolar constraint = [-0.004259380631293275]
epipolar constraint = [-0.0007738612238716719]
根据前面的讨论，分解得到的 R , t R,t R,t 一共有 4 种可能性。不过 OpenCV 会检测角点的深度是否为正从而选择出正确的解。
尺度不确定性
对 t 长度进行归一化，直接导致了单目视觉的尺度不确定性。程序中输出的 t 的第一维约为 -0.882。这个-0.882究竟是指0.882米还是0.822厘米，是无法确定的。因为 t 乘以任意比例常数后，对极约束依然是成立的。换言之，在单目 SLAM 中，对轨迹和地图同时缩放任意倍数得到的图像依然是一样的。
在单目视觉中，对两张图像的 t 进行归一化相当于固定了尺度。虽然不知道它的实际长度是多少，但这时的 t 为单位1，计算相机运动和特征点的 3D 位置。这被称为单目 SLAM 的初始化。在初始化之后，就可以用3D-2D计算相机运动了。初始化之后的轨迹和地图的单位，就是初始化时固定的尺度。因此，单目SLAM有一步不可避免的初始化。初始化的两张图片必须有一定程度的平移，而后的轨迹都将以此步的平移为单位。
除了对 t 进行归一化，另一种方法是令初始化时所有的特征点平均深度为1，也可以固定一个尺度。相比于令t长度为1的做法，把特征点深度归一化可以控制场景的规模大小，使计算数值上更稳定。
初始化的纯旋转问题
从 E E E 分解得到 R , t R,t R,t 的过程中，如果相机发生了纯旋转，导致 t t t 为零，那么，得到的 E E E也将为零，这会导致无法求解 R R R。单目初始化时不能只有纯旋转，必须要有一定程度的平移。在实践中，如果初始化时平移太小，会使得位姿求解与三角化结果不稳定，从而导致失败。有经验的SLAM研究人员，在单目SLAM情况下经常选择让相机进行左右平移以顺利地进行初始化。
多于8点的情况
当给定的点数多于 8 对时，可以计算一个最小二乘解。
如下方程：
( u 1 1 u 2 1 u 1 1 v 2 1 u 1 1 v 1 1 u 2 1 v 1 1 v 2 1 v 1 1 u 2 1 v 2 1 1 u 1 2 u 2 2 u 1 2 v 2 2 u 1 2 v 1 2 u 2 2 v 1 2 v 2 2 v 1 2 u 2 2 v 2 2 1 ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ u 1 8 u 2 8 u 1 8 v 2 8 u 1 8 v 1 8 u 2 8 v 1 8 v 2 8 v 1 8 u 2 8 v 2 8 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) = 0 \left(\begin{array}{ccccccccc} u_{1}^{1} u_{2}^{1} & u_{1}^{1} v_{2}^{1} & u_{1}^{1} & v_{1}^{1} u_{2}^{1} & v_{1}^{1} v_{2}^{1} & v_{1}^{1} & u_{2}^{1} & v_{2}^{1} & 1 \\ u_{1}^{2} u_{2}^{2} & u_{1}^{2} v_{2}^{2} & u_{1}^{2} & v_{1}^{2} u_{2}^{2} & v_{1}^{2} v_{2}^{2} & v_{1}^{2} & u_{2}^{2} & v_{2}^{2} & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ u_{1}^{8} u_{2}^{8} & u_{1}^{8} v_{2}^{8} & u_{1}^{8} & v_{1}^{8} u_{2}^{8} & v_{1}^{8} v_{2}^{8} & v_{1}^{8} & u_{2}^{8} & v_{2}^{8} & 1 \end{array}\right)\left(\begin{array}{c} e_{1} \\ e_{2} \\ e_{3} \\ e_{4} \\ e_{5} \\ e_{6} \\ e_{7} \\ e_{8} \\ e_{9} \end{array}\right)=0 ⎝⎜⎜⎜⎛u11u21u12u22⋮u18u28u11v21u12v22⋮u18v28u11u12⋮u18v11u21v12u22⋮v18u28v11v21v12v22⋮v18v28v11v12⋮v18u21u22⋮u28v21v22⋮v28111⎠⎟⎟⎟⎞⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛e1e2e3e4e5e6e7e8e9⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞=0
把左侧的系数矩阵记作 A A A，对于八点法， A A A 的大小为 8 * 9。如果给定的匹配点多于8，则该方程构成一个超定方程，即不一定存在 e e e 使得上式成立。因此，可以通过最小化一个二次型来求：
min e ∥ A e ∥ 2 2 = min e e T A T A e \min _{e}\|A e\|_{2}^{2}=\min _{e} e^{T} A^{T} A e emin∥Ae∥22=emineTATAe
于是就求出了在最小二乘意义下的 E E E 矩阵。
不过，当可能存在误匹配的情况下，会更倾向于使用随机采样一致性(Random Sample Concensus, RANSAC)来求，而不是最小二乘。
RANSAC是一种通用的方法，适用于很多带错误数据的情况，可以处理带有错误匹配的数据。