一、ICP-SVD推导流程
1. 流程图
2. 有两个点集
其中
X
,
Y
X,Y
X,Y是两个原始点云的子集,选取的是两个点集中能相互关联的点,即
N
x
=
N
y
N_x=N_y
Nx=Ny
3. 目标
m
i
n
E
(
R
,
t
)
=
m
i
n
1
N
y
∑
i
=
1
N
y
∣
∣
x
i
−
R
y
i
−
t
∣
∣
2
minE(R,t)=min \frac {1} {N_y}\sum_{i=1}^{N_y}{||x_i-Ry_i-t||}^2
minE(R,t)=minNy1i=1∑Ny∣∣xi−Ryi−t∣∣2
取平方可以避免相减后结果为负,如果直接相加可能会导致正负抵消
4. 推导
其中
u
x
u_x
ux和
u
y
u_y
uy分别是点集
X
X
X和
Y
Y
Y的质心,即
∵
∑
i
=
1
N
y
(
x
i
−
u
x
)
=
0
\textcolor{red}{\because\sum_{i=1}^{N_y}(x_i-u_x)=0}
∵i=1∑Ny(xi−ux)=0
令
E
1
(
R
,
t
)
=
1
N
y
∑
i
=
1
N
y
∣
∣
x
i
−
u
x
−
R
(
y
i
−
u
y
)
∣
∣
2
E_1(R,t)= \frac {1} {N_y}\sum_{i=1}^{N_y}{||x_i-u_x-R(y_i-u_y)||}^2
E1(R,t)=Ny1∑i=1Ny∣∣xi−ux−R(yi−uy)∣∣2(只与旋转部分有关)
E
2
(
R
,
t
)
=
∣
∣
u
x
−
R
u
y
−
t
∣
∣
2
\quad E_2(R,t)={||u_x-Ru_y-t||}^2
E2(R,t)=∣∣ux−Ruy−t∣∣2(用于求平移部分)
则
E
(
R
,
t
)
=
E
1
+
E
2
E(R,t)=E_1+E_2
E(R,t)=E1+E2
那么对于任意的
R
R
R,均可以找到一个
t
t
t,使得
u
x
−
R
u
y
−
t
=
0
u_x-Ru_y-t=0
ux−Ruy−t=0,即
E
2
=
0
E_2=0
E2=0;
因此可以先根据
E
1
(
R
,
t
)
E_1(R,t)
E1(R,t)求旋转,在根据
E
2
(
R
,
t
)
E_2(R,t)
E2(R,t)求平移
所以接下来求解
E
1
\textcolor{red}{所以接下来求解E_1}
所以接下来求解E1
问题转化为找到合适的
R
R
R,使得
T
r
a
c
e
(
R
H
)
Trace(RH)
Trace(RH)达到最大值
a. 求解旋转部分
定理:若有正定矩阵
A
A
T
AA^T
AAT则对于任意正交矩阵
B
B
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)
意义:若能找到
R
R
R,把
T
r
a
c
e
(
R
H
)
Trace(RH)
Trace(RH)转换成
T
r
a
c
e
(
A
A
T
)
Trace(AA^T)
Trace(AAT)的形式,则该
R
R
R就是我们要找的旋转矩阵
方法:对
H
H
H进行SVD分解
H
=
U
Σ
V
T
R
=
V
U
T
R
H
=
V
U
T
U
Σ
V
T
=
V
Σ
V
T
=
V
Σ
1
2
Σ
1
2
V
T
=
V
Σ
1
2
(
V
Σ
1
2
)
T
H=U\Sigma V^T \\ R=VU^T \\ RH=VU^T U\Sigma V^T=V\Sigma V^T=V\Sigma^{\frac{1}{2}}\Sigma^{\frac{1}{2}}V^T=V\Sigma^{\frac{1}{2}}\left(V\Sigma^{\frac{1}{2}}\right)^T
H=UΣVTR=VUTRH=VUTUΣVT=VΣVT=VΣ21Σ21VT=VΣ21(VΣ21)T
b.求解平移部分
旋转
R
R
R确定后根据
E
2
(
R
,
t
)
=
∥
u
x
−
R
u
y
−
t
∥
2
E_2(R,t)=\|u_x-Ru_y-t\|^2
E2(R,t)=∥ux−Ruy−t∥2
直接得到平移量
t
=
u
x
−
R
u
y
t=u_x-Ru_y
t=ux−Ruy
二、ICP-基于优化推导流程
ICP求解问题,可以重新表示为
min
T
=
1
2
∑
i
=
1
n
∥
(
x
i
−
T
y
i
)
∥
2
2
\underset{T}{\min}=\frac{1}{2}\sum_{i=1}^n\left\|(x_i-Ty_i)\right\|_2^2
Tmin=21i=1∑n∥(xi−Tyi)∥22
其中
T
T
T为位姿变换矩阵
T
=
[
R
t
0
1
]
T=\begin{bmatrix}R&t\\ 0&1\end{bmatrix}
T=[R0t1]
在李代数模式下,又可以重新表示为
min
ξ
=
1
2
∑
i
=
1
n
∣
∣
(
x
i
−
exp
(
ξ
∧
)
y
i
)
∣
∣
2
2
\underset{\xi}{\min}=\frac{1}{2}\sum_{i=1}^n||(x_i-\exp(\xi^{\wedge})y_i)||_2^2
ξmin=21i=1∑n∣∣(xi−exp(ξ∧)yi)∣∣22
其中
ξ
\xi
ξ为
T
T
T对应的李代数
J
=
∂
e
∂
δ
ξ
=
−
(
exp
(
ξ
∧
)
y
i
)
⊙
J=\frac{\partial e}{\partial\delta\xi}=-\left(\exp\left(\xi^\wedge\right)y_i\right)^\odot
J=∂δξ∂e=−(exp(ξ∧)yi)⊙
随后,便可根据优化的固定步骤求解位姿。