对极几何约束
前言
一、理论推导
这部分将讲解计算帧与帧之间相机相应位姿及特征点三维坐标。
M M M为观测对象，定义 C C C为左视图的相机中心， C ′ C' C′为右视图的相机中心，由 M C C ′ MCC' MCC′构成一个 π \pi π平面；其中 C C ′ CC' CC′为基线， l 1 l_1 l1为左视图的极线， l 2 l_2 l2为右视图的极线， e 1 e_1 e1为基线与左视图的交点，称为极点，同理 e 2 e_2 e2为右视图的极点；
- 找 M 1 与 M 2 M_1与M_2 M1与M2之间关系
设左右视图的投影矩阵分别为
P
P
P与
P
′
P'
P′，内参矩阵为
K
K
K与
K
′
K'
K′，左视图坐标系为世界坐标系。
R
、
t
R、t
R、t分别为左视图坐标系到右视图坐标系的旋转矩阵和平移向量，
M
M
M在左视图投影为
M
1
M_1
M1，在右视图投影为
M
2
M_2
M2，齐次坐标为$\hat{M_1}、\hat{M_2} $所以有：
M
1
^
∼
K
[
I
∣
0
]
M
^
\hat{\boldsymbol M_1} \sim \boldsymbol K[\boldsymbol I|\boldsymbol 0]\hat{\boldsymbol M}
M1^∼K[I∣0]M^
M 2 ^ ∼ K ′ [ R ∣ t ] M ^ \hat{\boldsymbol M_2} \sim \boldsymbol K'[\boldsymbol R|\boldsymbol t]\hat{\boldsymbol M} M2^∼K′[R∣t]M^
由于，
C
C
C是相机中心，
M
1
M
M_1M
M1M直线上所有点在左视图的投影都为
M
1
M_1
M1，
P
P
P是一个3*4的矩阵，
r
a
n
k
(
P
)
=
3
rank(P)=3
rank(P)=3，具有广义逆，记作
P
+
P^+
P+，
P
+
=
P
T
(
P
P
T
)
−
1
P^+=P^T(PP^T)^{-1}
P+=PT(PPT)−1, 所以：
M
^
=
s
P
+
M
1
^
+
C
\hat{\boldsymbol M}=s \boldsymbol P^+\hat{\boldsymbol M_1}+\boldsymbol C
M^=sP+M1^+C
P M ^ = s M 1 ^ \boldsymbol P\hat{\boldsymbol M}=s\hat{\boldsymbol M_1} PM^=sM1^
同理，设
H
=
P
′
P
+
H=P'P^+
H=P′P+
M
2
^
∼
P
′
M
^
=
s
P
′
P
+
M
1
^
+
P
′
C
=
s
H
M
1
^
+
e
2
\hat{\boldsymbol M_2}\sim \boldsymbol P'\hat{\boldsymbol M}=s\boldsymbol P'\boldsymbol P^+\hat{\boldsymbol M_1}+\boldsymbol P'\boldsymbol C=s\boldsymbol H\hat{\boldsymbol M_1}+\boldsymbol e_2
M2^∼P′M^=sP′P+M1^+P′C=sHM1^+e2
即找到
M
1
与
M
2
M_1与M_2
M1与M2之间的关系；
- 几何约束
M
1
和
M
2
M_1和M_2
M1和M2对应的极线为
l
1
l_1
l1和
l
2
l_2
l2，
e
2
e_2
e2和
M
2
M_2
M2经过极线
l
2
l_2
l2，所以：
M
1
^
T
l
1
=
0
\hat{\boldsymbol M_1}^T\boldsymbol l_1=0
M1^Tl1=0
M 2 ^ T l 2 = 0 \hat{\boldsymbol M_2}^T\boldsymbol l_2=0 M2^Tl2=0
l 2 = e 2 × M 2 ^ \boldsymbol l_2=\boldsymbol e_2\times \hat{\boldsymbol M_2} l2=e2×M2^
- 联立
联立可以得到：
l
2
=
e
2
×
(
s
H
M
1
^
+
e
2
)
=
s
e
2
×
H
M
1
^
\boldsymbol l_2=\boldsymbol e_2\times(s\boldsymbol H\hat{\boldsymbol M_1}+\boldsymbol e_2)=s\boldsymbol e_2\times \boldsymbol H\hat{\boldsymbol M_1}
l2=e2×(sHM1^+e2)=se2×HM1^
同时左乘
M
2
^
\hat{M_2}
M2^可以得到：
0
=
M
2
^
T
e
2
×
H
M
1
^
0=\hat{\boldsymbol M_2}^T\boldsymbol e_2\times \boldsymbol H\hat{\boldsymbol M_1}
0=M2^Te2×HM1^
令
F
=
e
2
×
H
F=e_2\times H
F=e2×H，则
M
2
^
T
F
M
1
^
=
0
\hat{\boldsymbol M_2}^T\boldsymbol F\hat{\boldsymbol M_1}=0
M2^TFM1^=0
- 基本矩阵的性质及8点法求解 F F F
F F F是自由度为7，秩为2的矩阵。
M 2 ^ T F M 1 ^ = 0 \hat{M_2}^TF\hat{M_1}=0 M2^TFM1^=0，所以根据匹配到的特征点，利用最小二乘进行结算，即可得到相应的基本矩阵，但是由于最小二乘计算过程中并没有添加基本矩阵所要的约束条件，所以得到的解可能不满足性质，因此需要进一步修正。
- 优化基本矩阵
为了满足条件，使用SVD分解来求解优化问题：
a
r
g
m
i
n
∣
∣
F
−
F
∗
∣
∣
,
s
.
t
.
s
v
d
(
F
)
=
[
σ
1
,
σ
2
,
0
]
arg\ min||\boldsymbol F-\boldsymbol F^*||,s.t. svd(\boldsymbol F)=[\sigma_1,\sigma_2,0]
arg min∣∣F−F∗∣∣,s.t.svd(F)=[σ1,σ2,0]
使用SVD分解得到
F
∗
F^*
F∗的奇异值，令其最小值为零，再代入求解新的
F
F
F，即为满足要求的基本矩阵。
当我们选用的点数>8时，我们需用RANSAC方法来进行优化。
- RANSAC-随机一致性采样算法
/*
1、设定内点判断标准，随机采样8对匹配点；
2、利用8点法求解初始基础矩阵F；
3、优化初始基础急诊；
4、计算误差，并统计内点个数；
5、重复上述过程，至循环次数，并选择内点数最多的结果；
6、利用内点次数最多的结果所有内点重新估计参数。
*/
RANSAC采样次数的计算：
M
=
M
=
log
（
1
−
p
K
）
（
1
−
z
）
=
l
g
(
1
−
z
)
l
g
(
1
−
p
K
)
M =M=\log_{（1-p^K）}{（1-z）} = \frac{lg(1-z)}{lg(1-p^K)}
M=M=log（1−pK）（1−z）=lg(1−pK)lg(1−z)
N
N
N：样本点数；
K K K：求解模型需要最少的点的个数；
p p p：内点的概率；
p K p^K pK： K K K个点都是内点的概率；
1 − p K 1-p^K 1−pK：K个点至少有一个外点的概率；
z = 1 − ( 1 − p K ) M z=1-(1-p^K)^M z=1−(1−pK)M： M M M次采样至少有1次成功的概率。
内点评判标准（ x 1 x_1 x1为左视图投影点， x 2 x_2 x2为右视图投影点）：
1、一阶几何误差，又称Sampson distance。
d
(
x
1
,
x
2
)
=
(
x
2
T
F
x
1
)
2
(
F
x
1
)
x
2
+
(
F
x
1
)
y
2
+
(
x
2
T
F
)
x
2
+
(
x
2
T
F
)
y
2
d\left(\boldsymbol{x}_{1}, \boldsymbol{x}_{2}\right)=\frac{\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F} \boldsymbol{x}_{1}\right)^{2}}{\left(\boldsymbol{F} \boldsymbol{x}_{1}\right)_{x}^{2}+\left(\boldsymbol{F} \boldsymbol{x}_{1}\right)_{y}^{2}+\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F}\right)_{x}^{2}+\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F}\right)_{y}^{2}}
d(x1,x2)=(Fx1)x2+(Fx1)y2+(x2TF)x2+(x2TF)y2(x2TFx1)2
对该公式进行说明：
F
(
1
,
:
)
F(1,:)
F(1,:)与
x
1
x_1
x1对应相乘，
F
(
2
,
:
)
F(2,:)
F(2,:)与
x
1
x_1
x1对应相乘，
F
(
:
,
1
)
F(:,1)
F(:,1)与
x
2
x_2
x2对应相乘，
F
(
:
,
2
)
F(:,2)
F(:,2)与
x
2
x_2
x2对应相乘。
2、对称极线距离
d
(
x
1
,
x
2
)
=
(
x
2
T
F
x
1
)
2
(
F
x
1
)
x
2
+
(
F
x
1
)
y
2
+
(
x
2
T
F
x
1
)
2
(
x
2
T
F
)
x
2
+
(
x
2
T
F
)
y
2
d\left(\boldsymbol{x}_{1}, \boldsymbol{x}_{2}\right)=\frac{\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F} \boldsymbol{x}_{1}\right)^{2}}{\left(\boldsymbol{F} \boldsymbol{x}_{1}\right)_{x}^{2}+\left(\boldsymbol{F} \boldsymbol{x}_{1}\right)_{y}^{2}}+\frac{\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F} \boldsymbol{x}_{1}\right)^{2}}{\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F}\right)_{x}^{2}+\left(\boldsymbol{x}_{2}^{T} \boldsymbol{F}\right)_{y}^{2}}
d(x1,x2)=(Fx1)x2+(Fx1)y2(x2TFx1)2+(x2TF)x2+(x2TF)y2(x2TFx1)2
-
*用所有内点进行非线性优化– L M LM LM（选用）
-
计算本质矩阵 E E E
F = e 2 × H = K ′ t × P ′ P = K ′ T t × R K − 1 \boldsymbol F=\boldsymbol e_2\times \boldsymbol H=\boldsymbol K'\boldsymbol t\times \boldsymbol P'\boldsymbol P=\boldsymbol K'^T\boldsymbol t\times \boldsymbol R\boldsymbol K^{-1} F=e2×H=K′t×P′P=K′Tt×RK−1
式中，
E
=
t
×
R
E=t \times R
E=t×R即本质矩阵，所以：
E
=
K
′
T
F
K
\boldsymbol E=\boldsymbol K'^T\boldsymbol F\boldsymbol K
E=K′TFK
本质矩阵性质：当且仅当它的两个奇异值相等且第三个奇异值为零；所以，求解
F
F
F同理，利用SVD分解来重构
E
E
E，不同的是：
E
=
U
d
i
a
g
(
σ
1
+
σ
2
2
,
σ
1
+
σ
2
2
,
0
)
V
T
\boldsymbol E=\boldsymbol Udiag(\frac{\sigma_1+\sigma_2}{2},\frac{\sigma_1+\sigma_2}{2},0)\boldsymbol V^T
E=Udiag(2σ1+σ2,2σ1+σ2,0)VT
- 单应矩阵
当场景中的特征点都落在同一个平面时，通过单应性进行估计。设距光心为
d
d
d平面，特征点
X
\boldsymbol X
X在两个视图上对应点
x
1
\boldsymbol x_1
x1、
x
2
\boldsymbol x_2
x2满足：
n
T
X
+
d
=
0
\boldsymbol n^T \boldsymbol X+d=0
nTX+d=0
− n T X d = 1 -\frac{\boldsymbol n^T \boldsymbol X}{d}=1 −dnTX=1
x 2 ∼ K ′ ( R X + t ) x_2 \sim \boldsymbol K'(\boldsymbol R\boldsymbol X+\boldsymbol t) x2∼K′(RX+t)
x 2 ∼ K ′ ( R X + t ( − n T X d ) ) \boldsymbol x_2\sim \boldsymbol K'(\boldsymbol R \boldsymbol X + \boldsymbol t(-\frac{\boldsymbol n^T \boldsymbol X}{d})) x2∼K′(RX+t(−dnTX))
x 2 ∼ K ( R − t n T d ) X \boldsymbol x_2 \sim \boldsymbol K(\boldsymbol R-\boldsymbol t\frac{\boldsymbol n^T}{d})\boldsymbol X x2∼K(R−tdnT)X
x 2 ∼ K ( R − t n T d ) K − 1 x 1 \boldsymbol x_2 \sim\boldsymbol K(\boldsymbol R-\boldsymbol t\frac{\boldsymbol n^T}{d})\boldsymbol K^{-1}\boldsymbol x_1 x2∼K(R−tdnT)K−1x1
令
K
(
R
−
t
n
T
d
)
K
−
1
=
H
K(\boldsymbol R-\boldsymbol t\frac{\boldsymbol n^T}{d})\boldsymbol K^{-1}=\boldsymbol H
K(R−tdnT)K−1=H，称之为单应性矩阵，得：
x
2
∼
H
x
1
\boldsymbol x_2 \sim \boldsymbol H\boldsymbol x_1
x2∼Hx1
类似于
F
\boldsymbol F
F得求解方法
(
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} \sim \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⎠⎞
即:
s
(
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
)
s\begin{pmatrix}u_2\\v_2\\1\end{pmatrix} \sim \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}
s⎝⎛u2v21⎠⎞∼⎝⎛h1h4h7h2h5h8h3h6h9⎠⎞⎝⎛u1v11⎠⎞
s
s
s是处理齐次性的比例因子，从方程第三行求解
s
s
s带入第一、二行，可得到：
h
1
u
1
+
h
2
v
1
+
h
3
−
h
7
u
1
u
2
−
h
8
v
1
u
2
−
h
9
u
2
=
0
h_1u_1+h_2v_1+h_3-h_7u_1u_2-h_8v_1u_2-h_9u_2=0
h1u1+h2v1+h3−h7u1u2−h8v1u2−h9u2=0
h 4 u 1 + h 5 v 1 + h 6 − h 7 u 1 u 2 − h 8 v 1 v 2 − h 9 v 2 = 0 h_4u_1+h_5v_1+h_6-h_7u_1u_2-h_8v_1v_2-h_9v_2=0 h4u1+h5v1+h6−h7u1u2−h8v1v2−h9v2=0
A i = ( u 1 v 1 1 0 0 0 − u 1 u 2 − v 1 u 2 − u 2 0 0 0 u 1 v 1 1 − u 1 v 2 − v 1 v 2 − v 2 ) \boldsymbol A_i=\begin{pmatrix} u1 & v1 & 1 & 0 & 0 & 0 & -u_1u_2 & -v_1u_2 & -u_2\\ 0 & 0 & 0 & u_1 & v_1 & 1 & -u_1v_2 & -v_1v_2 &-v_2 \end{pmatrix} Ai=(u10v10100u10v101−u1u2−u1v2−v1u2−v1v2−u2−v2)
h T = ( h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ) \boldsymbol h^T = \begin{pmatrix} h_1 & h_2 & h_3 & h_4 & h_5 & h_6 & h_7 & h_8 &h_9 \end{pmatrix} hT=(h1h2h3h4h5h6h7h8h9)
得到两个约束，单应性矩阵自由度为8（ h 9 = 1 h_9=1 h9=1），则至少需要4对匹配点进行求解。 A h = 0 \boldsymbol A\boldsymbol h=\boldsymbol 0 Ah=0，求 A \boldsymbol A A为0特征值的特征向量，或通过 S V D \boldsymbol {SVD} SVD分解求得 A \boldsymbol A A的一维零子空间。
在工程应用时，我们会倾向于同时计算基础矩阵和单应性矩阵，从而选择从投影误差最小的作为估计相机运动的矩阵。
- 帧与帧之间相机相应位姿的求解
根据前面，我们知道
E
=
t
×
R
\boldsymbol E=\boldsymbol t \times \boldsymbol R
E=t×R，
R
\boldsymbol R
R、
t
\boldsymbol t
t由
E
\boldsymbol E
E的奇异值分解（SVD）得到，设
E
\boldsymbol E
E的SVD为：
E
=
U
Σ
V
T
\boldsymbol E=\boldsymbol U\Sigma \boldsymbol V^T
E=UΣVT
在奇异值分解中，对于任意一个
E
\boldsymbol E
E，存在两个可能的
t
\boldsymbol t
t，
R
\boldsymbol R
R与之对应：
t
1
=
U
(
:
,
2
)
t
2
=
−
U
(
:
,
2
)
\boldsymbol{t}_{1}=\boldsymbol{U}(:, 2) \ \ \ \ \ \ \ \ \ \ \boldsymbol{t}_{2}=-\boldsymbol{U}(:, 2)
t1=U(:,2) t2=−U(:,2)
R 1 = U R z ( π 2 ) V T R 2 = U R z T ( π 2 ) V T \boldsymbol R_1=\boldsymbol U\boldsymbol R_z(\frac{\pi}{2})\boldsymbol V^T \ \ \ \ \ \ \boldsymbol R_2=\boldsymbol U\boldsymbol R_z^T(\frac{\pi}{2})\boldsymbol V^T R1=URz(2π)VT R2=URzT(2π)VT
我们所得到的四个解，相机2对应有四种姿态和位移，我们选择P同时满足在两个相机前方。判断方法：
/*
1、利用三角测量（p1, p2, K1, R1, t1, K2, R2, t2），求出四种情况对应的三维坐标；
2、利用R、t，求出投影的(x,y,z);
3、z>0，对两个相机均成立，则正确。
*/
1727
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
