第一部分:相机模型和对极几何
第1节:相机模型
1.针孔相机模型
(1)空间中两个坐标系之间的变化属于刚体变换,可由旋转矩阵
R
R
R和平移向量
t
t
t 描述,则世界坐标系到相机坐标系表示为:
X
c
=
R
X
w
+
t
X_c=RX_w+t
Xc=RXw+t
平移向量
t
t
t的意义是世界坐标系的原点在相机坐标系中的表达;由于旋转矩阵是正交阵,
R
T
=
R
−
1
,
R
T
R
=
E
R^T=R^{-1},R^TR=E
RT=R−1,RTR=E,因此相机坐标系到世界坐标系可由表示为:
X
w
=
R
T
X
c
−
R
T
t
X_w=R^TX_c-R^Tt
Xw=RTXc−RTt
相机中心
O
c
O_c
Oc在世界坐标系的表示为:
X
w
c
=
−
R
T
t
X_w^c=-R^Tt
Xwc=−RTt
相机坐标系的
Z
Z
Z轴在世界坐标系中的表示为旋转矩阵的第三行,
证明:设
r
c
=
Z
c
−
O
c
=
[
0
0
1
]
T
r_c=Z_c -O_c=[0\quad0\quad1]^T
rc=Zc−Oc=[001]T,则
r
w
=
(
R
T
Z
c
−
R
T
t
)
−
(
R
T
O
c
−
R
T
t
)
=
R
T
[
0
0
1
]
r_w=(R^TZ_c-R^Tt)-(R^TO_c-R^Tt)=R^T\begin{bmatrix}0 \\ 0 \\ 1\end{bmatrix}
rw=(RTZc−RTt)−(RTOc−RTt)=RT⎣
⎡001⎦
⎤
(2)相机坐标系到物理像平面坐标系
物理像平面:相机CCD阵列所在的平面。物理像平面与相机坐标系中心的距离为焦距 f f f.
这个过程由三维的点变换到2维,会损失深度信息
[
x
y
1
]
=
1
z
c
[
f
0
0
0
f
0
0
0
1
]
[
x
c
y
c
z
c
]
\begin{bmatrix} x \\ y \\ 1 \end{bmatrix}=\frac{1}{z_{c}}\begin{bmatrix} f & 0 & 0 \\ 0 & f & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{c} \\ y_{c} \\ z_{c} \end{bmatrix}
⎣
⎡xy1⎦
⎤=zc1⎣
⎡f000f0001⎦
⎤⎣
⎡xcyczc⎦
⎤
(3)物理像平面坐标系到图像坐标系
图像坐标系一般以左上角为坐标原点,需要进行离散化和坐标系平移。
d
x
、
d
y
dx、dy
dx、dy表示一个像素的宽和高(mm).
[
u
v
1
]
=
[
1
/
d
x
0
u
0
0
1
/
d
y
v
0
0
0
1
]
[
x
y
1
]
\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=\begin{bmatrix} 1/dx & 0 & u_0 \\ 0 & 1/dy & v_0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x \\ y\\ 1 \end{bmatrix}
⎣
⎡uv1⎦
⎤=⎣
⎡1/dx0001/dy0u0v01⎦
⎤⎣
⎡xy1⎦
⎤
整个成像过程可以描述为:
[
u
v
1
]
=
1
z
c
[
f
/
d
x
0
u
0
0
f
/
d
y
v
0
0
0
1
]
R
[
X
w
Y
w
Z
w
]
+
t
=
1
z
c
K
R
[
X
w
Y
w
Z
w
]
+
t
\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=\frac{1}{z_{c}}\begin{bmatrix} f/dx & 0 & u_0 \\ 0 & f/dy & v_0 \\ 0 & 0 & 1 \end{bmatrix} R\begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w}\\ \end{bmatrix}+t=\frac{1}{z_{c}}K R\begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w}\\ \end{bmatrix}+t
⎣
⎡uv1⎦
⎤=zc1⎣
⎡f/dx000f/dy0u0v01⎦
⎤R⎣
⎡XwYwZw⎦
⎤+t=zc1KR⎣
⎡XwYwZw⎦
⎤+t
设系统矩阵
P
=
K
[
R
t
]
P=K[R\quad t]
P=K[Rt],其中内参为
K
K
K、外参为
[
R
t
]
[R\quad t]
[Rt].
2.畸变模型
相机畸变模型,一般只考虑径向畸变 k k k和切向畸变 p p p,畸变参数也很重要的,也是内参的重要组成。
径向畸变主要由镜头径向曲率产生(光线在远离透镜中心的地方比靠近中心的地方更加弯曲)。导致真实成像点向内或向外偏离理想成像点。径向畸变就是沿着透镜半径方向分布的畸变。
切向畸变是由于透镜本身与相机传感器平面(成像平面)或图像平面不平行而产生的。
径向畸变模型:
x
distorted
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
y
distorted
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
\begin{aligned} &x_{\text {distorted }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \\ &y_{\text {distorted }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \end{aligned}
xdistorted =x(1+k1r2+k2r4+k3r6)ydistorted =y(1+k1r2+k2r4+k3r6)
畸变模型:枕型畸变 ( k > 0 ) (k>0) (k>0),畸点相对于理想像点沿径向向外偏移,远离中心的;
桶型畸变 ( k < 0 ) (k<0) (k<0),畸点相对于理想点沿径向向中心靠拢。
分析:畸变模型与距离成正比,当 k > 0 k>0 k>0时,物点离中心点越远, x distorted x_{\text {distorted }} xdistorted 越大,呈现枕形。
即:当k>0时,r越大(点离中心越远),畸变量越大,r越小,畸变量越小,呈枕型。
当k<0时,r越大(点离中心越远),畸变量越小,r越小,畸变量越大,呈桶型。
切向畸变模型:
x
distorted
=
x
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
y
distorted
=
y
+
2
p
2
x
y
+
p
1
(
r
2
+
2
y
2
)
x_{\text {distorted }}=x+2 p_{1}xy+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {distorted }}=y+2 p_{2}xy+p_{1}\left(r^{2}+2 y^{2}\right)
xdistorted =x+2p1xy+p2(r2+2x2)ydistorted =y+2p2xy+p1(r2+2y2)
其中 r 2 = x 2 + y 2 r^2=x^2+y^2 r2=x2+y2, 因此我们一共需要5个畸变参数 ( k 1 , k 2 , k 3 , p 1 , p 2 ) (k_1,k_2,k_3,p_1,p_2) (k1,k2,k3,p1,p2)描述透镜畸变。
畸变系数求解方法:
提供理想点(针孔相机模型) [ u , v ] T [u,v]^T [u,v]T和畸变点 [ u ′ , v ′ ] T [u',v']^T [u′,v′]T的对应关系,通过最小二乘进行估计.
至此,相机模型分析分析完了
外参 [ R t ] [R\quad t] [Rt]:6个自由度,其中旋转矩阵可由三个欧拉角表示,平移矩阵也由三个平移量表示。外参估计对应计算机视觉中的姿态估计
内参:一般9个,缩放因子 s s s、焦距 f f f、主点坐标 ( u 0 , v 0 ) (u_0,v_0) (u0,v0)、畸变系数 k 1 、 k 2 、 k 3 、 p 1 、 p 2 k_1、k_2、k_3、p_1、p_2 k1、k2、k3、p1、p2。内参的估计对应计算视觉中的相机标定
相机标定的目的:获取畸变系数,对图像进行矫正
内参决定了图像的分辨率和图像的大小,在矫正后,可以人为给定。
第2节:特征检测与匹配
1.特征检测子
Harris角点检测:常用于跟踪,不具有尺度不变性, k k k越大,越灵敏
核心思想:让窗口内的差异足够大, C = d e t ( H ) − k ⋅ t r a c e ( H ) 2 = λ 1 λ 2 − k ( λ 1 + λ 2 ) 2 , k = 0.04 C=det(H)-k\cdot trace(H)^2=\lambda_1\lambda_2-k(\lambda_1+\lambda_2)^2,k=0.04 C=det(H)−k⋅trace(H)2=λ1λ2−k(λ1+λ2)2,k=0.04
SIFT特征点检测:具有尺度、旋转、光照不变性,计算量大
ORB特征点检测:速度快、具有尺度旋转不变性
SuperPoint: Self Supervised Interest Point Detection and Description,深度学习提供极端场景下关键点的提取方法,但需要大量的训练数据才能够得到较好的泛化性
2.描述子:每个关键点的特征
-
SIFT,统计局部梯度信息,生成128维的特征描述子。将区域划分成 4 × 4 4\times4 4×4的block,把16个block的梯度方向的直方图(高斯加权梯度作为系数)并成128个特征。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ceJkxkQy-1662429502487)(images/1_sift.png)]
-
BRIEF,Binary Robust Independent Elementary Features
首先对图像进行平滑处理,然后在特征点周围随机选择 N N N对大小 5 × 5 5\times5 5×5的Patch,比较每对Patch内像素和的大小,生成了一个 N N N长的二进制串。
τ ( p ; x , y ) = { 1 , if p ( x ) < p ( y ) 0 , otherwise \tau(p ; x, y)=\left\{\begin{array}{cc} 1, & \text {if}\quad p(x)<p(y) \\ 0, & \text { otherwise } \end{array}\right. τ(p;x,y)={1,0,ifp(x)<p(y) otherwise
描述向量由𝑁个0或者1组成 𝑁=128,256,512生成速度快,匹配效率高,不具有旋转不变性
-
Steer BRIEF
对N对采样点 S S S,根据特征点的主方向计算旋转 S θ = R θ S S_\theta=R_\theta S Sθ=RθS,在新的采样点上进行BRIEF描述子生成。
3.特征点之间的距离,计算两幅图像中特征描述子的距离
欧式距离、马氏距离、归一化互相关:NCC、汉明距离
4.匹配策略
最近邻搜索:
a
a
a的最近邻是
b
b
b,
b
b
b的最近邻是
a
a
a,立体匹配中的视差一致性
b
⋆
=
arg
min
b
∈
B
D
(
a
,
b
)
,
D
(
a
,
b
⋆
)
<
β
b^{\star}=\arg \min _{b \in B} D(\boldsymbol{a}, \boldsymbol{b}), D\left(a, b^{\star}\right)<\beta
b⋆=argb∈BminD(a,b),D(a,b⋆)<β
最近邻距离比(Lowe-ratio):最近邻距离和次近邻距离的比值小于一个设定的阈值
快速最近邻搜索:哈希表、多维Kd-tree等
第3节:2D-2D:对极几何
1. 对极约束
给定第一幅视图中像点 x x x,怎么约束第二幅视图中对应点 x ′ x' x′的位置?
本质上,两幅视图之间的对极几何是图像平面与以极线为轴的平面束的交的几何。这种几何通常由立体匹配中搜索对应点的问题驱动的。本质矩阵是对极几何的代数表示
基线:左右相机光心的连线
对极平面:空间点,两个相机光心决定的平面
对极点:基线与两图像平面的交点
对极线:对极平面与图像平面交线
设世界原点在 O 1 O_1 O1处,空间点在 O 1 O_1 O1相机坐标系中的坐标为 X X X ,则在 O 2 O_2 O2相机坐标系中的坐标为 R X + t RX+t RX+t
相机坐标系通过内参矩阵
K
K
K可转换到图像坐标系,可得:
x
1
=
1
Z
c
K
1
X
x
2
=
1
Z
c
′
K
2
(
R
X
+
t
)
x_1=\frac{1}{Z_c}K_1X \\ x_2=\frac{1}{Z'_c}K_2(RX+t)
x1=Zc1K1Xx2=Zc′1K2(RX+t)
等式分别左乘
K
1
−
1
、
K
2
−
1
K_1^{-1}、K_2^{-1}
K1−1、K2−1得:
K
1
−
1
x
1
=
1
Z
c
X
K
2
−
1
x
2
=
1
Z
c
′
(
R
X
+
t
)
K_1^{-1}x_1=\frac{1}{Z_c}X \\ K_2^{-1}x_2=\frac{1}{Z'_c}(RX+t)
K1−1x1=Zc1XK2−1x2=Zc′1(RX+t)
消去
X
X
X,得:
Z
c
′
K
2
−
1
x
2
=
R
Z
c
K
1
−
1
x
1
+
t
{Z'_c}K_2^{-1}x_2=RZ_cK_1^{-1}x_1+t
Zc′K2−1x2=RZcK1−1x1+t
乘
t
t
t的反对称矩阵
[
t
]
×
[t]_{\times}
[t]×,相当于
t
×
t
=
0
t\times t=0
t×t=0,消去
t
t
t:
Z
c
′
[
t
]
×
K
2
−
1
x
2
=
Z
c
[
t
]
×
R
K
1
−
1
x
1
{Z'_c}[t]_{\times}K_2^{-1}x_2=Z_c[t]_{\times}RK_1^{-1}x_1
Zc′[t]×K2−1x2=Zc[t]×RK1−1x1
注:设
A
=
(
a
i
j
)
n
×
n
A=\left(a_{i j}\right)_{n \times n}
A=(aij)n×n ,若其中元素满足
a
i
j
=
a
j
i
,
∀
i
,
j
⇔
A
T
=
A
a_{i j}=a_{j i}, \forall i, j \Leftrightarrow A^{T}=A
aij=aji,∀i,j⇔AT=A ,则称A是对称矩阵;
若其元素满足
a
i
j
=
−
a
j
i
,
∀
i
,
j
⇔
A
T
=
−
A
a_{i j}=-a_{j i}, \forall i, j \Leftrightarrow A^{T}=-A
aij=−aji,∀i,j⇔AT=−A ,则称A为反对称矩阵。
若A是反对称矩阵,则
a
i
j
=
−
a
j
i
a_{i j}=-a_{j i}
aij=−aji ,当
i
=
j
i=j
i=j 时,便有
a
i
i
=
0
a_{i i}=0
aii=0 ,即反对称矩阵主对角线上的元全为零,而位于主对角线两侧对称的元素反号。
向量的叉乘可以用反对称矩阵表示成如下形式:
a
×
b
=
[
i
j
k
a
1
a
2
a
3
b
1
b
2
b
3
]
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
1
]
=
[
0
−
a
3
a
2
a
3
0
a
1
−
a
2
a
1
0
]
b
=
[
a
]
×
b
,
a
∈
R
3
,
b
∈
R
3
\boldsymbol{a} \times \boldsymbol{b}=\left[\begin{array}{ccc} \boldsymbol{i} & \boldsymbol{j} & \boldsymbol{k} \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{array}\right]=\left[\begin{array}{c} a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{1} \end{array}\right]=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] \boldsymbol{b}=[\boldsymbol{a}]_{\times} \boldsymbol{b}, \boldsymbol{a} \in \boldsymbol{R}^{3}, \boldsymbol{b} \in \boldsymbol{R}^{3}
a×b=⎣
⎡ia1b1ja2b2ka3b3⎦
⎤=⎣
⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦
⎤=⎣
⎡0a3−a2−a30a1a2a10⎦
⎤b=[a]×b,a∈R3,b∈R3
反对称矩阵的基本性质: [ a ] × = − [ a ] × T [\boldsymbol{a}]_{\times}=-[\boldsymbol{a}]_{\times}^{T} [a]×=−[a]×T
叉乘的基本性质,叉乘顺序互换,叉乘结果大小不变,方向相反: [ a ] × b = − [ b ] × a [\boldsymbol{a}]_{\times}\boldsymbol{b}=-[\boldsymbol{b}]_{\times}\boldsymbol{a} [a]×b=−[b]×a
叉乘的基本性质,向量与自己叉乘等于零向量: [ a ] × a = 0 [\boldsymbol{a}]_{\times} \boldsymbol{a}=\mathbf{0} [a]×a=0
叉乘的基本性质,向量叉乘的结果同时垂直于叉乘的两个向量: b T [ a ] × b = 0 \boldsymbol{b}^{T}[\boldsymbol{a}]_{\times} \boldsymbol{b}=0 bT[a]×b=0
若 向量 t = ( t 1 , t 2 , t 3 ) T t=\left(t_{1}, t_{2}, t_{3}\right)^{T} t=(t1,t2,t3)T ,则向量 t t t的反对称矩阵为: [ t ] × = [ 0 − t 3 t 2 t 3 0 − t 1 − t 2 t 1 0 ] [\boldsymbol{t}]_{\times}=\left[\begin{array}{ccc} 0 & -t_{3} & t_{2} \\ t_{3} & 0 & -t_{1} \\ -t_{2} & t_{1} & 0 \end{array}\right] [t]×=⎣ ⎡0t3−t2−t30t1t2−t10⎦ ⎤。
两边同时左乘
x
2
T
K
2
−
T
x_2^TK_2^{-T}
x2TK2−T,得:
Z
c
′
x
2
T
K
2
−
T
[
t
]
×
K
2
−
1
x
2
=
Z
c
x
2
T
K
2
−
T
[
t
]
×
R
K
1
−
1
x
1
{Z'_c}x_2^TK_2^{-T}[t]_{\times}K_2^{-1}x_2=Z_cx_2^TK_2^{-T}[t]_{\times}RK_1^{-1}x_1
Zc′x2TK2−T[t]×K2−1x2=Zcx2TK2−T[t]×RK1−1x1
因为
x
2
T
K
2
−
T
x_2^TK_2^{-T}
x2TK2−T与
[
t
]
×
K
2
−
1
x
2
[t]_\times K_2^{-1}x_2
[t]×K2−1x2垂直(即
(
K
2
−
1
x
2
)
T
(K_2^{-1}x_2)^T
(K2−1x2)T与
[
t
]
×
K
2
−
1
x
2
[t]_\times K_2^{-1}x_2
[t]×K2−1x2垂直),所以等式左边为0,约去常量
Z
c
Z_c
Zc得到:
x
2
T
K
2
−
T
[
t
]
×
R
K
1
−
1
x
1
=
0
x_2^TK_2^{-T}[t]_{\times}RK_1^{-1}x_1=0
x2TK2−T[t]×RK1−1x1=0
注:约去
Z
c
Z_c
Zc ,导致结构恢复(structure reconstruction),只是恢复了物体的结构框架,而不是实际意义的物体尺寸。
为什么用PnP而不是用对极几何来恢复相机的结构?
如果一直采用这种2d-2d匹配计算位姿的方式,那每次计算的 t t t都是在不同尺度下的。
在三维重建时,尺度统一的另一种思路就是后续的位姿估计PnP来进行位姿估计,这样后续的视频序列就不用计算本质矩阵从而绕过了尺度,所以后续的视频序列的位姿和最开始的两帧的尺度是一样的了。
注1:利用上式具有尺度不确定性
该式描述了两个像素坐标 x 1 , x 2 x_1,x_2 x1,x2之间的联系,或者称为一种约束关系,这个约束就叫做对极约束。
基础矩阵:
F = K 2 − T [ t ] × R K 1 − 1 F=K_2^{-T}[t]_{\times}RK_1^{-1} F=K2−T[t]×RK1−1
-
F F F是 3 × 3 3\times3 3×3的矩阵,具有7个自由度,秩为2
矩阵的秩,指经过初等变换后的非零行(列)的个数;
矩阵的自由度指的要想求解矩阵所有元素,至少需要多少个线性方程组;
有8个独立的比率( F F F矩阵有9个元素,公共因子不重要,所以都除以最后一个元素,让最后一个元素为1);因为 [ t ] × [t]_\times [t]×的秩为2,所以 d e t ( F ) = 0 det(F)=0 det(F)=0,所以 F F F具有7个自由度,秩为2.
-
奇异值为 [ σ 1 , σ 2 , 0 ] [\sigma_1,\sigma_2,0 ] [σ1,σ2,0]
-
极线约束 l 1 T = x 2 T F l_1^T=x_2^TF l1T=x2TF, l 2 = F x 1 l_2=Fx_1 l2=Fx1, x 2 T F x 1 = 0 x^T_2Fx_1=0 x2TFx1=0
本质矩阵:
E = [ t ] × R E=[t]_{\times}R E=[t]×R
注:本质矩阵是由对极约束定义的,由于对极约束是等式为0的约束,所以对 E E E乘以任意非零常数后,对极约束依然满足。我们把这件事情称为 E E E在不同尺度下是等价的。
注: E E E的内在性质是一种非线性性质,在估计时会带来麻烦,因此也可以只考虑它的尺度等价性,使用8对点来估计 E E E。——这就是经典的八点法(Eight-point-algorithm)。八点法只利用了 E E E的线性性质, 因此可以在线性代数框架下求解。
-
E E E是 3 × 3 3\times3 3×3的矩阵,具有5个自由度,秩为2
旋转和平移矩阵一共6个自由度,但由于尺度等价性,所以可以都除以最后一个元素,让最后一个元素为1,所以 E E E具有5个自由度,秩为2.
-
一个 3 × 3 3\times 3 3×3矩阵是本质矩阵的充要条件是它的奇异值中有两个相等,而第三个为零。即,奇异值为 [ σ , σ , 0 ] T [\sigma,\sigma,0 ]^T [σ,σ,0]T
**这称为本质矩阵的内在性质。**下面给出证明:
对 E E E 的奇异值分解可以由对 E E T E E^{T} EET 的特征值分解得到:
E E E的奇异值分解: E = U Σ V ⊤ E=U \Sigma V^{\top} E=UΣV⊤
E E T EE^T EET的奇异值分解: E E ⊤ = U Σ V ⊤ ( U Σ V ⊤ ) ⊤ = U Σ V ⊤ V Σ ⊤ U ⊤ = U M U ⊤ EE^{\top}=U\Sigma V^{\top}\left(U \Sigma V^{\top}\right)^{\top}=U \Sigma V^{\top} V \Sigma^{\top} U^{\top}=U M U^{\top} EE⊤=UΣV⊤(UΣV⊤)⊤=UΣV⊤VΣ⊤U⊤=UMU⊤ ,其中 V ⊤ V = I , V V^{\top} V=I,V V⊤V=I,V是正交矩阵,定义 Σ Σ ⊤ = M \Sigma\Sigma^{\top}=M ΣΣ⊤=M. 对 E E T EE^T EET特征值分解可以得到 U 、 M U、M U、M,对 M M M对角线上的特征值开方可得 Σ \Sigma Σ.
证明本质矩阵 E E E 奇异值具有 [ σ , σ , 0 ] T [\sigma, \sigma, 0]^{T} [σ,σ,0]T 的形式,只需证明矩阵 E T E E^{T} E ETE 的特征值具有 [ λ , λ , 0 ] T [\lambda, \lambda, 0]^{T} [λ,λ,0]T 的形式。
由于矩阵 E E T E E^{T} EET 与 E T E E^{T} E ETE 有相同的非零特征值,所以证明内在性质也等价于证明矩阵 E E T E E^{T} EET 的特征值具有 [ λ , λ , 0 ] T [\lambda, \lambda, 0]^{T} [λ,λ,0]T 的形式,下面我们就来证明这一 结论。
1.求 E E T EE^T EET
E E T = ( [ t ] × R ) ( [ t ] × R ) T = [ t ] × R R T ( [ t ] × ) T = [ t ] × ( [ t ] × ) T E E^{T}=\left([t]_{\times} R\right)\left([t]_{\times}R\right)^{T}=[t]_{\times} R R^{T}\left([t]_{\times}\right)^{T}=[t]_{\times}\left([t]_{\times}\right)^{T} EET=([t]×R)([t]×R)T=[t]×RRT([t]×)T=[t]×([t]×)T
2.对 E E T EE^T EET的特征值分解得到 M , U M,U M,U设 t = [ a 1 a 2 a 3 ] ⇒ t ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] t=\left[\begin{array}{l}a_{1} \\ a_{2} \\ a_{3}\end{array}\right] \Rightarrow t^{\wedge}=\left[\begin{array}{ccc}0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0\end{array}\right] t=⎣ ⎡a1a2a3⎦ ⎤⇒t∧=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤把 t t t规范化为长度1,故 a 1 2 + a 2 2 + a 3 2 = 1 a^2_1+a^2_2+a^2_3=1 a12+a22+a32=1(尺度等价)
E E ⊤ = t ∧ t ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ 0 a 3 − a 2 − a 3 0 a 1 a 2 − a 1 0 ] = [ a 3 2 + a 2 2 − a 1 a 2 − a 1 a 3 − a 1 a 2 a 1 2 + a 3 2 − a 2 a 3 − a 1 a 3 − a 2 a 3 a 2 2 + a 1 2 ] \begin{aligned} E E^{\top} &=t^{\wedge} t^{\wedge} =\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right]\left[\begin{array}{ccc} 0 & a_{3} & -a_{2} \\ -a_{3} & 0 & a_{1} \\ a_{2} & -a_{1} & 0 \end{array}\right]=\left[\begin{array}{ccc} a_{3}^{2}+a_{2}^{2} & -a_{1} a_{2} & -a_{1} a_{3} \\ -a_{1} a_{2} & a_{1}^{2}+a_{3}^{2} & -a_{2} a_{3} \\ -a_{1} a_{3} & -a_{2} a_{3} & a_{2}^{2}+a_{1}^{2} \end{array}\right] \end{aligned} EE⊤=t∧t∧=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤⎣ ⎡0−a3a2a30−a1−a2a10⎦ ⎤=⎣ ⎡a32+a22−a1a2−a1a3−a1a2a12+a32−a2a3−a1a3−a2a3a22+a12⎦ ⎤
求特征值: ( E E ⊤ − λ I ) x = 0 \left(E E^{\top}-\lambda I\right) x=0 \quad (EE⊤−λI)x=0 ,即 ∣ E E ⊤ − λ I ∣ = 0 \left|E E^{\top}-\lambda I\right|=0 ∣ ∣EE⊤−λI∣ ∣=0
⇒ ∣ a 3 2 + a 2 2 − λ − a 1 a 2 − a 1 a 3 − a 1 a 2 a 1 2 + a 3 2 − λ − a 2 a 3 − a 1 a 3 − a 2 a 3 a 1 2 + a 2 2 − λ ∣ = 0 \Rightarrow\left|\begin{array}{ccc}a_{3}^{2}+a_{2}^{2}-\lambda & -a_{1} a_{2} & -a_{1} a_{3} \\ -a_{1} a_{2} & a_{1}^{2}+a_{3}^{2}-\lambda & -a_{2} a_{3} \\ -a_{1} a_{3} & -a_{2} a_{3} & a_{1}^{2}+a_{2}^{2}-\lambda\end{array}\right|=0 ⇒∣ ∣a32+a22−λ−a1a2−a1a3−a1a2a12+a32−λ−a2a3−a1a3−a2a3a12+a22−λ∣ ∣=0⇒ ∣ 1 − λ − a 1 2 − a 1 a 2 − a 1 a 3 − a 1 a 2 1 − λ − a 2 2 − a 2 a 3 − a 1 a 3 − a 2 a 3 1 − λ − a 3 2 ∣ = 0 \Rightarrow\left|\begin{array}{ccc} 1-\lambda-a_{1}^{2} & -a_{1} a_{2} & -a_{1} a_{3} \\ -a_{1} a_{2} & 1-\lambda-a_{2}^{2} & -a_{2} a_{3} \\ -a_{1} a_{3} & -a_{2} a_{3} & 1-\lambda-a_{3}^{2} \end{array}\right|=0 ⇒∣ ∣1−λ−a12−a1a2−a1a3−a1a21−λ−a22−a2a3−a1a3−a2a31−λ−a32∣ ∣=0
拆开可得:$\Rightarrow(1-\lambda)^{2}(-\lambda)=0
$解得:$ \lambda_{1}=\lambda_{2}=1 \quad \lambda_{3}=0 $
2.基础矩阵&本质矩阵求解方法
基础矩阵求解方法:
-
直接线性变换法
对与一对匹配点 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 F x 1 = 0 x_2^TFx_1=0 x2TFx1=0,
( u 2 v 2 1 ) [ F 11 F 12 F 13 F 21 F 22 F 23 F 31 F 32 F 33 ] ( u 1 v 1 1 ) = 0 \left(\begin{array}{lll}u_{2} & v_{2} & 1\end{array}\right)\left[\begin{array}{lll}F_{11} & F_{12} & F_{13} \\ F_{21} & F_{22} & F_{23} \\ F_{31} & F_{32} & F_{33}\end{array}\right]\left(\begin{array}{c}u_{1} \\ v_{1} \\ 1\end{array}\right)=0 (u2v21)⎣ ⎡F11F21F31F12F22F32F13F23F33⎦ ⎤⎝ ⎛u1v11⎠ ⎞=0
令 F = [ F 11 , F 12 , F 13 , F 21 , F 22 , F 23 , F 31 , F 32 , F 33 ] T \boldsymbol{F}=\left[\begin{array}{llll}F_{11}, & F_{12}, & F_{13}, & F_{21}, & F_{22}, & F_{23}, & F_{31}, & F_{32}, & F_{33}\end{array}\right]^{T} F=[F11,F12,F13,F21,F22,F23,F31,F32,F33]T, 则有,
[ u 1 u 2 , u 2 v 1 , u 2 , u 1 v 2 , v 1 v 2 , v 2 , u 1 , v 1 , 1 ] F = 0 \left[\begin{array}{llll}u_{1} u_{2}, & u_{2} v_{1}, & u_{2}, & u_{1} v_{2}, & v_{1} v_{2}, & v_{2}, & u_{1}, & v_{1}, & 1\end{array}\right] \boldsymbol{F}=0 [u1u2,u2v1,u2,u1v2,v1v2,v2,u1,v1,1]F=0
记为:
A F = 0 AF=0 AF=0
每对匹配点提供一个约束,要保证有唯一解至少需要8对匹配点;n = 8 n = 8 n=8时,若 A A A非奇异,则有唯一解,称为8点法
n ≥ 8 n ≥ 8 n≥8 时,可用最小二乘法求解。 A T A A^TA ATA的最小特征值对应的特征向量即为最优解。
-
奇异值约束
直接线性变换法无法保证基础矩阵的奇异值约束——有两个非0的奇异值,因此根据奇异值约束对矩阵进行重构,其数学描述为:
m i n ∣ ∣ F − F ^ ∣ ∣ , w r t . s v d ( F ) = [ σ 1 , σ 2 , 0 ] min||F-\hat{F}||,wrt.svd(F)=[\sigma_1,\sigma_2,0] min∣∣F−F^∣∣,wrt.svd(F)=[σ1,σ2,0]
求解:
(1)对得到的基础矩阵
F
^
\hat F
F^进行奇异值分解,即:
F
^
=
U
S
V
T
S
=
diag
(
σ
1
,
σ
2
,
σ
3
)
\hat{F}=USV^T \quad S=\text{diag}(\sigma_1,\sigma_2,\sigma_3)
F^=USVTS=diag(σ1,σ2,σ3)
(2)对分解后的奇异值进行约束(把最小的奇异值设为0),利用奇异值约束对基础矩阵进行重构:
F
=
U
diag
(
σ
1
,
σ
2
,
0
)
V
T
F=U\text{diag}(\sigma_1,\sigma_2,0)V^T
F=Udiag(σ1,σ2,0)VT
-
基于RANSAC的鲁棒方法
1.随机采样8对匹配点
2.8点法求解基础矩阵 F ^ \hat F F^;
3.奇异值约束获取基础矩阵 F F F;
4.计算误差,并统计内点个数;
内点判断标准—一阶几何误差(first-order geometric error),又名辛普森距离(Sampson distance):
令 e = x 2 T F x 1 , J = δ ( x i ′ T F x i ) δ x i e=x_{2}^{T} F x_{1}, J=\frac{\delta\left(x_{i}^{\prime T} F x_{i}\right)}{\delta x_{i}} e=x2TFx1,J=δxiδ(xi′TFxi) 则该对应点的辛普森距离为 d ( x 1 , x 2 ) d(x_1,x_2) d(x1,x2)为:
d ( x 1 , x 2 ) = e T e J J T = ( x 2 T F x 1 ) 2 ( F x 1 ) 1 2 + ( F x 1 ) 2 2 + ( x 2 T F ) 1 2 + ( x 2 T F ) 2 2 d(x_1,x_2)=\frac{e^{T} e}{J J^{T}}=\frac{\left(x_{2}^{T} F x_{1}\right)^{2}}{\left(F x_{1}\right)_{1}^{2}+\left(F x_{1}\right)_{2}^{2}+\left(x_{2}^{T}F \right)_{1}^{2}+\left(x_{2}^{T}F\right)_{2}^{2}} d(x1,x2)=JJTeTe=(Fx1)12+(Fx1)22+(x2TF)12+(x2TF)22(x2TFx1)2
内点的判断标准:
d ( x 1 , x 2 ) < τ d(x_1,x_2)<\tau d(x1,x2)<τ
5.重复上述过程,选择内点数最多的结果;6.对所有内点执行2,3,重新计算 𝐹 𝐹 F.
本质矩阵求解方法:
-
先求基础矩阵 F F F,通过基础矩阵可得 E ^ = K 2 T F K 1 \hat E=K_2^TFK_1 E^=K2TFK1
-
再对 E ^ \hat E E^进行奇异值分解: E ^ = U diad ( σ 1 , σ 2 , 0 ) V T \hat E=U\text{diad}(\sigma_1,\sigma_2,0)V^T E^=Udiad(σ1,σ2,0)VT
-
利用奇异值约束(奇异值中有两个相等,第三个为零)对本质矩阵进行重构:
E = U diad ( ( σ 1 + σ 2 ) 2 , ( σ 1 + σ 2 ) 2 , 0 ) V T E=U\text{diad}(\frac{(\sigma_1+\sigma_2)}{2},\frac{(\sigma_1+\sigma_2)}{2},0)V^T E=Udiad(2(σ1+σ2),2(σ1+σ2),0)VT
-
基于RANSAC的鲁棒方法
3.相机姿态的恢复(从 E E E中恢复 R R R、 t t t)
[Golub-89]:G.H.Golub and C.F.Van Loan. Matrix Computations. The Johns Hopkins University Press,second edition,1989
若 S S S是实的反对称矩阵,则 S = U B U T S=UBU^T S=UBUT,其中 B B B为分块对角矩阵,形如 diag ( σ 1 Z , σ 2 Z , … , σ 2 Z , 0 , … , 0 ) \text{diag}(\sigma_1 Z,\sigma_2 Z,…,\sigma_2 Z,0,…,0) diag(σ1Z,σ2Z,…,σ2Z,0,…,0),其中 Z = [ 0 , 1 − 1 , 0 ] Z=\left[\begin{array}{ccc}0, & 1\\ -1, & 0\end{array}\right] Z=[0,−1,10].
由本质矩阵恢复相机姿态,会得到4种不同的姿态:
E
=
[
t
]
×
R
=
U
[
σ
,
0
,
0
0
,
σ
,
0
0
,
0
,
0
]
V
T
E=[t]_{\times}R=U\left[\begin{array}{ccc}\sigma, & 0, & 0 \\ 0, & \sigma, & 0 \\ 0, & 0, & 0\end{array}\right]{V}^{T}
E=[t]×R=U⎣
⎡σ,0,0,0,σ,0,000⎦
⎤VT
第一种分解:
U
[
σ
,
0
,
0
0
,
σ
,
0
0
,
0
,
0
]
V
T
=
U
[
0
,
σ
,
0
−
σ
,
0
,
0
0
,
0
,
0
]
[
0
,
−
1
,
0
1
,
0
,
0
0
,
0
,
1
]
V
T
=
U
[
0
,
σ
,
0
−
σ
,
0
,
0
0
,
0
,
0
]
U
T
U
[
0
,
−
1
,
0
1
,
0
,
0
0
,
0
,
1
]
V
T
U\left[\begin{array}{ccc}\sigma, & 0, & 0 \\ 0, & \sigma, & 0 \\ 0, & 0, & 0\end{array}\right]{V}^{T}=U\left[\begin{array}{ccc}0,&\sigma, & 0\\ -\sigma, & 0, & 0 \\ 0, & 0, & 0\end{array}\right]\left[\begin{array}{ccc}0, & -1, & 0 \\ 1, & 0, & 0 \\ 0, & 0, & 1\end{array}\right]{V}^{T}=U\left[\begin{array}{ccc}0,&\sigma, & 0\\ -\sigma, & 0, & 0 \\ 0, & 0, & 0\end{array}\right]U^TU\left[\begin{array}{ccc}0, & -1, & 0 \\ 1, & 0, & 0 \\ 0, & 0, & 1\end{array}\right]{V}^{T}
U⎣
⎡σ,0,0,0,σ,0,000⎦
⎤VT=U⎣
⎡0,−σ,0,σ,0,0,000⎦
⎤⎣
⎡0,1,0,−1,0,0,001⎦
⎤VT=U⎣
⎡0,−σ,0,σ,0,0,000⎦
⎤UTU⎣
⎡0,1,0,−1,0,0,001⎦
⎤VT
第二种分解:(
E
E
E具有尺度等价性,即
E
=
−
E
E=-E
E=−E)
U
[
σ
,
0
,
0
0
,
σ
,
0
0
,
0
,
0
]
V
T
=
U
[
0
,
−
σ
,
0
σ
,
0
,
0
0
,
0
,
0
]
[
0
,
1
,
0
−
1
,
0
,
0
0
,
0
,
0
]
V
T
=
U
[
0
,
−
σ
,
0
σ
,
0
,
0
0
,
0
,
0
]
U
T
U
[
0
,
1
,
0
−
1
,
0
,
0
0
,
0
,
0
]
V
T
U\left[\begin{array}{ccc}\sigma, & 0, & 0 \\ 0, & \sigma, & 0 \\ 0, & 0, & 0\end{array}\right]{V}^{T}=U\left[\begin{array}{ccc}0,&-\sigma, & 0\\ \sigma, & 0, & 0 \\ 0, & 0, & 0\end{array}\right]\left[\begin{array}{ccc}0, & 1, & 0 \\ -1, & 0, & 0 \\ 0, & 0, & 0\end{array}\right]{V}^{T}=U\left[\begin{array}{ccc}0,&-\sigma, & 0\\ \sigma, & 0, & 0 \\ 0, & 0, & 0\end{array}\right]U^TU\left[\begin{array}{ccc}0, & 1, & 0 \\ -1, & 0, & 0 \\ 0, & 0, & 0\end{array}\right]{V}^{T}
U⎣
⎡σ,0,0,0,σ,0,000⎦
⎤VT=U⎣
⎡0,σ,0,−σ,0,0,000⎦
⎤⎣
⎡0,−1,0,1,0,0,000⎦
⎤VT=U⎣
⎡0,σ,0,−σ,0,0,000⎦
⎤UTU⎣
⎡0,−1,0,1,0,0,000⎦
⎤VT
对本质矩阵的分解,可得到
E
=
[
t
]
×
R
E=[t]_{\times}R
E=[t]×R 分解有如下的两种形式:
[
t
]
×
=
U
[
0
,
1
,
0
−
1
,
0
,
0
0
,
0
,
0
]
U
T
或
[
t
]
×
=
U
[
0
,
−
1
,
0
1
,
0
,
0
0
,
0
,
0
]
U
T
,
R
=
U
[
0
,
−
1
,
0
1
,
0
,
0
0
,
0
,
1
]
V
T
或
R
=
U
[
0
,
1
,
0
−
1
,
0
,
0
0
,
0
,
1
]
V
T
[t]_{\times}={U}\left[\begin{array}{ccc}0, & 1, & 0 \\ -1, & 0, & 0 \\ 0, & 0, & 0\end{array}\right]U^T\quad 或\quad [t]_{\times}={U}\left[\begin{array}{ccc}0, & -1, & 0 \\ 1, & 0, & 0 \\ 0, & 0, & 0\end{array}\right]U^T,\\ {R}={U} \left[\begin{array}{ccc}0, & -1, & 0 \\ 1, & 0, & 0 \\ 0, & 0, & 1\end{array}\right]{V}^{T}\quad 或\quad {R}={U} \left[\begin{array}{ccc}0, & 1, & 0 \\ -1, & 0, & 0 \\ 0, & 0, & 1\end{array}\right]{V}^{T}
[t]×=U⎣
⎡0,−1,0,1,0,0,000⎦
⎤UT或[t]×=U⎣
⎡0,1,0,−1,0,0,000⎦
⎤UT,R=U⎣
⎡0,1,0,−1,0,0,001⎦
⎤VT或R=U⎣
⎡0,−1,0,1,0,0,001⎦
⎤VT
可得:
t
=
U
(
:
,
3
)
,
或
t
=
−
U
(
:
,
3
)
,
R
=
U
R
Z
T
(
π
2
)
V
T
,
或
R
=
U
R
Z
T
(
−
π
2
)
V
T
t={U}(:,3),或\quad t=-{U}(:,3),\\{R}={U} {R}_{Z}^T\left(\frac{\pi}{2}\right) {V}^{T},或{R}={U} {R}_{Z}^T\left(-\frac{\pi}{2}\right) {V}^{T}
t=U(:,3),或t=−U(:,3),R=URZT(2π)VT,或R=URZT(−2π)VT
本质矩阵分解:这4种情况都满足投影关系(下图中的三角形)。
下面来看,如何得到准确的相机姿态:
利用相机姿态 R , t R,t R,t和匹配点 x 1 , x 2 x_1,x_2 x1,x2 进行三角量测,可得到三维点 X X X;
相机中心在世界坐标系的坐标为: O 1 = 0 O 2 = − R T t O_1=0 \quad O_2=-R^Tt O1=0O2=−RTt;
相机的世界坐标中的朝向: d 1 = [ 0 , 0 , 1 ] T d 2 = r 3 T d_1=[0,0,1]^T \quad d_2=r_3^T d1=[0,0,1]Td2=r3T(旋转矩阵的第三行);
X w X_w Xw需满足同时位于两个相机的前方:
方法1:
(
X
w
−
O
1
)
T
d
1
>
0
(
X
w
−
O
2
)
T
d
2
>
0
(X_w-O_1)^Td_1>0\\ (X_w-O_2)^Td_2>0\\
(Xw−O1)Td1>0(Xw−O2)Td2>0
方法2:
[
X
c
,
Y
c
,
Z
c
]
T
=
R
X
w
+
t
,
Z
c
>
0
[X_c,Y_c,Z_c]^T=RX_w+t,\quad Z_c>0
[Xc,Yc,Zc]T=RXw+t,Zc>0
4.单应矩阵 & 求解方法
除了基本矩阵和本质矩阵,二视图之间还存在另一种单应矩阵关系,它描述了两个平面之间的映射关系。
单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变化关系。
设空间中特征点
X
X
X位于一平面上,设平面的法向量为
n
T
n^T
nT ,则平面方程为:
n
T
X
+
d
=
0
n^TX+d=0
nTX+d=0
即:
−
n
T
X
d
=
1
-\frac{n^TX}{d}=1
−dnTX=1
推导单应性矩阵,
X
X
X 在
O
2
O_2
O2相机坐标系中的表达为:
x
2
=
K
2
(
R
X
+
t
)
=
K
2
(
R
X
+
t
⋅
(
−
n
T
X
d
)
)
=
K
2
(
R
−
t
n
T
d
)
X
=
K
2
(
R
−
t
n
T
d
)
K
1
−
1
x
1
x_2=K_2(RX+t)=K_2(RX+t\cdot(-\frac{n^TX}{d}))\\ =K_2(R-\frac{tn^T}{d})X=K_2(R-\frac{tn^T}{d})K_1^{-1}x_1
x2=K2(RX+t)=K2(RX+t⋅(−dnTX))=K2(R−dtnT)X=K2(R−dtnT)K1−1x1
单应性矩阵:
定义单应性矩阵
H
H
H为:
H
=
K
2
(
R
−
t
n
T
d
)
K
1
−
1
H=K_2(R-\frac{tn^T}{d})K_1^{-1}
H=K2(R−dtnT)K1−1
即,
x
2
=
H
x
1
x_2=Hx_1
x2=Hx1。
单应性矩阵和空间的平面有关,也和平面到相机之间的变换有关。单应性矩阵是满秩的。当 t = 0 t=0 t=0时, H = K 2 R K 1 − 1 H=K_2RK_1^{-1} H=K2RK1−1对应的是纯旋转。
因此,单应矩阵有两种情况:
-
场景为单一平面,则两幅图像的关系可以用单应性矩阵来描述
-
相机纯旋转
单应性矩阵线性求解
( u 2 v 2 1 ) = [ H 11 H 12 H 13 H 21 H 22 H 23 H 31 H 32 H 33 ] ( u 1 v 1 1 ) \left(\begin{array}{c} u_{2} \\ v_{2} \\ 1 \end{array}\right)=\left[\begin{array}{lll} H_{11} & H_{12} & H_{13} \\ H_{21} & H_{22} & H_{23} \\ H_{31} & H_{32} & H_{33} \end{array}\right]\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right) ⎝ ⎛u2v21⎠ ⎞=⎣ ⎡H11H21H31H12H22H32H13H23H33⎦ ⎤⎝ ⎛u1v11⎠ ⎞
u 2 = H 11 u 1 + H 12 v 1 + H 13 H 31 u 1 + H 32 v 1 + H 33 v 2 = H 21 u 1 + H 22 v 1 + H 23 H 31 u 1 + H 32 v 1 + H 33 \begin{aligned} u_{2} &=\frac{H_{11} u_{1}+H_{12} v_{1}+H_{13}}{H_{31} u_{1}+H_{32} v_{1}+H_{33}} \\ v_{2} &=\frac{H_{21} u_{1}+H_{22} v_{1}+H_{23}}{H_{31} u_{1}+H_{32} v_{1}+H_{33}} \end{aligned} u2v2=H31u1+H32v1+H33H11u1+H12v1+H13=H31u1+H32v1+H33H21u1+H22v1+H23
令
H
33
=
1
H_{33}=1
H33=1,则单应性矩阵有8 个自由度,每对点有两个约束:
H
11
u
1
+
H
12
v
1
+
H
13
−
H
31
u
1
u
2
−
H
32
u
2
v
1
=
H
33
u
2
H
21
u
1
+
H
22
v
1
+
H
23
−
H
31
u
1
v
2
−
H
32
v
1
v
2
=
H
33
v
2
\begin{aligned} &H_{11} u_{1}+H_{12} v_{1}+H_{13}-H_{31} u_{1} u_{2}-H_{32} u_{2} v_{1}=H_{33} u_{2} \\ &H_{21} u_{1}+H_{22} v_{1}+H_{23}-H_{31} u_{1} v_{2}-H_{32} v_{1} v_{2}=H_{33} v_{2} \end{aligned}
H11u1+H12v1+H13−H31u1u2−H32u2v1=H33u2H21u1+H22v1+H23−H31u1v2−H32v1v2=H33v2
RANSAC-估计单应矩阵
算法流程:
-
随机采样4对匹配点;
-
4点法求解单应矩阵 H H H;
-
计算误差,并统计内点个数;
内点判断标准-Sampson Distance:
E
(
x
1
,
x
2
,
H
)
<
τ
E\left({x}_{1}, {x}_{2}, {H}\right)<\tau
E(x1,x2,H)<τ
E ( x 1 , x 2 , H ) = d ( x 1 , H − 1 x 2 ) 2 + d ( x 2 , H x 1 ) 2 E\left({x}_{1}, {x}_{2}, {H}\right)=d\left({x}_{1}, {H}^{-1} \boldsymbol{x}_{2}\right)^{2}+d\left({x}_{2}, {H} {x}_{1}\right)^{2} E(x1,x2,H)=d(x1,H−1x2)2+d(x2,Hx1)2
-
重复上述过程,选择内点数最多的结果;
-
对所有内点执行3,4,重新计算 H H H。