本篇详细推导对极几何中，本质矩阵的计算。
根据上篇的内容，由对极几何约束获得相邻帧相对位姿与匹配点坐标的关系：
P
2
T
t
∧
R
P
1
=
0
p
2
T
K
−
T
t
∧
R
K
−
1
p
1
=
0
P_2^Tt^\land RP_1 = 0 \\ \quad \\ p_2^TK^{-T}t^\land R K^{-1}p_1 = 0
P2Tt∧RP1=0p2TK−Tt∧RK−1p1=0
①对于本质矩阵 E = t ∧ R E=t^\land R E=t∧R，原本有旋转+位移六个自由度，但是等式右边为零，乘以任意尺度等式都成立，因此方程缺少尺度约束，减去一个自由度，共五个自由度。
②本质矩阵有一个内在性质：奇异值必为 σ , σ , 0 {\sigma,\sigma,0} σ,σ,0的形式。
性质②需要进行证明：
首先，对于奇数阶反对称矩阵
A
A
A，必不满秩：
A
T
=
−
A
∣
A
T
∣
=
∣
−
A
∣
∣
A
∣
=
(
−
1
)
n
∣
A
∣
=
−
∣
A
∣
→
∣
A
∣
=
0
A^T=-A \\ |A^T|=|-A|\\ |A|=(-1)^n|A|=-|A| \to |A|=0
AT=−A∣AT∣=∣−A∣∣A∣=(−1)n∣A∣=−∣A∣→∣A∣=0
任意
n
n
n阶反对称矩阵合同于矩阵
d
i
a
g
{
[
0
1
−
1
0
]
,
[
0
1
−
1
0
]
,
…
[
0
1
−
1
0
]
,
0
,
0
,
…
,
0
}
diag\{\begin{bmatrix}0 & 1 \\ -1 & 0 \end{bmatrix} , \begin{bmatrix}0 & 1 \\ -1 & 0 \end{bmatrix}, \dots \begin{bmatrix}0 & 1 \\ -1 & 0 \end{bmatrix}, 0, 0 ,\dots , 0 \}
diag{[0−110],[0−110],…[0−110],0,0,…,0}
并正交相似于
d
i
a
g
{
[
0
a
1
−
a
1
0
]
,
[
0
a
2
−
a
2
0
]
,
…
[
0
a
r
−
a
r
0
]
,
0
,
0
,
…
,
0
}
diag\{\begin{bmatrix}0 & a_1 \\ -a_1 & 0 \end{bmatrix} , \begin{bmatrix}0 & a_2 \\ -a_2 & 0 \end{bmatrix}, \dots \begin{bmatrix}0 & a_r \\ -a_r & 0 \end{bmatrix}, 0, 0 ,\dots , 0 \}
diag{[0−a1a10],[0−a2a20],…[0−arar0],0,0,…,0}
因此，反对称矩阵的秩必为偶数，则三阶非零反对称矩阵
A
A
A的秩必为2，可将
A
A
A分解：
A
=
U
[
0
a
1
0
−
a
1
0
0
0
0
0
]
U
T
,
U
U
T
=
E
Z
=
[
0
1
0
−
1
0
0
0
0
0
]
W
=
[
0
1
0
−
1
0
0
0
0
1
]
,
W
W
T
=
E
Z
=
W
d
i
a
g
{
1
,
1
,
0
}
A
=
a
1
U
Z
U
T
=
a
1
U
W
d
i
a
g
{
1
,
1
,
0
}
U
T
注
意
，
当
Z
=
W
T
d
i
a
g
{
−
1
,
−
1
,
0
}
(
1
)
A
=
−
a
1
U
Z
U
T
=
a
2
U
W
T
d
i
a
g
{
1
,
1
,
0
}
U
T
也
满
足
分
解
要
求
A = U \begin{bmatrix}0 & a_1 & 0 \\ -a_1 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix}U^T,UU^T=E \\ \quad \\ Z= \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 0\\ \end{bmatrix} \\ \quad \\ W = \begin{bmatrix} 0&1&0 \\ -1&0&0\\ 0&0&1\\ \end{bmatrix},WW^T=E \\ \quad \\ Z=W diag\{1,1,0 \} \\ \quad \\ A=a_1UZU^T=a_1UWdiag\{1,1,0 \}U^T \\ \quad \\ 注意，当Z=W^Tdiag\{-1,-1,0\} \quad (1) \\ \quad \\ A=-a_1UZU^T=a_2UW^Tdiag\{1,1,0 \}U^T也满足分解要求
A=U⎣⎡0−a10a100000⎦⎤UT,UUT=EZ=⎣⎡0−10100000⎦⎤W=⎣⎡0−10100001⎦⎤,WWT=EZ=Wdiag{1,1,0}A=a1UZUT=a1UWdiag{1,1,0}UT注意，当Z=WTdiag{−1,−1,0}(1)A=−a1UZUT=a2UWTdiag{1,1,0}UT也满足分解要求
由本质矩阵
E
=
t
∧
R
E=t^\land R
E=t∧R，
t
∧
t^\land
t∧是一个反对称矩阵，可得：
t
∧
=
σ
U
W
d
i
a
g
{
1
,
1
,
0
}
U
T
E
=
U
W
d
i
a
g
{
σ
,
σ
,
0
}
U
T
R
∵
U
W
(
U
W
)
T
=
E
,
U
T
R
(
U
T
R
)
T
=
E
U
W
=
U
∗
,
U
T
R
=
(
V
∗
)
T
∴
E
=
U
∗
d
i
a
g
{
σ
,
σ
,
0
}
(
V
∗
)
T
t^\land = \sigma UWdiag\{1,1,0 \}U^T \\ E = UWdiag\{\sigma,\sigma,0 \}U^TR \\ \quad \\ \because UW(UW)^T=E,U^TR(U^TR)^T=E \\ UW=U^*,U^TR=(V^*)^T\\ \therefore E = U^* diag\{\sigma,\sigma,0\}(V^*)^T
t∧=σUWdiag{1,1,0}UTE=UWdiag{σ,σ,0}UTR∵UW(UW)T=E,UTR(UTR)T=EUW=U∗,UTR=(V∗)T∴E=U∗diag{σ,σ,0}(V∗)T
即
E
E
E的SVD分解得到的奇异值为
{
σ
,
σ
,
0
}
\{\sigma,\sigma,0 \}
{σ,σ,0}的形式。
由于本质矩阵自由度为5，因此最少需要五对匹配点就可以计算出 E E E。五点法计算比较复杂，后面有时间我再补充。
如果只考虑本质矩阵的尺度等价性，则
E
E
E具有八个自由度，需要八对匹配点计算出
E
E
E。对于一对匹配点
P
1
=
{
x
1
,
y
1
,
1
}
,
{
x
2
,
y
2
,
1
}
P_1=\{x_1,y_1,1\},\{x_2,y_2,1\}
P1={x1,y1,1},{x2,y2,1}，由对极几何约束有：
[
x
2
y
2
1
]
[
e
1
e
2
e
3
e
4
e
5
e
6
e
7
e
8
e
9
]
[
x
1
y
1
1
]
=
0
→
[
x
2
x
1
,
x
2
y
1
,
x
2
,
y
2
x
1
,
y
2
y
1
,
y
2
,
x
1
,
y
1
,
1
]
[
e
1
e
2
e
3
e
4
e
5
e
6
e
7
e
8
e
9
]
=
0
\begin{bmatrix} x_2 & y_2 & 1 \end{bmatrix} \begin{bmatrix} e_1 & e_2 & e_3 \\ e_4 & e_5 & e_6 \\ e_7 & e_8 & e_9 \\ \end{bmatrix} \begin{bmatrix} x_1 \\ y_1 \\ 1 \end{bmatrix} = 0 \\ \quad \\ \to \begin{bmatrix} x_2x_1,x_2y_1,x_2,y_2x_1,y_2y_1,y_2,x_1,y_1,1 \end{bmatrix} \begin{bmatrix} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ e_6 \\ e_7 \\ e_8 \\ e_9 \\ \end{bmatrix} = 0 \\ \quad \\
[x2y21]⎣⎡e1e4e7e2e5e8e3e6e9⎦⎤⎣⎡x1y11⎦⎤=0→[x2x1,x2y1,x2,y2x1,y2y1,y2,x1,y1,1]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡e1e2e3e4e5e6e7e8e9⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=0
则对于八对匹配点而言，形成了一个线性齐次方程组 A x = 0 ， A ∈ R n × n Ax=0，A\in {\bf R}^{n\times n} Ax=0，A∈Rn×n。如果 A A A行满秩， d i m N U L ( A ) = 9 − r a n k ( A ) = 1 dimNUL(A)=9-rank(A)=1 dimNUL(A)=9−rank(A)=1，即零空间是一维直线，刚好满足本质矩阵的尺度不确定性。
对于欠定方程组，可以采用SVD来解方程，如数值计算之 最小二乘法（3）最小二乘的矩阵解法所示。
通过SVD解方程，获得本质矩阵
E
E
E后，需要解算位姿
R
,
t
R,t
R,t，根据上面的证明，将
E
E
E进行奇异值分解：
E
=
U
∗
d
i
a
g
{
σ
,
σ
,
0
}
(
V
∗
)
T
=
U
W
{
σ
,
σ
,
0
}
U
T
R
奇
异
值
分
解
可
得
U
∗
,
V
∗
,
σ
U
∗
=
U
W
,
W
=
[
0
1
0
−
1
0
0
0
0
1
]
,
V
∗
=
(
U
T
R
)
T
U
=
U
∗
W
T
R
=
U
∗
W
T
(
V
∗
)
T
t
∧
=
U
∗
{
σ
,
σ
,
0
}
W
(
U
∗
)
T
E = U^* diag\{\sigma,\sigma,0\}(V^*)^T=UW\{\sigma,\sigma,0\} U^TR \\ \quad \\ 奇异值分解可得U^*,V^*,\sigma \\ \quad \\ U^*=UW,W = \begin{bmatrix} 0&1&0 \\ -1&0&0\\ 0&0&1\\ \end{bmatrix}, V^*=(U^TR)^T \\ \quad \\ U=U^*W^T \\ R=U^*W^T(V^*)^T \\ t^\land = U^*\{\sigma,\sigma,0\}W(U^*)^T
E=U∗diag{σ,σ,0}(V∗)T=UW{σ,σ,0}UTR奇异值分解可得U∗,V∗,σU∗=UW,W=⎣⎡0−10100001⎦⎤,V∗=(UTR)TU=U∗WTR=U∗WT(V∗)Tt∧=U∗{σ,σ,0}W(U∗)T
关于《十四讲》中的四种情况，其原因是根据式(1)， W , W T W,W^T W,WT都能满足本质矩阵奇异值为 { σ , σ , 0 } \{\sigma,\sigma,0\} {σ,σ,0}的形式，导致 E E E的分解有两种形式；而通过欠定方程求 E E E时，因为缺少尺度约束，可以得到 E E E和 − E -E −E两种解，因此最后出现了四种 R , t R,t R,t可能。
这时，只要把其中一个匹配点代入四个解确定的相机位姿模型，得到两个相机坐标的深度都为正时，表示该解正确。
①由于本质矩阵缺乏尺度约束，因此求出来的 R , t R,t R,t也是没有尺度的。不过旋转矩阵 R R R在正交群中，自身有一个 ∣ R ∣ = 1 |R|=1 ∣R∣=1的约束； t t t的尺度没有任何约束，因此单目求出的 t t t是没有尺度的。
也就是说，单目给出的 t = 1 t=1 t=1，可能指1m，1cm，10cm；而 t = 2 t=2 t=2，就是对应尺度的2倍。
②因为 t t t没有尺度，因此在单目SLAM中，需要在开始时进行尺度初始化，将初始化两帧之间的 t t t作为之后SLAM的尺度。因此，初始化的两帧不能是纯旋转。
③匹配点多于八组。通常ORB提取和匹配到的点数远多于8组。此时可以先通过RANSAC将误匹配点去除，然后通过线性最小二乘计算最优本质矩阵。
④求出的本质矩阵奇异值不满足 σ , σ , 0 \sigma,\sigma,0 σ,σ,0的形式。假设得到的奇异值为 σ 1 ≥ σ 2 ≥ σ 3 \sigma_1\ge \sigma_2\ge \sigma_3 σ1≥σ2≥σ3，可以将奇异值调整为 ( σ 1 + σ 2 ) / 2 , ( σ 1 − σ 2 ) / 2 , 0 (\sigma_1+\sigma_2)/2,(\sigma_1-\sigma_2)/2,0 (σ1+σ2)/2,(σ1−σ2)/2,0，或者直接设定为 1 , 1 , 0 1,1,0 1,1,0的形式。
下篇记录单应矩阵。
