ICP问题 SVD方法推导(Markdown版)
目录
- 1 问题描述
- 2 解决方案
1 问题描述
利用SVD求解ICP问题的详细推导。
已知三维点集
P
{
P
1
,
P
2
,
⋯
,
P
n
}
P\{P_1,P_2,\cdots,P_n\}
P{P1,P2,⋯,Pn}和三维点集
Q
{
Q
1
,
Q
2
,
⋯
,
Q
n
}
Q\{Q_1,Q_2,\cdots,Q_n\}
Q{Q1,Q2,⋯,Qn}，且这两个点集已经进行了匹配，求其变换矩阵
T
T
T，
T
=
[
R
t
0
1
]
(1)
T=\begin{bmatrix} R & t\\ 0 & 1 \end{bmatrix} \tag{1}
T=[R0t1](1)
2 解决方案
将上述问题用数学公式描述如下，
m
i
n
R
,
t
∑
i
=
1
n
∥
Q
i
−
(
R
P
i
+
t
)
∥
2
=
m
i
n
R
,
t
∑
i
=
1
n
(
Q
i
−
R
P
i
−
t
)
T
(
Q
i
−
R
P
i
−
t
)
(2)
\underset{R,t}{min} \ \sum_{i=1}^n \Vert Q_i-(RP_i+t) \Vert^2=\underset{R,t}{min} \sum_{i=1}^n (Q_i-RP_i-t)^T(Q_i-RP_i-t) \tag{2}
R,tmin i=1∑n∥Qi−(RPi+t)∥2=R,tmini=1∑n(Qi−RPi−t)T(Qi−RPi−t)(2)
又因为，
Q
i
−
R
P
i
−
t
=
(
Q
i
−
Q
ˉ
)
−
R
(
P
i
−
P
ˉ
)
⏟
A
−
t
+
Q
ˉ
−
R
P
ˉ
⏟
B
(3)
Q_i-RP_i-t=\underbrace{(Q_i-\bar{Q})-R(P_i-\bar{P})}_{A} \underbrace{ -t+ \bar{Q}-R\bar{P} }_{B}\tag{3}
Qi−RPi−t=A
(Qi−Qˉ)−R(Pi−Pˉ)B
−t+Qˉ−RPˉ(3)
那么，
(
Q
i
−
R
P
i
−
t
)
T
(
Q
i
−
R
P
i
−
t
)
(Q_i-RP_i-t)^T(Q_i-RP_i-t)
(Qi−RPi−t)T(Qi−RPi−t)可以表示为，
(
Q
i
−
R
P
i
−
t
)
T
(
Q
i
−
R
P
i
−
t
)
=
(
A
+
B
)
T
(
A
+
B
)
=
A
T
A
+
A
T
B
+
B
T
A
+
B
T
B
=
A
T
A
+
B
T
B
+
2
A
T
B
(4)
(Q_i-RP_i-t)^T(Q_i-RP_i-t)=(A+B)^T(A+B)=A^TA+A^TB+B^TA+B^TB \\ = A^TA+B^TB+2A^TB \tag{4}
(Qi−RPi−t)T(Qi−RPi−t)=(A+B)T(A+B)=ATA+ATB+BTA+BTB=ATA+BTB+2ATB(4)
由于，
∑
i
=
1
n
2
A
T
B
=
2
(
∑
i
=
1
n
A
T
)
B
=
2
(
∑
i
=
1
n
A
)
T
B
(5)
\sum_{i=1}^n2A^TB=2\Big(\sum_{i=1}^n A^T \Big)B=2\Big(\sum_{i=1}^nA \Big)^TB \tag{5}
i=1∑n2ATB=2(i=1∑nAT)B=2(i=1∑nA)TB(5)
∑
i
=
1
n
A
=
∑
i
=
1
n
{
(
Q
i
−
Q
ˉ
)
−
R
(
P
i
−
P
ˉ
)
}
=
(
∑
i
=
1
n
Q
i
−
n
Q
ˉ
)
−
R
(
∑
i
=
1
n
P
i
−
n
P
ˉ
)
=
0
⃗
−
R
⋅
0
⃗
=
0
⃗
(6)
\sum_{i=1}^n A = \sum_{i=1}^n \Big\{ (Q_i-\bar{Q})-R(P_i - \bar{P}) \Big\} = \Big( \sum_{i=1}^n Q_i - n\bar{Q} \Big) - R\Big( \sum_{i=1}^nP_i - n\bar{P} \Big) = \vec{0} - R \cdot \vec{0} = \vec{0} \tag{6}
i=1∑nA=i=1∑n{(Qi−Qˉ)−R(Pi−Pˉ)}=(i=1∑nQi−nQˉ)−R(i=1∑nPi−nPˉ)=0−R⋅0=0(6)
故，公式(2)表示的优化问题，可以写成，
m
i
n
R
,
t
∑
i
=
1
n
(
Q
i
−
R
P
i
−
t
)
T
(
Q
i
−
R
P
i
−
t
)
=
m
i
n
R
,
t
∑
i
=
1
n
{
(
Q
i
′
−
R
P
i
′
)
T
(
Q
i
′
−
R
P
i
′
)
+
(
Q
ˉ
−
R
P
ˉ
−
t
)
T
(
Q
ˉ
−
R
P
ˉ
−
t
)
}
(7)
\underset{R,t}{min} \sum_{i=1}^n \Big( Q_i - RP_i-t \Big)^T(Q_i - RP_i - t) \\ = \underset{R,t}{min} \sum_{i=1}^n \Big\{ (Q_i'-RP_i')^T(Q_i'-RP_i') + (\bar{Q}-R\bar{P}-t)^T(\bar{Q}-R\bar{P}-t) \Big\} \tag{7}
R,tmini=1∑n(Qi−RPi−t)T(Qi−RPi−t)=R,tmini=1∑n{(Qi′−RPi′)T(Qi′−RPi′)+(Qˉ−RPˉ−t)T(Qˉ−RPˉ−t)}(7)
其中
Q
i
′
Q_i'
Qi′和
P
i
′
P_i'
Pi′分别表示
Q
i
Q_i
Qi和
P
i
P_i
Pi的去中心坐标。
那么，公式(7)表示的优化问题可以进一步写成如下两个优化问题，
m
i
n
R
∑
i
=
1
n
(
Q
i
′
−
R
P
i
′
)
T
(
Q
i
′
−
R
P
i
′
)
(8)
\underset{R}{min} \sum_{i=1}^n (Q_i'-RP_i')^T(Q_i'-RP_i') \tag{8}
Rmini=1∑n(Qi′−RPi′)T(Qi′−RPi′)(8)
m
i
n
R
,
t
∑
i
=
1
n
(
Q
ˉ
−
R
P
ˉ
−
t
)
T
(
Q
ˉ
−
R
P
ˉ
−
t
)
=
n
m
i
n
R
,
t
(
Q
ˉ
−
R
P
ˉ
−
t
)
T
(
Q
ˉ
−
R
P
ˉ
−
t
)
(9)
\underset{R,t}{min} \sum_{i=1}^n (\bar{Q}-R\bar{P}-t)^T(\bar{Q}-R\bar{P}-t) = n \ \underset{R,t}{min} (\bar{Q}-R\bar{P}-t)^T(\bar{Q}-R\bar{P}-t) \tag{9}
R,tmini=1∑n(Qˉ−RPˉ−t)T(Qˉ−RPˉ−t)=n R,tmin(Qˉ−RPˉ−t)T(Qˉ−RPˉ−t)(9)
对于公式(8)所表示的优化问题，有，
(
Q
i
′
−
R
P
i
′
)
T
(
Q
i
′
−
R
P
i
′
)
=
(
Q
i
′
T
−
P
i
′
T
R
T
)
(
Q
i
′
−
R
P
i
′
)
=
Q
i
′
T
Q
i
′
−
Q
i
′
T
R
P
i
′
−
P
i
′
T
R
T
Q
i
′
+
P
i
′
T
R
T
R
P
i
′
=
Q
i
′
T
Q
i
′
−
2
Q
i
′
T
R
P
i
′
+
P
i
′
T
P
i
′
(10)
(Q_i'-RP_i')^T(Q_i'-RP_i') = (Q_i'^T-P_i'^TR^T)(Q_i'-RP_i') \\ = Q_i'^TQ_i' - Q_i'^TRP_i'-P_i'^TR^TQ_i'+P_i'^TR^TRP_i' \\ = Q_i'^TQ_i'-2Q_i'^TRP_i'+P_i'^TP_i' \tag{10}
(Qi′−RPi′)T(Qi′−RPi′)=(Qi′T−Pi′TRT)(Qi′−RPi′)=Qi′TQi′−Qi′TRPi′−Pi′TRTQi′+Pi′TRTRPi′=Qi′TQi′−2Qi′TRPi′+Pi′TPi′(10)
故公式(8)的最优值
R
∗
R^*
R∗相当于如下优化问题的解，
m
a
x
R
∑
i
=
1
n
Q
i
′
T
R
P
i
′
=
m
a
x
R
∑
i
=
1
n
t
r
a
c
e
(
Q
i
′
T
R
P
i
′
)
(11)
\underset{R}{max} \sum_{i=1}^n Q_i'^TRP_i'=\underset{R}{max} \sum_{i=1}^n trace\Big( Q_i'^TRP_i' \Big) \tag{11}
Rmaxi=1∑nQi′TRPi′=Rmaxi=1∑ntrace(Qi′TRPi′)(11)
由于矩阵的迹具有性质
t
r
a
c
e
(
A
B
)
=
t
r
a
c
e
(
B
A
)
trace(AB)=trace(BA)
trace(AB)=trace(BA)，故上式可以写成，
m
a
x
R
∑
i
=
1
n
t
r
a
c
e
(
R
P
i
′
Q
i
′
T
)
=
m
a
x
R
t
r
a
c
e
(
R
∑
i
=
1
n
P
i
′
Q
i
′
T
)
(12)
\underset{R}{max} \sum_{i=1}^n trace\Big( RP_i'Q_i'^T \Big) = \underset{R}{max}\ trace\Big( R \sum_{i=1}^n P_i'Q_i'^T \Big) \tag{12}
Rmaxi=1∑ntrace(RPi′Qi′T)=Rmax trace(Ri=1∑nPi′Qi′T)(12)
记
W
=
∑
i
=
1
n
P
i
′
Q
i
′
T
W=\sum_{i=1}^n P_i'Q_i'^T
W=∑i=1nPi′Qi′T，上式可以写成，
m
a
x
R
t
r
a
c
e
(
R
W
)
(13)
\underset{R}{max} \ trace\Big( RW \Big) \tag{13}
Rmax trace(RW)(13)
考虑到如下定理，
定理：若有正定矩阵 A A T AA^T AAT，则对于任意正定矩阵 B B B，有 t r a c e ( B A A T ) ≤ t r a c e ( A A T ) trace(BAA^T) \leq trace(AA^T) trace(BAAT)≤trace(AAT)。
那么，依据上述定理，公式(13)的最优值
R
∗
R^{*}
R∗满足，矩阵
R
∗
W
R^*W
R∗W是一个正定矩阵。不妨对矩阵
W
W
W进行奇异值分解，有，
W
=
U
Σ
V
T
(14)
W = U\Sigma V^T \tag{14}
W=UΣVT(14)
那么当矩阵
R
R
R取为
V
U
T
VU^T
VUT时，
R
W
RW
RW可以表示为，
R
W
=
V
U
T
⋅
U
Σ
V
T
=
V
Σ
V
T
=
(
V
Σ
1
2
)
(
V
Σ
1
2
)
T
(15)
RW = VU^T \cdot U\Sigma V^T = V\Sigma V^T = \Big(V\Sigma^{\frac{1}{2}} \Big) \Big(V\Sigma^{\frac{1}{2}} \Big)^T \tag{15}
RW=VUT⋅UΣVT=VΣVT=(VΣ21)(VΣ21)T(15)
为正定矩阵，取最大值。
故，
R
∗
=
V
U
T
(16)
R^* = VU^T \tag{16}
R∗=VUT(16)
即证。