SLAM|【原创】SLAM学习笔记（三）视觉里程计-本质矩阵计算机视觉|SLAM

【SLAM|【原创】SLAM学习笔记（三）视觉里程计-本质矩阵】
文章目录

本质矩阵
八点法

本质矩阵根据定义，本质矩阵 E E E是一个3X3矩阵，有9个未知数，其有如下特征：

本质矩阵是有对极约束定义的
根据 E = t ^ R E=t\hat{}R E=t^R证得本质矩阵E的奇异值必定是 [ σ σ 0 ] T [\sigma \sigma 0]^T [σσ0]T的形式，这成为本质矩阵的内在性质
由于平移和旋转各有3个自由度（共6个自由度），又有尺度等价性，故 E E E有5个自由度

八点法由于 E E E有5个自由度，所以通常由8点法来估计 E E E，这就是8点法，设 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为一对匹配归一化坐标点，根据对极约束有
( 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}\Bigg) =0 (u1?,v1?,1)(e1?e4?e7??e2?e5?e8??e3?e6?e9??)(u2?v2?1?)=0
把矩阵E展开，写成向量形式，
e = ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) T e=(\begin{matrix} e_1&e_2&e_3&e_4&e_5&e_6&e_7&e_8&e_9 \end{matrix})^T e=(e1??e2??e3??e4??e5??e6??e7??e8??e9??)T
因此对极约束可以写成与e有关的线性形式
( u 2 v 1 u 2 v 1 u 2 u 2 v 1 u 2 v 1 v 2 v 1 1 ) e = 0 (\begin{matrix} u_2v_1&u_2v_1&u_2&u_2v_1&u_2v_1&v_2&v_1&1 \end{matrix})e=0 (u2?v1??u2?v1??u2??u2?v1??u2?v1??v2??v1??1?)e=0
对于其他点也有相同的表示，把所有点放到一个方程中，变为线性方程组：
( u 2 1 v 1 1 u 2 1 v 1 1 u 2 1 u 2 1 v 1 1 u 2 1 v 1 1 v 2 1 v 1 1 1 u 2 2 v 1 2 u 2 2 v 1 2 u 2 2 u 2 2 v 1 2 u 2 2 v 1 2 v 2 2 v 1 2 1 . . . . . . . . . . . . . . . . . . u 2 8 v 1 8 u 2 8 v 1 8 u 2 8 u 2 8 v 1 8 u 2 8 v 1 8 v 2 8 v 1 8 1 ) ( e 1 e 2 . . . . . . e 9 ) = 0 ( 矩阵 ) \Bigg(\begin{matrix} u_2^1v_1^1&u_2^1v_1^1&u_2^1&u_2^1v_1^1&u_2^1v_1^1&v_2^1&v_1^1&1 \\u_2^2v_1^2&u_2^2v_1^2&u_2^2&u_2^2v_1^2&u_2^2v_1^2&v_2^2&v_1^2&1 \\...&&&&...&&&...\\...&&&&...&&&...\\u_2^8v_1^8&u_2^8v_1^8&u_2^8&u_2^8v_1^8&u_2^8v_1^8&v_2^8&v_1^8&1 \\ \end{matrix}\Bigg)\Bigg(\begin{matrix}e_1\\e_2\\...\\...\\e_9 \end{matrix}\Bigg)=0(矩阵) (u21?v11?u22?v12?......u28?v18??u21?v11?u22?v12?u28?v18??u21?u22?u28??u21?v11?u22?v12?u28?v18??u21?v11?u22?v12?......u28?v18??v21?v22?v28??v11?v12?v18??11......1?)(e1?e2?......e9??)=0(矩阵)
其系数矩阵由特征点的位置构成大小为8X9，e位于该矩阵的零空间中，如果系数矩阵是满秩的（秩为8），e的各元素可由上述方程求得，根据估得的E恢复出相机运动Rt，这个过程由SVD分解求得。设E的SVD为 E = U D V T E=UDV^T E=UDVT,其中U、V为正交矩阵，D为奇异值对角阵
D = d i a g ( σ , σ , 0 ) D=diag(\sigma,\sigma,0) D=diag(σ,σ,0)在SVD中存在两个可能的tR与其对应
t 1 ^ = U R z ( π 2 ) Σ U T t_1\hat{}=UR_z(\frac\pi2)\Sigma U^T t1?^=URz?(2π?)ΣUT t 2 ^ = U R z ( ? π 2 ) Σ U T t_2\hat{}=UR_z(-\frac\pi2) \Sigma U^T t2?^=URz?(?2π?)ΣUT R 1 = U R z T ( ? π 2 ) Σ V T R_1=UR_z^T(-\frac\pi2)\Sigma V^T R1?=URzT?(?2π?)ΣVT R 2 = U R z T ( π 2 ) Σ V T R_2=UR_z^T(\frac\pi2) \Sigma V^T R2?=URzT?(2π?)ΣVT
R z ( π 2 ) = ( 0 ? 1 0 1 0 0 0 0 0 ) R_z(\frac\pi2)=\Bigg(\begin{matrix}0&-1&0\\1&0&0\\0&0&0 \end{matrix}\Bigg) Rz?(2π?)=(010??100?000?)表示沿Z轴旋转90度的矩阵
实际计算中 D = d i a g ( σ 1 , σ 2 , σ 3 ) D=diag(\sigma_1,\sigma_2,\sigma_3) D=diag(σ1?,σ2?,σ3?) 设 σ 1 ≥ σ 2 ≥ σ 3 \sigma_1\geq \sigma_2\geq \sigma_3 σ1?≥σ2?≥σ3?,舍弃第三个并且用 σ 1 \sigma_1 σ1?, σ 2 \sigma_2 σ2?均值代入计算
即：
E = U d i a g ( σ 1 + σ 2 2 , σ 1 + σ 2 2 , 0 ) V T E=Udiag(\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