摘要
两个点集 { p i } \{p_i\} {pi}和 { p i ′ } \{p_i^{'}\} {pi′}; i = 1 , 2 , . . . , N i = 1,2,...,N i=1,2,...,N由 p i ′ = R p i + T + N i p_i^{'} = Rp_i + T + N_i pi′=Rpi+T+Ni所关联,这里 R R R是旋转矩阵, T T T是平移向量, N i N_i Ni是噪声向量。给定 { p i } \{p_i\} {pi}和 { p i ′ } \{p_i^{'}\} {pi′},我们提出了一种算法来找到 R R R和 T T T的最小二乘解,这个方法基于 3 × 3 3 \times 3 3×3矩阵的奇异值分解(SVD)。在计算时间方面,将此算法与两种较早提出的算法进行了比较。
引言
在许多计算机视觉的应用,尤其是使用3-D点对的对应关系[1]来估计刚体的运动参数以及刚体相对于参考坐标系的位姿[2]中,我们遇到了下面的数学问题。我们给定两个3-D点集
{
p
i
}
\{p_i\}
{pi}和
{
p
i
′
}
\{p_i^{'}\}
{pi′};
i
=
1
,
2
,
.
.
.
,
N
i = 1, 2, ...,N
i=1,2,...,N(这里
p
i
p_i
pi和
p
i
′
p_i^{'}
pi′是视为
3
×
1
3 \times 1
3×1的列矩阵)
p
i
′
=
R
p
i
+
T
+
N
i
(1)
p_i^{'} = Rp_i + T+N_i \tag{1}
pi′=Rpi+T+Ni(1)
这里
R
R
R是
3
×
3
3 \times 3
3×3的旋转矩阵,
T
T
T是平移向量(
3
×
1
3 \times 1
3×1的列矩阵),
N
i
N_i
Ni是噪声向量(我们假设旋转是沿着一个通过原点的旋转轴进行的)。我们想要找到一个
R
R
R和
T
T
T来最小化
Σ
2
=
∑
i
=
1
N
∣
∣
p
i
−
(
R
p
i
+
T
)
∣
∣
2
(2)
\Sigma^2 = \sum_{i=1}^{N}{|| p_i - (Rp_i + T) ||^2} \tag{2}
Σ2=i=1∑N∣∣pi−(Rpi+T)∣∣2(2)
Huang,Blostein和Margerum[3]提出了一种用于寻找解的迭代算法,Faugeras和Hebert提出了一种基于四元数的非迭代算法。在本文中,我们提出了一种新的非迭代算法,该算法涉及到
3
×
3
3 \times 3
3×3矩阵的奇异值分解(SVD)。我们比较了三种算法的计算时间。
解耦平移和旋转
根据[3]中所述,如果公式(1)的最小二乘解为
R
^
\hat{R}
R^和
T
^
\hat{T}
T^,则
{
p
i
′
}
\{p_i^{'}\}
{pi′}和
{
p
i
′
′
≡
R
^
p
i
+
T
^
}
\{p_i^{''} \equiv \hat{R}p_i +\hat{T}\}
{pi′′≡R^pi+T^}有着相同的质心,也就是说
p
′
=
p
′
′
.
(3)
p^{'} = p^{''}. \tag{3}
p′=p′′.(3)
这里
p
′
=
1
N
∑
i
=
1
N
p
i
′
.
(4)
p^{'} = \frac{1}{N} \sum_{i = 1}^{N}{p_i^{'}}. \tag{4}
p′=N1i=1∑Npi′.(4)
p ′ ′ = 1 N ∑ i = 1 N p i ′ ′ = R ^ p + T ^ (5) p^{''} = \frac{1}{N} \sum_{i=1}^{N}{p_i^{''}} = \hat{R}p + \hat{T} \tag{5} p′′=N1i=1∑Npi′′=R^p+T^(5)
p = 1 N ∑ i = 1 N p i p = \frac{1}{N} \sum_{i=1}^{N}{p_i} p=N1i=1∑Npi
令
q
i
=
p
i
−
p
(7)
q_i = p_i - p \tag{7}
qi=pi−p(7)
q i ′ = p i ′ − p ′ (8) q_i^{'} = p_i^{'} - p^{'} \tag{8} qi′=pi′−p′(8)
我们有
∑
i
=
1
N
∣
∣
q
i
′
−
R
q
i
∣
∣
2
(9)
\sum_{i = 1}^{N}{|| q_i^{'} - Rq_{i} ||^2} \tag{9}
i=1∑N∣∣qi′−Rqi∣∣2(9)
因此,原始的最小二乘问题可以被分为两部分
(i) 寻找
R
^
\hat{R}
R^来最小化公式(9)中的
Σ
2
\Sigma^2
Σ2
(ii) 之后平移向量可以根据以下公式计算得到
T
^
=
p
′
−
R
^
p
(10)
\hat{T} = p^{'} - \hat{R}p \tag{10}
T^=p′−R^p(10)
在下一部分,我们将会描述一种使用奇异值分解来求解第i部分的算法。
一种使用奇异值分解来寻找 R ^ \hat{R} R^的算法
A.算法
Step 1:
从 { p i } \{p_i\} {pi}, { p i ′ } \{p_i^{'}\} {pi′}计算 p p p和 p ′ p^{'} p′,之后计算 { q i } \{q_i\} {qi}和 { q i ′ } \{q_i^{'}\} {qi′}
Step 2:
计算
3
×
3
3 \times 3
3×3的矩阵
H
=
∑
i
=
1
N
q
i
q
i
′
T
(11)
H = \sum_{i=1}^{N}{q_i {q_i^{'}}^T} \tag{11}
H=i=1∑Nqiqi′T(11)
这里上标
T
T
T定义为矩阵的转置
Step 3:
对矩阵H进行SVD分解
H
=
U
Λ
V
T
(12)
H = U \Lambda V^T \tag{12}
H=UΛVT(12)
Step 4:
计算
X
=
V
U
T
(13)
X = VU^T \tag{13}
X=VUT(13)
Step 5:
计算矩阵X的行列式
d
e
t
(
X
)
det(X)
det(X)
如果
d
e
t
(
X
)
=
+
1
det(X) = +1
det(X)=+1,则
R
^
=
X
\hat{R} = X
R^=X
如果
d
e
t
(
X
)
=
−
1
det(X) = -1
det(X)=−1,则算法失败(这种情况通常不会出现)
再十四讲中当行列式为负值,取相反数,具体证明有待研究
B.推导
展开公式(9)的右侧
Σ
2
=
∑
i
=
1
N
(
q
i
′
−
R
q
i
)
T
(
q
i
′
−
R
q
i
)
=
∑
i
=
1
N
(
q
i
′
T
q
i
′
+
q
i
T
q
i
−
q
i
′
T
R
q
i
−
q
i
T
R
T
q
i
′
)
=
∑
i
=
1
N
(
q
i
′
T
q
i
′
+
q
i
T
q
i
−
2
q
i
′
T
R
q
i
)
\Sigma^2 = \sum_{i=1}^{N}{ (q_i^{'} - Rq_i)^T (q_i^{'} - Rq_i) } \\ = \sum_{i=1}^{N}{( {q_i^{'}}^Tq_i^{'} + q_i^T q_i - {q_i^{'}}^TRq_i - q_i^T R^T q_i^{'} )} \\ = \sum_{i=1}^{N}{( {q_i^{'}}^Tq_i^{'} + q_i^T q_i - 2{q_i^{'}}^TRq_i )}
Σ2=i=1∑N(qi′−Rqi)T(qi′−Rqi)=i=1∑N(qi′Tqi′+qiTqi−qi′TRqi−qiTRTqi′)=i=1∑N(qi′Tqi′+qiTqi−2qi′TRqi)
因此,最小化
Σ
2
\Sigma^2
Σ2等价于最大化
F
=
∑
i
=
1
N
q
i
′
T
R
q
i
=
T
r
a
c
e
(
∑
i
=
1
N
R
q
i
q
i
′
T
)
=
T
r
a
c
e
(
R
H
)
(14)
F = \sum_{i=1}^{N}{{q_i^{'}}^T R q_i} \\ = Trace(\sum_{i=1}^{N}{ Rq_i {q_i^{'}}^T}) \\ = Trace(RH) \tag{14}
F=i=1∑Nqi′TRqi=Trace(i=1∑NRqiqi′T)=Trace(RH)(14)
这里
H
=
∑
i
=
1
N
q
i
q
i
′
T
(15)
H = \sum_{i=1}^{N}{q_i {q_i^{'}}^T} \tag{15}
H=i=1∑Nqiqi′T(15)
定理:
对于任何正定的矩阵
A
A
T
AA^T
AAT以及任何正交矩阵B
T
r
a
c
e
(
A
A
T
)
≥
T
r
a
c
e
(
B
A
A
T
)
Trace(AA^T) \geq Trace(BAA^T)
Trace(AAT)≥Trace(BAAT)
定理证明:
令
a
i
a_i
ai为矩阵
A
A
A的第
i
i
i列,所以
T
r
a
c
e
(
B
A
A
T
)
=
T
r
a
c
e
(
A
T
B
A
)
=
∑
i
=
1
a
i
T
(
B
a
i
)
Trace(BAA^T) = Trace(A^TBA) = \sum_{i=1}{a_i^T(Ba_i)}
Trace(BAAT)=Trace(ATBA)=i=1∑aiT(Bai)
但是,根据施瓦茨不等式
a
i
T
(
B
a
i
)
≤
(
a
i
T
a
i
)
(
a
i
T
B
T
B
a
i
)
=
a
i
T
a
i
a_i^T(B a_i) \leq \sqrt{(a_i^Ta_i)(a_i^T B^T B a_i)} = a_i^Ta_i
aiT(Bai)≤(aiTai)(aiTBTBai)=aiTai
因此
T
r
a
c
e
(
B
A
A
T
)
≤
∑
i
a
i
T
a
i
=
T
r
a
c
e
(
A
A
T
)
Trace(BAA^T) \leq \sum_{i}{a_i^Ta_i} = Trace(AA^T)
Trace(BAAT)≤∑iaiTai=Trace(AAT)
对
H
H
H进行
S
V
D
SVD
SVD分解
H
=
U
Λ
V
T
(16)
H = U \Lambda V^T \tag{16}
H=UΛVT(16)
这里
U
U
U和
V
V
V为
3
×
3
3 \times 3
3×3的正交矩阵,
Λ
\Lambda
Λ为具有非负元素的
3
×
3
3 \times 3
3×3的对角矩阵,现在令
X
=
V
U
T
X = VU^T
X=VUT
我们有
X
H
=
V
U
T
U
Λ
V
T
=
V
Λ
V
T
(17)
XH = VU^TU \Lambda V^T = V \Lambda V^T \tag{17}
XH=VUTUΛVT=VΛVT(17)
这里
X
H
XH
XH为正定对称矩阵,因此,根据定理,对于任何的
3
×
3
3 \times 3
3×3正交矩阵B
T
r
a
c
e
(
X
H
)
≥
T
r
a
c
e
(
B
X
H
)
(18)
Trace(XH) \geq Trace(BXH) \tag{18}
Trace(XH)≥Trace(BXH)(18)
因此,在所有的
3
×
3
3 \times 3
3×3正交矩阵中,
X
X
X最大化了公式(14)中的
F
F
F,并且如果
d
e
t
(
X
)
=
+
1
det(X) = +1
det(X)=+1,
X
X
X是一个旋转,这正是我们想要的。
然而,如果 d e t ( X ) = − 1 det(X) = -1 det(X)=−1, X X X是一个反射,这并不是我们想要的。幸运的是,这种退化的情况通常来说并不会发生,我们将会在下面的两个章节中关于这种退化的细节。
退化:无噪声的情况
假设公式(1)中对于所有 i i i, N i = 0 N_i = 0 Ni=0。那么显然存在一个解 R ^ \hat{R} R^(这里 R ^ \hat{R} R^是一个旋转,也就是 d e t ( R ^ ) = + 1 det(\hat{R}) = +1 det(R^)=+1),并且这个 R ^ \hat{R} R^对于 { q i ′ } \{q_i^{'}\} {qi′}和 { R ^ q i } \{\hat{R}q_i\} {R^qi}是全等的,因此 Σ 2 = 0 \Sigma^2 = 0 Σ2=0。从几何层面来考虑,很容易看出存在三种可能性。
1)
{
q
i
}
\{q_i\}
{qi}不共面,那么旋转的解是唯一的。进一步,没有反射
X
X
X可以使得
Σ
2
=
0
\Sigma^2 = 0
Σ2=0。因此,
S
V
D
SVD
SVD算法给出了一个期望的解。
2)
{
q
i
}
\{q_i\}
{qi}是共面但是不共线的,这里存在一个唯一的旋转和一个唯一的反射可以使得
Σ
2
=
0
\Sigma^2 = 0
Σ2=0。因此,
S
V
D
SVD
SVD算法可能给出另外一个解。我们将看到,这种情况很容易解决。
3)
{
q
i
}
\{q_i\}
{qi}是共线的,这里有无数种旋转和反射可以使得
Σ
2
=
0
\Sigma^2 = 0
Σ2=0.
现在我们回到共面的情况,通过检查
3
×
3
3 \times 3
3×3矩阵
H
H
H的元素,可以很容易发现当且仅当矩阵H的三个奇异值之中的一个为0时点集
{
q
i
}
\{q_i\}
{qi}是共面的。令三个奇异值为
λ
1
>
λ
2
>
λ
3
=
0
\lambda_1 > \lambda_2 > \lambda_3 = 0
λ1>λ2>λ3=0。然后
H
=
λ
1
u
i
v
1
T
+
λ
2
u
2
v
2
T
+
0
⋅
u
3
v
3
T
.
(19)
H = \lambda_1 u_i v_1^T + \lambda_2u_2v_2^T + 0 \cdot u_3 v_3^T. \tag{19}
H=λ1uiv1T+λ2u2v2T+0⋅u3v3T.(19)
这里
u
i
u_i
ui和
v
i
v_i
vi分别是矩阵
U
U
U和
V
V
V对应的列。注意,改变
u
3
u_3
u3或者
v
3
v_3
v3的符号不会改变
H
H
H。因此,如果
X
=
V
U
T
X = VU^T
X=VUT最小化了
Σ
2
\Sigma^2
Σ2,则
X
′
=
V
′
U
T
X^{'} = V^{'}U^T
X′=V′UT也是如此,这里
V
′
=
[
v
1
,
v
2
,
−
v
3
]
(20)
V^{'} = [v_1,v_2,-v_3] \tag{20}
V′=[v1,v2,−v3](20)
如果
X
X
X是一个反射,则
X
′
X^{'}
X′是一个旋转,反之依然。因此,如果
S
V
D
SVD
SVD分解给出了一个解
X
X
X且
d
e
t
(
X
)
=
−
1
det(X) = -1
det(X)=−1,我们只需要令
X
′
=
V
′
U
T
X^{'} = V^{'}U^T
X′=V′UT,这是我们需要的旋转。
我们顺便注意到,当且仅当矩阵 H H H的三个奇异值中的两个是相等时, { q i } \{q_i\} {qi}是共线的。
退化:带噪声的情况
如果 { q i } \{q_i\} {qi}或者 { q i ′ } \{q_i^{'}\} {qi′}是共面的,那么很容易证明前面的讨论仍然是有效的,除了 Σ 2 \Sigma^2 Σ2不再为0。因此,如果 S V D SVD SVD算法给出了一个反射 X = V U T X = VU^T X=VUT,我们只需要令 X ′ = V ′ U T X^{'} = V^{'}U^T X′=V′UT,这是我们需要的旋转。一种特殊的情况是当 N = 3 N=3 N=3并且 { q i } \{q_i\} {qi}或者 { q i ′ } \{q_i^{'}\} {qi′}是共面的点集。
我们无法处理的情况是 S V D SVD SVD算法给出了一个 d e t ( X ) = − 1 det(X) = -1 det(X)=−1的 X X X,并且 H H H的奇异值均不为0时。这意味着 { q i } \{q_i\} {qi}和 { q i ′ } \{q_i^{'}\} {qi′}都是共面的点集,但是没有一个旋转可以使得 Σ 2 \Sigma^2 Σ2比反射计算的值更小。这种情况只会发生在噪声 N i N_i Ni非常大时。在这种情况下,最小二乘解可能无论如何都是无效的。一种更好的方法是使用类似RANSAC的方法来去除外点。
算法总结
使用前述的方法,我们可以得到
X
=
V
U
T
X = VU^T
X=VUT
1)如果
d
e
t
(
X
)
=
+
1
det(X) = +1
det(X)=+1,则
X
X
X就是我们期望的旋转的解
2)如果
d
e
t
(
X
)
=
−
1
det(X) = -1
det(X)=−1,则
X
X
X是一个反射,对于这种情况,我们有以下两种更处理方法
-
如果矩阵 H H H的任何一个奇异值为0,则期望的旋转矩阵为
X = V ′ U T X = V^{'}U^T X=V′UT
这里 V ′ V^{'} V′是通过改变矩阵 V V V第三列的符号获得的。 -
如果矩阵 H H H的任何一个奇异值都不为0,则最小二乘方法可能是无效的,我们需要使用类似RANSAC的技术。
计算时间需求
在VAX 11/780上进行计算机模拟,以在时间需求方面比较三种算法(SVD,四元数,迭代)。在每次模拟中,都会生成一组3D点
{
p
i
}
\{p_i\}
{pi}。他们随机分布在一个中心为(0,0,0)的尺寸为
6
×
6
×
6
6\times6\times6
6×6×6的立方体中。然后将点
{
p
i
}
\{p_i\}
{pi}平移(80,60,70),接着绕着通过原点并且方向余弦为(0.6,0.7,0.39)的旋转轴旋转
7
5
°
75^{\degree}
75°,最后在结果点的每一个坐标上添加均值为0,标准差为0.5的高斯随机噪声来计算得到
{
p
i
′
}
\{p_i^{'}\}
{pi′}。然后使用三种算法来估计
R
^
\hat{R}
R^和
T
^
\hat{T}
T^,对应的CPU使用时间如表I所示。对于迭代算法,迭代次数在括号中给出。
我们注意到,SVD和四元数算法的计算时间需求是可比的,而迭代算法需要更长的时间。但是,在迭代算法中,解的计算精度为7位。如果我们可以接受百分之十的精度,那么迭代次数可以减少2-3倍。此外,收敛速率可以通过超松弛来增加。
感谢大佬ZJJ提供的md文件,互相学习交流,附上大佬的CSDN:https://blog.csdn.net/weixin_39061796?type=blog