点云处理-ICP算法推导
一、ICP算法简介
ICP算法可以用于匹配两个点云,得到两个点云之间的位姿(位移矩阵T和旋转矩阵R)转换关系。
二、ICP算法推导
对于两组点云:
X
=
x
1
,
x
2
,
.
.
.
x
n
P
=
p
1
,
p
2
,
.
.
.
p
n
X =x_1 ,x_2 ,...x_n \\ P =p_1 ,p_2 ,...p_n
X=x1,x2,...xnP=p1,p2,...pn
求解位移矩阵T和旋转矩阵R,使得下式最小:
E
(
R
,
T
)
=
1
N
∑
i
=
1
N
∣
∣
x
i
−
R
p
i
−
T
∣
∣
2
(
1
)
E(R,T) = \frac{1}{N}\begin{matrix} \sum_{i=1}^N ||x_i-Rp_i-T||^2 \end{matrix} \quad \quad \quad(1)
E(R,T)=N1∑i=1N∣∣xi−Rpi−T∣∣2(1)
计算点云质心
u
x
=
1
N
x
∑
i
=
1
N
x
x
i
u
p
=
1
N
p
∑
i
=
1
N
p
p
i
u_x= \frac{1}{N_x}\begin{matrix} \sum_{i=1}^{N_x} x_i \end{matrix}\\ u_p= \frac{1}{N_p}\begin{matrix} \sum_{i=1}^{N_p} p_i \end{matrix}
ux=Nx1∑i=1Nxxiup=Np1∑i=1Nppi
对式(1)进行变换:
E
(
R
,
T
)
=
1
N
∑
i
=
1
N
∣
∣
x
i
−
u
x
−
R
(
p
i
−
u
p
)
+
u
x
−
R
u
p
−
T
∣
∣
2
(
2
)
E(R,T) = \frac{1}{N}\begin{matrix} \sum_{i=1}^N ||x_i-u_x-R(p_i-u_p)+u_x-Ru_p-T||^2 \end{matrix} \qquad(2)
E(R,T)=N1∑i=1N∣∣xi−ux−R(pi−up)+ux−Rup−T∣∣2(2)
展开得到:
E
(
R
,
T
)
=
1
N
∑
i
=
1
N
∣
∣
x
i
−
u
x
−
R
(
p
i
−
u
p
)
∣
∣
2
+
∣
∣
u
x
−
R
u
p
−
T
∣
∣
2
+
2
(
x
i
−
u
x
−
R
(
p
i
−
u
p
)
)
(
u
x
−
R
u
p
−
T
)
(
3
)
E(R,T) = \frac{1}{N}\begin{matrix} \sum_{i=1}^N ||x_i-u_x-R(p_i-u_p)||^2+||u_x-Ru_p-T||^2+2(x_i-u_x-R(p_i-u_p))(u_x-Ru_p-T) \end{matrix} \qquad(3)
E(R,T)=N1∑i=1N∣∣xi−ux−R(pi−up)∣∣2+∣∣ux−Rup−T∣∣2+2(xi−ux−R(pi−up))(ux−Rup−T)(3)
式子(3)的第三项:
∑
i
=
1
N
(
x
i
−
u
x
−
R
(
p
i
−
u
p
)
)
(
u
x
−
R
u
p
−
T
)
=
(
u
x
−
R
u
p
−
T
)
∑
i
=
1
N
(
x
i
−
u
x
−
R
(
p
i
−
u
p
)
)
=
(
u
x
−
R
u
p
−
T
)
(
∑
i
=
1
N
x
i
−
N
u
x
−
R
(
∑
i
=
1
N
p
i
−
N
u
p
)
)
=
0
\begin{matrix} \sum_{i=1}^N (x_i-u_x-R(p_i-u_p))(u_x-Ru_p-T) \end{matrix} \\=(u_x-Ru_p-T)\begin{matrix} \sum_{i=1}^N (x_i-u_x-R(p_i-u_p)) \end{matrix} \\=(u_x-Ru_p-T)(\begin{matrix} \sum_{i=1}^N x_i \end{matrix} -Nu_x-R(\begin{matrix} \sum_{i=1}^N p_i \end{matrix}-Nu_p))\\=0
∑i=1N(xi−ux−R(pi−up))(ux−Rup−T)=(ux−Rup−T)∑i=1N(xi−ux−R(pi−up))=(ux−Rup−T)(∑i=1Nxi−Nux−R(∑i=1Npi−Nup))=0
因此式(3)简化为:
E
(
R
,
T
)
=
1
N
∑
i
=
1
N
∣
∣
x
i
−
u
x
−
R
(
p
i
−
u
p
)
∣
∣
2
+
∣
∣
u
x
−
R
u
p
−
T
∣
∣
2
(
4
)
E(R,T) = \frac{1}{N}\begin{matrix} \sum_{i=1}^N ||x_i-u_x-R(p_i-u_p)||^2+||u_x-Ru_p-T||^2 \end{matrix} \qquad(4)
E(R,T)=N1∑i=1N∣∣xi−ux−R(pi−up)∣∣2+∣∣ux−Rup−T∣∣2(4)
平移矩阵T只与式(4)第二项有关,可令:
1
N
∑
i
=
1
N
∣
∣
u
x
−
R
u
p
−
T
∣
∣
2
=
0
\frac{1}{N}\begin{matrix} \sum_{i=1}^N ||u_x-Ru_p-T||^2 \end{matrix} =0
N1∑i=1N∣∣ux−Rup−T∣∣2=0
则可以计算出平移矩阵T:
T
=
u
x
−
R
u
p
T=u_x-Ru_p
T=ux−Rup
则式(4)可以进一步简化:
E
(
R
,
T
)
=
1
N
∑
i
=
1
N
∣
∣
x
i
−
u
x
−
R
(
p
i
−
u
p
)
∣
∣
2
=
1
N
∑
i
=
1
N
∣
∣
x
^
i
−
R
p
^
i
∣
∣
2
=
1
N
∑
i
=
1
N
x
^
i
T
x
^
i
+
p
^
i
T
R
T
R
p
^
i
−
2
x
^
i
T
R
p
^
i
E(R,T) = \frac{1}{N}\begin{matrix} \sum_{i=1}^N ||x_i-u_x-R(p_i-u_p)||^2\end{matrix} \\= \frac{1}{N}\begin{matrix} \sum_{i=1}^N ||\widehat{x}_i-R\widehat{p}_i||^2\end{matrix} \\= \frac{1}{N}\begin{matrix} \sum_{i=1}^N {\widehat{x}_i}^T \widehat{x}_i+{\widehat{p}_i}^TR^TR \widehat{p}_i -2 {\widehat{x}_i}^TR \widehat{p}_i \end{matrix}
E(R,T)=N1∑i=1N∣∣xi−ux−R(pi−up)∣∣2=N1∑i=1N∣∣x
i−Rp
i∣∣2=N1∑i=1Nx
iTx
i+p
iTRTRp
i−2x
iTRp
i
第一项为常数;因为旋转矩阵R为正交矩阵,所以RTR为单位矩阵,则第二项也是常数。则E的最小值只与第三项有关。
则最后简化为取下式的最大值:
∑
i
=
1
N
x
^
i
T
R
p
^
i
=
∑
i
=
1
N
T
r
a
c
e
(
R
x
^
i
p
^
i
T
)
=
T
r
a
c
e
(
R
H
)
(
5
)
\begin{matrix} \sum_{i=1}^N {\widehat{x}_i}^T R \widehat{p}_i \end{matrix} =\begin{matrix} \sum_{i=1}^N Trace(R \widehat{x}_i {\widehat{p}_i}^T ) \end{matrix}= Trace(RH) \qquad(5)
∑i=1Nx
iTRp
i=∑i=1NTrace(Rx
ip
iT)=Trace(RH)(5)
其中:
H
=
∑
i
=
1
N
x
^
i
p
^
i
T
H=\begin{matrix} \sum_{i=1}^N \widehat{x}_i {\widehat{p}_i}^T \end{matrix}
H=∑i=1Nx
ip
iT
定理:
若A为正定对称矩阵,则对于任意的正交矩阵B,都有:
T
r
a
c
e
(
A
)
≥
T
r
a
c
e
(
B
A
)
Trace(A)\ge Trace(BA)
Trace(A)≥Trace(BA)
因此需要构建正定对称矩阵,对H进行SVD分解得到:
H
=
U
Λ
V
T
H=U \Lambda V^T
H=UΛVT
令一个变量X为:
X
=
V
U
T
X=V U^T
X=VUT
则X是正交矩阵,容易证明:
X X T = V U T ( V U T ) T = V U T U T T = E XX^T=V U^T(V U^T)^T=V U^TU T^T=E XXT=VUT(VUT)T=VUTUTT=E
得到:
X
H
=
V
U
T
U
Λ
V
T
=
V
Λ
V
T
XH=V U^TU \Lambda V^T=V \Lambda V^T
XH=VUTUΛVT=VΛVT
则XH为正定矩阵,因此可以应用定理得到:
T
r
a
c
e
(
X
H
)
≥
T
r
a
c
e
(
B
X
H
)
Trace(XH)\ge Trace(BXH)
Trace(XH)≥Trace(BXH)
可知,H乘以X时式(5)取最大值,所以:
R
=
X
=
V
U
T
R=X=V U^T
R=X=VUT
三、举例验证
由上述推导可得到结论:
R
=
V
U
T
T
=
u
x
−
R
u
p
R=V U^T \\ T=u_x-Ru_p
R=VUTT=ux−Rup
其中V和U由H的SVD分解得到。
下面我举一个简单的例子进行验证:
对于两组点云:
X
=
x
1
(
4
,
1
)
,
x
2
(
6
,
3
)
,
x
3
(
5
,
5
)
P
=
p
1
(
0
,
3
)
,
p
2
(
−
2
,
5
)
,
p
3
(
−
4
,
4
)
X =x_1(4,1) ,x_2(6,3) ,x_3(5,5) \\ P =p_1(0,3),p_2(-2,5) ,p_3(-4,4)
X=x1(4,1),x2(6,3),x3(5,5)P=p1(0,3),p2(−2,5),p3(−4,4)
各自质心:
u
x
=
(
5
,
3
)
u
p
=
(
−
2
,
4
)
u_x= (5,3)\\ u_p=(-2,4)
ux=(5,3)up=(−2,4)
则H为:
H
=
∑
i
=
1
N
x
^
i
p
^
i
T
=
[
−
2
2
−
8
2
]
H=\begin{matrix} \sum_{i=1}^N \widehat{x}_i {\widehat{p}_i}^T \end{matrix}= \begin{bmatrix} -2 & 2 \\ -8& 2 \end{bmatrix}
H=∑i=1Nx
ip
iT=[−2−822]
对H进行SVD分解:
H
=
U
Λ
V
T
=
[
−
0.2898
−
0.9571
−
0.9571
0.2898
]
[
8.6056
0
0
1.3944
]
[
0.9571
−
0.2898
−
0.2898
−
0.9571
]
H=U \Lambda V^T= \begin{bmatrix} -0.2898 & -0.9571\\ -0.9571 & 0.2898 \end{bmatrix} \begin{bmatrix} 8.6056 & 0\\ 0 & 1.3944 \end{bmatrix} \begin{bmatrix} 0.9571 &-0.2898\\ -0.2898 & -0.9571 \end{bmatrix}
H=UΛVT=[−0.2898−0.9571−0.95710.2898][8.6056001.3944][0.9571−0.2898−0.2898−0.9571]
则可以求出旋转矩阵R和平移矩阵T:
R
=
[
0
1
−
1
0
]
T
=
[
1
1
]
R= \begin{bmatrix} 0 & 1 \\ -1& 0 \end{bmatrix}\\ T= \begin{bmatrix} 1 \\ 1 \end{bmatrix}
R=[0−110]T=[11]