三维点云课程—ICP(Point-to-Point)
三维点云课程---ICP Point-to-Point
1.ICP问题描述
Iterative Closest Point (ICP)作为点云中的经典配准方法,现在进行原理的介绍。
给予两组点云数据,源点云(通常是旋转的)
P
=
{
p
1
,
.
.
.
,
p
N
p
}
,
p
i
∈
R
3
P=\{p_1,...,p_{N_p}\},p_i \in R^3
P={p1,...,pNp},pi∈R3,目标点云(参考点云,通常是固定的)
Q
=
{
q
1
,
.
.
.
,
q
N
1
}
,
q
i
∈
R
3
Q=\{q_1,...,q_{N_1}\},q_i \in R^3
Q={q1,...,qN1},qi∈R3,找到一个旋转矩阵
R
∈
R
3
,
R
R
T
=
I
,
∣
R
∣
=
1
R\in R^3,RR^T=I,|R|=1
R∈R3,RRT=I,∣R∣=1和平移向量
t
∈
R
3
t\in R^3
t∈R3,使得
P
,
Q
P,Q
P,Q达到"最好的配准"。那么"最好的配置",用数学公式表达如下
E
(
R
,
t
)
=
1
N
∑
t
=
1
N
∣
∣
q
i
−
R
p
i
−
t
∣
∣
2
E(R,t)=\frac{1}{N}{\sum \limits_{t=1}^N{||q_i-Rp_i-t||^2}}
E(R,t)=N1t=1∑N∣∣qi−Rpi−t∣∣2
上式假设已经知道哪几个点一一对应了。
2.ICP问题分析
将上述问题写成数学的表达如下
R
,
t
=
a
r
g
R
,
t
m
i
n
E
(
R
,
t
)
=
a
r
g
R
,
t
m
i
n
1
N
∑
t
=
1
N
∣
∣
q
i
−
R
p
i
−
t
∣
∣
2
R,t=arg_{R,t}minE(R,t)=arg_{R,t}min\frac{1}{N}{\sum \limits_{t=1}^N{||q_i-Rp_i-t||^2}}
R,t=argR,tminE(R,t)=argR,tminN1t=1∑N∣∣qi−Rpi−t∣∣2
此问题还可以扩展到
m
m
m维,其中
A
=
[
a
1
,
.
.
.
,
a
N
]
∈
R
m
×
N
,
B
=
[
b
1
,
.
.
.
,
b
N
]
∈
R
m
×
N
,
1
=
[
1
,
.
.
.
,
1
]
∈
R
N
A=[a_1,...,a_N] \in R^{m\times N},\\ B=[b_1,...,b_N] \in R^{m\times N},\\ 1=[1,...,1] \in R^N
A=[a1,...,aN]∈Rm×N,B=[b1,...,bN]∈Rm×N,1=[1,...,1]∈RN
那么
标
量
形
式
→
f
(
R
,
t
)
=
1
N
∑
t
=
1
N
∣
∣
b
i
−
R
a
i
−
t
∣
∣
2
=
∣
∣
B
−
(
R
A
+
t
1
T
)
∣
∣
F
2
←
矩
阵
形
式
min
R
,
t
f
(
R
,
t
)
,
s
.
t
.
R
R
T
=
I
m
,
∣
R
∣
=
1
标量形式\to f(R,t)=\frac{1}{N}{\sum \limits_{t=1}^N{||b_i-Ra_i-t||^2}}=||B-(RA+t1^T)||_F^2 \leftarrow 矩阵形式\\ \mathop {\min }\limits_{R,t}f(R,t),s.t.RR^T=I_m,|R|=1
标量形式→f(R,t)=N1t=1∑N∣∣bi−Rai−t∣∣2=∣∣B−(RA+t1T)∣∣F2←矩阵形式R,tminf(R,t),s.t.RRT=Im,∣R∣=1
关于上述如何将标量形式转为矩阵的形式,请参考附录1的证明。
现在给出ICP的解法
1.减去中心值进行归一化
A
,
B
A,B
A,B为
A
′
,
B
′
A',B'
A′,B′
L
=
I
N
−
1
N
1
1
T
A
′
=
A
L
B
′
=
B
L
L=I_N-\frac{1}{N}11^T\\ A'=AL\\ B'=BL
L=IN−N111TA′=ALB′=BL
2.对
B
′
A
′
T
B'A'^T
B′A′T进行SVD分解
B
′
A
′
T
=
U
Σ
V
T
B'A'^T=U \Sigma V^T
B′A′T=UΣVT
3.
R
,
t
R,t
R,t的最优解为
R
∗
=
U
V
T
t
∗
=
1
N
(
B
−
R
∗
A
)
1
R^*=UV^T\\ t^*=\frac{1}{N}{(B-R^*A)1}
R∗=UVTt∗=N1(B−R∗A)1
其中
A
1
N
,
B
1
N
\frac{A1}{N},\frac{B1}{N}
NA1,NB1分别为
{
a
i
}
,
{
b
i
}
\{a_i\},\{b_i\}
{ai},{bi}的平均值,记为
μ
a
,
μ
b
\mu_a,\mu_b
μa,μb,那么上式化简为
R
∗
=
U
V
T
t
∗
=
1
N
(
B
−
R
∗
A
)
1
=
μ
b
−
R
∗
μ
a
R^*=UV^T\\ t^*=\frac{1}{N}{(B-R^*A)1}=\mu_b-R^*\mu_a
R∗=UVTt∗=N1(B−R∗A)1=μb−R∗μa
3.ICP证明
化简
f
(
R
,
t
)
f(R,t)
f(R,t)
f
(
R
,
t
)
=
∣
∣
B
−
R
A
−
t
1
T
∣
∣
F
2
=
∣
∣
(
B
−
R
A
)
+
(
−
t
1
T
)
∣
∣
F
2
=
∣
∣
B
−
R
A
∣
∣
F
2
+
∣
∣
−
t
1
T
∣
∣
F
2
+
2
⟨
B
−
R
A
,
−
t
1
T
⟩
=
∣
∣
B
−
R
A
∣
∣
F
2
+
N
t
T
t
−
2
t
r
(
(
B
−
R
A
)
1
t
T
)
\begin{aligned} f(R,t)&=||B-RA-t1^T||_F^2\\ &=||(B-RA)+(-t1^T)||_F^2\\ &=||B-RA||_F^2+||-t1^T||_F^2+2\left\langle {B - RA, - t{1^T}} \right\rangle\\ &=||B-RA||_F^2+Nt^Tt-2tr((B-RA)1t^T) \\ \end{aligned}
f(R,t)=∣∣B−RA−t1T∣∣F2=∣∣(B−RA)+(−t1T)∣∣F2=∣∣B−RA∣∣F2+∣∣−t1T∣∣F2+2⟨B−RA,−t1T⟩=∣∣B−RA∣∣F2+NtTt−2tr((B−RA)1tT)
关于这部分的推导,请参考附录2的Frobenius范数的性质。
那么问题转化为求解 min R , t f ( R , t ) \mathop {\min }\limits_{R,t}f(R,t) R,tminf(R,t),第一想法是将 f ( R , t ) f(R,t) f(R,t)对 R , t R,t R,t分别进行求导,那么是先对 R R R进行求导还是先对 t t t进行求导呢?应该是先对 t t t进行求导,因为 R R R要满足 R R T = I , ∣ R ∣ = 1 RR^T=I,|R|=1 RRT=I,∣R∣=1的约束。
对
t
t
t进行求导
∂
f
∂
t
=
0
2
N
t
−
2
(
B
−
R
A
)
1
=
0
\begin {aligned} \frac{{\partial f}}{{\partial t}} = 0 \\ 2Nt-2(B-RA)1=0 \end {aligned}
∂t∂f=02Nt−2(B−RA)1=0
关于矩阵求导的一些性质参考附录3的矩阵求导
t
=
1
N
(
B
−
R
A
)
1
t=\frac{1}{N}{(B-RA)1}
t=N1(B−RA)1
将
t
t
t代入
f
(
R
,
t
)
f(R,t)
f(R,t)得
f
(
R
,
t
)
=
∣
∣
B
−
R
A
∣
∣
F
2
+
N
t
T
t
−
2
t
r
(
(
B
−
R
A
)
1
t
T
)
=
∣
∣
B
−
R
A
∣
∣
F
2
+
1
N
1
T
(
B
−
R
A
)
T
(
B
−
R
A
)
1
−
2
t
r
(
(
B
−
R
A
)
1
1
N
1
T
(
B
−
R
A
)
T
)
\begin {aligned} f(R,t)&=||B-RA||_F^2+Nt^Tt-2tr((B-RA)1t^T)\\ &=||B-RA||_F^2+\frac{1}{N}1^T(B-RA)^T(B-RA)1-2tr((B-RA)1\frac{1}{N}1^T(B-RA)^T) \end {aligned}
f(R,t)=∣∣B−RA∣∣F2+NtTt−2tr((B−RA)1tT)=∣∣B−RA∣∣F2+N11T(B−RA)T(B−RA)1−2tr((B−RA)1N11T(B−RA)T)
3.1 1 N 1 T ( B − R A ) T ( B − R A ) 1 \frac{1}{N}1^T(B-RA)^T(B-RA)1 N11T(B−RA)T(B−RA)1的化简
化简如下,关于如何将矩阵形式转为标量形式,请参考附录4的证明
1
N
1
T
(
B
−
R
A
)
T
(
B
−
R
A
)
1
=
[
1
.
.
.
1
]
[
(
b
1
−
R
a
1
)
T
.
.
.
(
b
N
−
R
a
N
)
T
]
[
(
b
1
−
R
a
1
)
.
.
.
(
b
N
−
R
a
N
)
]
[
1
.
.
.
1
]
=
1
N
(
(
(
b
1
−
R
a
1
)
T
+
.
.
.
+
(
b
N
−
R
a
N
)
T
)
(
(
b
1
−
R
a
1
)
+
.
.
.
(
b
N
−
R
a
N
)
)
)
=
1
N
(
∑
i
=
1
N
(
b
i
−
R
a
i
)
T
)
(
∑
i
=
1
N
(
b
i
−
R
a
i
)
)
\begin {aligned} \frac{1}{N}1^T(B-RA)^T(B-RA)1 &=\begin{bmatrix} 1&{...}&1 \end{bmatrix} \begin{bmatrix} {{{({b_1} - R{a_1})}^T}}\\ {...}\\ {{{({b_N} - R{a_N})}^T}} \end{bmatrix} \begin{bmatrix} {({b_1} - R{a_1})}&{...}&{({b_N} - R{a_N})} \end{bmatrix} \begin{bmatrix} 1\\ {...}\\ 1 \end{bmatrix}\\ &=\frac{1}{N}{\bigg(\Big((b_1-Ra_1)^T+...+(b_N-Ra_N)^T\Big) \Big((b_1-Ra_1)+...(b_N-Ra_N)\Big)\bigg)}\\ &=\frac{1}{N}{\Big(\sum \limits_{i=1}^N{(b_i-Ra_i)^T}\Big) \Big(\sum \limits_{i=1}^N{(b_i-Ra_i)}\Big)}\\ \end {aligned}
N11T(B−RA)T(B−RA)1=[1...1]⎣⎡(b1−Ra1)T...(bN−RaN)T⎦⎤[(b1−Ra1)...(bN−RaN)]⎣⎡1...1⎦⎤=N1(((b1−Ra1)T+...+(bN−RaN)T)((b1−Ra1)+...(bN−RaN)))=N1(i=1∑N(bi−Rai)T)(i=1∑N(bi−Rai))
又因为
μ
a
=
1
N
∑
i
=
1
N
a
i
∈
R
m
μ
b
=
1
N
∑
i
=
1
N
b
i
∈
R
m
\mu_a=\frac{1}{N}{\sum \limits_{i=1}^N {a_i} \in R^m}\\ \mu_b=\frac{1}{N}{\sum \limits_{i=1}^N {b_i} \in R^m}
μa=N1i=1∑Nai∈Rmμb=N1i=1∑Nbi∈Rm
那么上式可化简为
=
1
N
N
(
μ
b
−
R
μ
a
)
T
(
μ
b
−
R
μ
a
)
=
N
∣
∣
μ
b
−
R
μ
a
∣
∣
2
(
注
意
这
是
一
个
数
)
\begin {aligned} &=\frac{1}{N}{N(\mu_b-R\mu_a)^T(\mu_b-R\mu_a)}\\ &=N||\mu_b-R\mu_a||^2 (注意这是一个数) \end {aligned}
=N1N(μb−Rμa)T(μb−Rμa)=N∣∣μb−Rμa∣∣2(注意这是一个数)
故
1
N
1
T
(
B
−
R
A
)
T
(
B
−
R
A
)
1
=
N
∣
∣
μ
b
−
R
μ
a
∣
∣
2
,
μ
a
=
1
N
∑
i
=
1
N
a
i
∈
R
m
,
μ
b
=
1
N
∑
i
=
1
N
b
i
∈
R
m
\frac{1}{N}1^T(B-RA)^T(B-RA)1=N||\mu_b-R\mu_a||^2,\mu_a=\frac{1}{N}{\sum \limits_{i=1}^N {a_i} \in R^m},\mu_b=\frac{1}{N}{\sum \limits_{i=1}^N {b_i} \in R^m}
N11T(B−RA)T(B−RA)1=N∣∣μb−Rμa∣∣2,μa=N1i=1∑Nai∈Rm,μb=N1i=1∑Nbi∈Rm
3.2 2 t r ( ( B − R A ) 1 1 N 1 T ( B − R A ) T ) 2tr((B-RA)1\frac{1}{N}1^T(B-RA)^T) 2tr((B−RA)1N11T(B−RA)T)的化简
化简如下
2
t
r
(
(
B
−
R
A
)
1
1
N
1
T
(
B
−
R
A
)
T
)
=
2
1
N
t
r
(
[
(
b
1
−
R
a
1
)
.
.
.
(
b
N
−
R
a
N
)
]
[
1
.
.
.
1
]
[
1
.
.
.
1
]
[
(
b
1
−
R
a
1
)
T
.
.
.
(
b
N
−
R
a
N
)
T
]
)
=
2
1
N
t
r
(
(
(
b
1
−
R
a
1
)
+
.
.
.
(
b
N
−
R
a
N
)
)
(
(
b
1
−
R
a
1
)
T
+
.
.
.
+
(
b
N
−
R
a
N
)
T
)
)
)
=
2
1
N
t
r
(
(
∑
i
=
1
N
(
b
i
−
R
a
i
)
)
(
∑
i
=
1
N
(
b
i
−
R
a
i
)
T
)
)
=
2
1
N
t
r
(
N
(
μ
b
−
R
μ
a
)
N
(
μ
b
−
R
μ
a
)
T
)
=
2
N
t
r
(
(
μ
b
−
R
μ
a
)
(
μ
b
−
R
μ
a
)
T
)
=
2
N
∣
∣
μ
b
−
R
μ
a
∣
∣
2
\begin {aligned} 2tr((B-RA)1\frac{1}{N}1^T(B-RA)^T)&=2\frac{1}{N}tr(\begin{bmatrix} {({b_1} - R{a_1})}&{...}&{({b_N} - R{a_N})} \end{bmatrix} \begin{bmatrix} 1\\ {...}\\ 1 \end{bmatrix} \begin{bmatrix} 1&{...}&1 \end{bmatrix} \begin{bmatrix} {{{({b_1} - R{a_1})}^T}}\\ {...}\\ {{{({b_N} - R{a_N})}^T}} \end{bmatrix})\\ &=2\frac{1}{N}{tr\bigg(\Big((b_1-Ra_1)+...(b_N-Ra_N)\Big) \Big((b_1-Ra_1)^T+...+(b_N-Ra_N)^T)\Big)\bigg)}\\ &=2\frac{1}{N}{tr\Big((\sum \limits_{i=1}^N{(b_i-Ra_i)}\Big) \Big(\sum \limits_{i=1}^N{(b_i-Ra_i)^T})\Big)}\\ &=2\frac{1}{N}tr(N(\mu_b-R\mu_a)N(\mu_b-R\mu_a)^T)\\ &=2Ntr\Big((\mu_b-R\mu_a)(\mu_b-R\mu_a)^T\Big)\\ &=2N||\mu_b-R\mu_a||^2 \end {aligned}
2tr((B−RA)1N11T(B−RA)T)=2N1tr([(b1−Ra1)...(bN−RaN)]⎣⎡1...1⎦⎤[1...1]⎣⎡(b1−Ra1)T...(bN−RaN)T⎦⎤)=2N1tr(((b1−Ra1)+...(bN−RaN))((b1−Ra1)T+...+(bN−RaN)T)))=2N1tr((i=1∑N(bi−Rai))(i=1∑N(bi−Rai)T))=2N1tr(N(μb−Rμa)N(μb−Rμa)T)=2Ntr((μb−Rμa)(μb−Rμa)T)=2N∣∣μb−Rμa∣∣2
其实上式的推导的过程中用到一些关于矩阵迹的知识,详情参见附录5
3.3 进一步化简代价函数 f ( R , t ) f(R,t) f(R,t)
结合3.1和3.2的式子,化简如下
f
(
R
,
t
)
=
∣
∣
B
−
R
A
∣
∣
F
2
+
N
t
T
t
−
2
t
r
(
(
B
−
R
A
)
1
t
T
)
=
∣
∣
B
−
R
A
∣
∣
F
2
+
1
N
1
T
(
B
−
R
A
)
T
(
B
−
R
A
)
1
−
2
t
r
(
(
B
−
R
A
)
1
1
N
1
T
(
B
−
R
A
)
T
)
=
∑
i
=
1
N
∣
∣
b
i
−
R
a
i
∣
∣
2
+
N
∣
∣
μ
b
−
R
μ
a
∣
∣
2
−
2
N
∣
∣
μ
b
−
R
μ
a
∣
∣
2
=
∑
i
=
1
N
∣
∣
b
i
−
R
a
i
∣
∣
2
+
∑
i
=
1
N
∣
∣
μ
b
−
R
μ
a
∣
∣
2
−
2
N
(
μ
b
−
R
μ
a
)
T
(
μ
b
−
R
μ
a
)
=
∑
i
=
1
N
∣
∣
b
i
−
R
a
i
∣
∣
2
+
∑
i
=
1
N
∣
∣
μ
b
−
R
μ
a
∣
∣
2
−
2
∑
i
=
1
N
(
b
i
−
R
a
i
)
T
(
μ
b
−
R
μ
a
)
=
∑
i
=
1
N
∣
∣
(
b
i
−
R
a
i
)
−
(
μ
b
−
R
μ
a
)
∣
∣
2
=
∑
i
=
1
N
∣
∣
(
b
i
−
μ
b
)
−
R
(
a
i
−
μ
a
)
∣
∣
2
=
∑
i
=
1
N
∣
∣
b
i
′
−
R
a
i
′
∣
∣
2
=
∣
∣
B
′
−
R
A
′
∣
∣
F
2
\begin {aligned} f(R,t) &=||B-RA||_F^2+Nt^Tt-2tr((B-RA)1t^T)\\ &=||B-RA||_F^2+\frac{1}{N}1^T(B-RA)^T(B-RA)1-2tr((B-RA)1\frac{1}{N}1^T(B-RA)^T)\\ &=\sum \limits_{i=1}^N{||b_i-Ra_i||^2}+N||\mu_b-R\mu_a||^2-2N||\mu_b-R\mu_a||^2\\ &=\sum \limits_{i=1}^N{||b_i-Ra_i||^2}+\sum \limits_{i=1}^N{||\mu_b-R\mu_a||^2}-2N(\mu_b-R\mu_a)^T(\mu_b-R\mu_a)\\ &=\sum \limits_{i=1}^N{||b_i-Ra_i||^2}+\sum \limits_{i=1}^N{||\mu_b-R\mu_a||^2}-2\sum \limits_{i=1}^N{(b_i-Ra_i)^T}(\mu_b-R\mu_a)\\ &=\sum \limits_{i=1}^N{||(b_i-Ra_i)-(\mu_b-R\mu_a)||^2}\\ &=\sum \limits_{i=1}^N{||(b_i-\mu_b)-R(a_i-\mu_a)||^2}\\ &=\sum \limits_{i=1}^N{||b'_i-Ra'_i||^2}\\ &=||B'-RA'||_F^2 \end {aligned}
f(R,t)=∣∣B−RA∣∣F2+NtTt−2tr((B−RA)1tT)=∣∣B−RA∣∣F2+N11T(B−RA)T(B−RA)1−2tr((B−RA)1N11T(B−RA)T)=i=1∑N∣∣bi−Rai∣∣2+N∣∣μb−Rμa∣∣2−2N∣∣μb−Rμa∣∣2=i=1∑N∣∣bi−Rai∣∣2+i=1∑N∣∣μb−Rμa∣∣2−2N(μb−Rμa)T(μb−Rμa)=i=1∑N∣∣bi−Rai∣∣2+i=1∑N∣∣μb−Rμa∣∣2−2i=1∑N(bi−Rai)T(μb−Rμa)=i=1∑N∣∣(bi−Rai)−(μb−Rμa)∣∣2=i=1∑N∣∣(bi−μb)−R(ai−μa)∣∣2=i=1∑N∣∣bi′−Rai′∣∣2=∣∣B′−RA′∣∣F2
其中注意
A
A
A与
A
′
A'
A′和
B
B
B与
B
′
B'
B′的关系
a
′
=
a
i
−
μ
a
,
A
′
=
A
(
I
N
−
1
N
1
1
T
)
b
′
=
b
i
−
μ
b
,
B
′
=
B
(
I
N
−
1
N
1
1
T
)
a'=a_i-\mu_a,A'=A(I_N-\frac{1}{N}11^T)\\ b'=b_i-\mu_b,B'=B(I_N-\frac{1}{N}11^T)
a′=ai−μa,A′=A(IN−N111T)b′=bi−μb,B′=B(IN−N111T)
继续对上式的代价函数进行化简
f
(
R
,
t
)
=
∣
∣
B
′
−
R
A
′
∣
∣
F
2
=
∣
∣
B
′
∣
∣
F
2
+
∣
∣
−
R
A
′
∣
∣
F
2
+
2
⟨
B
′
,
−
R
A
′
⟩
=
∣
∣
B
′
∣
∣
F
2
+
∣
∣
R
A
′
∣
∣
F
2
−
2
⟨
B
′
,
R
A
′
⟩
=
∣
∣
B
′
∣
∣
F
2
+
∣
∣
A
′
∣
∣
F
2
−
2
t
r
(
R
A
′
B
′
T
)
=
∣
∣
B
′
∣
∣
F
2
+
∣
∣
A
′
∣
∣
F
2
−
2
t
r
(
R
V
Σ
U
T
)
=
∣
∣
B
′
∣
∣
F
2
+
∣
∣
A
′
∣
∣
F
2
−
2
t
r
(
U
T
R
V
Σ
)
=
∣
∣
B
′
∣
∣
F
2
+
∣
∣
A
′
∣
∣
F
2
−
2
t
r
(
T
Σ
)
=
∣
∣
B
′
∣
∣
F
2
+
∣
∣
A
′
∣
∣
F
2
−
2
∑
i
=
1
m
T
i
i
σ
i
\begin {aligned} f(R,t) &=||B'-RA'||_F^2\\ &=||B'||_F^2+||-RA'||_F^2+2\left\langle{B',-RA'}\right\rangle\\ &=||B'||_F^2+||RA'||_F^2-2\left\langle{B',RA'}\right\rangle\\ &=||B'||_F^2+||A'||_F^2-2tr(RA'B'^T)\\ &=||B'||_F^2+||A'||_F^2-2tr(RV\Sigma U^T)\\ &=||B'||_F^2+||A'||_F^2-2tr(U^T RV\Sigma )\\ &=||B'||_F^2+||A'||_F^2-2tr(T \Sigma)\\ &=||B'||_F^2+||A'||_F^2-2\sum \limits_{i=1}^m{T_{ii}\sigma_i} \end {aligned}
f(R,t)=∣∣B′−RA′∣∣F2=∣∣B′∣∣F2+∣∣−RA′∣∣F2+2⟨B′,−RA′⟩=∣∣B′∣∣F2+∣∣RA′∣∣F2−2⟨B′,RA′⟩=∣∣B′∣∣F2+∣∣A′∣∣F2−2tr(RA′B′T)=∣∣B′∣∣F2+∣∣A′∣∣F2−2tr(RVΣUT)=∣∣B′∣∣F2+∣∣A′∣∣F2−2tr(UTRVΣ)=∣∣B′∣∣F2+∣∣A′∣∣F2−2tr(TΣ)=∣∣B′∣∣F2+∣∣A′∣∣F2−2i=1∑mTiiσi
思考一下上式的推导过程中
∣
∣
R
A
′
∣
∣
F
2
=
∣
∣
A
′
∣
∣
F
2
||RA'||_F^2=||A'||_F^2
∣∣RA′∣∣F2=∣∣A′∣∣F2是怎么成立的,现在问题转化为最小化代价函数即最大化
∑
i
=
1
m
T
i
i
σ
i
\sum \limits_{i=1}^m{T_{ii}\sigma_i}
i=1∑mTiiσi,又因为
T
=
U
T
R
V
T=U^TRV
T=UTRV,而
U
,
R
,
V
U,R,V
U,R,V均是正交矩阵,所以
T
T
T也是正交矩阵。由于正交矩阵的每一个元素都进行了单位化,即
∣
T
i
i
∣
≤
1
|T_{ii}|\le 1
∣Tii∣≤1,要使
∑
i
=
1
m
T
i
i
σ
i
\sum \limits_{i=1}^m{T_{ii}\sigma_i}
i=1∑mTiiσi最大,即
T
=
I
m
T=I_m
T=Im,所以
T
=
U
T
R
V
=
I
m
R
=
U
V
T
t
=
1
N
(
B
−
R
A
)
1
\begin {aligned} T&=U^TRV=I_m\\ R&=UV^T\\ t&=\frac{1}{N}{(B-RA)1} \end {aligned}
TRt=UTRV=Im=UVT=N1(B−RA)1
4.ICP的完整步骤
给予两组点云集
- 源点云,旋转的 , P = { p 1 , . . . , p N p } , p i ∈ R 3 P=\{p_1,...,p_{N_{p}}\},p_i \in R^3 P={p1,...,pNp},pi∈R3
- 目标点云,固定的, Q = { q 1 , . . . , q N q } , q i ∈ R 3 Q=\{q_1,...,q_{N_{q}}\},q_i\in R^3 Q={q1,...,qNq},qi∈R3
- 数据预处理
- 对每一个点 p i p_i pi在 Q Q Q内寻找最近的邻居
- 移除外点对,如 ∣ ∣ p i − q i ∣ ∣ ||p_i-q_i|| ∣∣pi−qi∣∣太大
-
R
,
t
=
a
r
g
R
,
t
m
i
n
E
(
R
,
t
)
=
a
r
g
R
,
t
m
i
n
1
N
∑
t
=
1
N
∣
∣
q
i
−
R
p
i
−
t
∣
∣
2
R,t=arg_{R,t}minE(R,t)=arg_{R,t}min\frac{1}{N}{\sum \limits_{t=1}^N{||q_i-Rp_i-t||^2}}
R,t=argR,tminE(R,t)=argR,tminN1t=1∑N∣∣qi−Rpi−t∣∣2
- 计算中心点 μ p = 1 N ∑ i = 1 N p i \mu_p=\frac{1}{N}{\sum \limits_{i=1}^N{p_i}} μp=N1i=1∑Npi, μ q = 1 N ∑ i = 1 N q i \mu_q=\frac{1}{N}{\sum \limits_{i=1}^N{q_i}} μq=N1i=1∑Nqi
- P ′ = p i − μ p = P ( I N − 1 N ( 1 × 1 T ) ) P'={p_i-\mu_p}=P(I_N-\frac{1}{N}{(1\times 1^T)}) P′=pi−μp=P(IN−N1(1×1T)), Q ′ = q i − μ q = Q ( I N − 1 N ( 1 × 1 T ) ) Q'={q_i-\mu_q}=Q(I_N-\frac{1}{N}{(1\times 1^T)}) Q′=qi−μq=Q(IN−N1(1×1T))
- 计算 Q ′ P ′ T Q'P'^T Q′P′T的SVD分解,即 Q ′ P ′ T = U Σ V T Q'P'^T=U\Sigma V^T Q′P′T=UΣVT
- 计算 R = U V T , t = 1 N ( B − R A ) 1 = μ q − R μ q R=UV^T,t=\frac{1}{N}{(B-RA)1}=\mu_q-R\mu_q R=UVT,t=N1(B−RA)1=μq−Rμq
- 校验
- 评估迭代的收敛性
- E ( R , t ) E(R,t) E(R,t)比较小
- Δ R , Δ t \Delta R,\Delta t ΔR,Δt比较小
- 迭代没有终止
- P ← R P + t P \leftarrow RP+t P←RP+t
- 重复1-3步,直至迭代成功。
- 评估迭代的收敛性
5.ICP的缺点
上述的ICP方法是基于Point-to-Point的方法,会由于平面之间的滑动导致匹配错误的问题,且迭代次数比较多,需要进行对Point-to-Point的ICP方法进行改进,进而提出了Point-to-Plane的方法。但实际工程中Point-to-Point的方法比较简单,故还是有很多人使用它的。但不可否认Point-to-Plane的方法更优。
附录
1证明
标
量
形
式
→
f
(
R
,
t
)
=
1
N
∑
t
=
1
N
∣
∣
b
i
−
R
a
i
−
t
∣
∣
2
=
∣
∣
B
−
(
R
A
+
t
1
T
)
∣
∣
F
2
←
矩
阵
形
式
标量形式\to f(R,t)=\frac{1}{N}{\sum \limits_{t=1}^N{||b_i-Ra_i-t||^2}}=||B-(RA+t1^T)||_F^2 \leftarrow 矩阵形式\\
标量形式→f(R,t)=N1t=1∑N∣∣bi−Rai−t∣∣2=∣∣B−(RA+t1T)∣∣F2←矩阵形式
定义
A
,
B
,
R
,
t
A,B,R,t
A,B,R,t的矩阵形式如下
B
=
[
b
11
.
.
.
b
1
n
.
.
.
.
.
.
.
.
.
b
m
1
.
.
.
b
m
n
]
m
×
n
,
b
i
=
[
b
1
i
.
.
.
b
m
i
]
A
=
[
a
11
.
.
.
a
1
n
.
.
.
.
.
.
.
.
.
a
m
1
.
.
.
a
m
n
]
m
×
n
,
a
i
=
[
a
1
i
.
.
.
a
m
i
]
R
=
[
R
11
.
.
.
R
1
m
.
.
.
.
.
.
.
.
.
R
m
1
.
.
.
R
m
m
]
m
×
m
t
=
[
t
1
.
.
.
t
m
]
m
×
1
B = {\begin{bmatrix} {{b_{11}}}&{...}&{{b_{1n}}}\\ {...}&{...}&{...}\\ {{b_{m1}}}&{...}&{{b_{mn}}} \end{bmatrix}_{m \times n}},{b_i} =\begin{bmatrix} {{b_{1i}}}\\ {...}\\ {{b_{mi}}} \end{bmatrix}\\ A = {\begin{bmatrix} {{a_{11}}}&{...}&{{a_{1n}}}\\ {...}&{...}&{...}\\ {{a_{m1}}}&{...}&{{a_{mn}}} \end{bmatrix}_{m \times n}},{a_i} =\begin{bmatrix} {{a_{1i}}}\\ {...}\\ {{a_{mi}}} \end{bmatrix}\\ R = {\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}_{m \times m}}\\ t = {\begin{bmatrix} {{t_1}}\\ {...}\\ {{t_m}} \end{bmatrix}_{m \times 1}}
B=⎣⎡b11...bm1.........b1n...bmn⎦⎤m×n,bi=⎣⎡b1i...bmi⎦⎤A=⎣⎡a11...am1.........a1n...amn⎦⎤m×n,ai=⎣⎡a1i...ami⎦⎤R=⎣⎡R11...Rm1.........R1m...Rmm⎦⎤m×mt=⎣⎡t1...tm⎦⎤m×1
那么
B
−
R
A
−
t
1
T
=
[
b
11
.
.
.
b
1
n
.
.
.
.
.
.
.
.
.
b
m
1
.
.
.
b
m
n
]
m
×
n
−
[
R
11
.
.
.
R
1
m
.
.
.
.
.
.
.
.
.
R
m
1
.
.
.
R
m
m
]
m
×
m
×
[
a
11
.
.
.
a
1
n
.
.
.
.
.
.
.
.
.
a
m
1
.
.
.
a
m
n
]
m
×
n
−
[
t
1
.
.
.
t
m
]
m
×
1
×
[
1
.
.
.
1
]
1
×
N
=
[
b
11
−
(
R
11
×
a
11
+
.
.
.
+
R
1
m
×
a
m
1
)
−
t
1
.
.
.
b
1
n
−
(
R
11
×
a
1
n
+
.
.
.
+
R
1
m
×
a
m
n
)
−
t
1
.
.
.
.
.
.
.
.
.
b
m
1
−
(
R
m
1
×
a
11
+
.
.
.
+
R
m
m
×
a
m
1
)
−
t
m
.
.
.
b
m
n
−
(
R
m
1
×
a
1
n
+
.
.
.
+
R
m
m
×
a
m
n
)
−
t
m
]
B-RA-t1^T={\begin{bmatrix} {{b_{11}}}&{...}&{{b_{1n}}}\\ {...}&{...}&{...}\\ {{b_{m1}}}&{...}&{{b_{mn}}} \end{bmatrix}_{m \times n}} - {\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}_{m \times m}} \times {\begin{bmatrix} {{a_{11}}}&{...}&{{a_{1n}}}\\ {...}&{...}&{...}\\ {{a_{m1}}}&{...}&{{a_{mn}}} \end{bmatrix}_{m \times n}} - {\begin{bmatrix} {{t_1}}\\ {...}\\ {{t_m}} \end{bmatrix}_{m \times 1}} \times {\begin{bmatrix} 1&{...}&1 \end{bmatrix}_{1 \times N}}\\ =\begin{bmatrix} {{b_{11}} - ({R_{11}} \times {a_{11}} + ... + {R_{1m}} \times {a_{m1}}) - {t_1}}&{...}&{{b_{1n}} - ({R_{11}} \times {a_{1n}} + ... + {R_{1m}} \times {a_{mn}}) - {t_1}}\\ {...}&{...}&{...}\\ {{b_{m1}} - ({R_{m1}} \times {a_{11}} + ... + {R_{mm}} \times {a_{m1}}) - {t_m}}&{...}&{{b_{mn}} - ({R_{m1}} \times {a_{1n}} + ... + {R_{mm}} \times {a_{mn}}) - {t_m}} \end{bmatrix}
B−RA−t1T=⎣⎡b11...bm1.........b1n...bmn⎦⎤m×n−⎣⎡R11...Rm1.........R1m...Rmm⎦⎤m×m×⎣⎡a11...am1.........a1n...amn⎦⎤m×n−⎣⎡t1...tm⎦⎤m×1×[1...1]1×N=⎣⎡b11−(R11×a11+...+R1m×am1)−t1...bm1−(Rm1×a11+...+Rmm×am1)−tm.........b1n−(R11×a1n+...+R1m×amn)−t1...bmn−(Rm1×a1n+...+Rmm×amn)−tm⎦⎤
因为对于矩阵
A
∈
R
m
×
n
A\in R^{m \times n}
A∈Rm×n的最大Frobenius范数
∣
∣
A
∣
∣
F
2
||A||_F^2
∣∣A∣∣F2
∣
∣
A
∣
∣
F
2
=
∑
i
=
1
m
∑
j
=
1
n
∣
a
i
j
2
∣
=
t
r
a
c
e
(
A
∗
A
)
=
∑
i
=
1
m
i
n
{
m
,
n
}
σ
i
2
(
A
)
||A||_F^2=\sqrt{\sum \limits_{i=1}^m{\sum \limits_{j=1}^n}{|a_{ij}^2|}}=\sqrt{trace(A^*A)}=\sqrt{\sum \limits_{i=1}^{min\{m,n\}}{\sigma_i^2(A)}}
∣∣A∣∣F2=i=1∑mj=1∑n∣aij2∣=trace(A∗A)=i=1∑min{m,n}σi2(A)
其中
A
∗
A*
A∗表示为
A
A
A的共轭转置矩阵,在实数域中
A
∗
=
A
T
A^*=A^T
A∗=AT。
σ
i
\sigma_i
σi表示为
A
A
A矩阵进行SVD分解的奇异值。
那么根据Frobenius范数的定义得
右
式
=
∣
∣
B
−
R
A
−
t
1
T
∣
∣
F
2
=
∣
∣
b
11
−
(
R
11
×
a
11
+
.
.
.
+
R
1
m
×
a
m
1
−
t
1
)
∣
∣
2
+
.
.
.
+
∣
∣
b
m
n
−
(
R
m
1
×
a
1
n
+
.
.
.
+
R
m
m
×
a
m
n
−
t
m
)
∣
∣
2
右式=||B-RA-t1^T||_F^2=||b_{11}-(R_{11} \times a_{11}+...+R_{1m} \times a_{m1}-t_1)||^2+...+||b_{mn}-(R_{m1} \times a_{1n}+...+R_{mm} \times a_{mn}-t_m)||^2
右式=∣∣B−RA−t1T∣∣F2=∣∣b11−(R11×a11+...+R1m×am1−t1)∣∣2+...+∣∣bmn−(Rm1×a1n+...+Rmm×amn−tm)∣∣2
即
∣
∣
B
−
R
A
−
t
1
T
∣
∣
F
2
||B-RA-t1^T||_F^2
∣∣B−RA−t1T∣∣F2表示为矩阵中的每一个元素先进行平方后,再进行求和。
对于
b
i
−
R
a
i
−
t
=
[
b
1
i
.
.
.
b
m
i
]
−
[
R
11
.
.
.
R
1
m
.
.
.
.
.
.
.
.
.
R
m
1
.
.
.
R
m
m
]
m
×
m
×
[
a
1
i
.
.
.
a
m
i
]
−
[
t
1
.
.
.
t
m
]
m
×
1
=
[
b
1
i
−
(
R
11
×
a
1
i
+
.
.
.
+
R
1
m
×
a
m
i
)
−
t
1
.
.
.
b
m
i
−
(
R
m
1
×
a
1
i
+
.
.
.
+
R
m
m
×
a
m
i
)
−
t
m
]
b_i-Ra_i-t=\begin{bmatrix} {{b_{1i}}}\\ {...}\\ {{b_{mi}}} \end{bmatrix}-{\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}_{m \times m}} \times \begin{bmatrix} {{a_{1i}}}\\ {...}\\ {{a_{mi}}} \end{bmatrix}-{\begin{bmatrix} {{t_1}}\\ {...}\\ {{t_m}} \end{bmatrix}_{m \times 1}}\\ =\begin{bmatrix} {{b_{1i}} - ({R_{11}} \times {a_{1i}} + ... + {R_{1m}} \times {a_{mi}}) - {t_1}}\\ {...}\\ {{b_{mi}} - ({R_{m1}} \times {a_{1i}} + ... + {R_{mm}} \times {a_{mi}}) - {t_m}} \end{bmatrix}
bi−Rai−t=⎣⎡b1i...bmi⎦⎤−⎣⎡R11...Rm1.........R1m...Rmm⎦⎤m×m×⎣⎡a1i...ami⎦⎤−⎣⎡t1...tm⎦⎤m×1=⎣⎡b1i−(R11×a1i+...+R1m×ami)−t1...bmi−(Rm1×a1i+...+Rmm×ami)−tm⎦⎤
所以
左
式
=
∑
i
=
1
N
∣
∣
b
i
−
R
a
i
−
t
∣
∣
2
=
∣
∣
[
b
11
−
(
R
11
×
a
11
+
.
.
.
+
R
1
m
×
a
m
1
)
−
t
1
.
.
.
b
m
1
−
(
R
m
1
×
a
11
+
.
.
.
+
R
m
m
×
a
m
1
)
−
t
m
]
∣
∣
2
+
.
.
.
+
∣
∣
[
b
1
n
−
(
R
11
×
a
1
n
+
.
.
.
+
R
1
m
×
a
m
n
)
−
t
1
.
.
.
b
m
n
−
(
R
m
1
×
a
1
n
+
.
.
.
+
R
m
m
×
a
m
n
)
−
t
m
]
∣
∣
2
左式=\sum \limits_{i=1}^N{||b_i-Ra_i-t||^2}=||\begin{bmatrix} {{b_{11}} - ({R_{11}} \times {a_{11}} + ... + {R_{1m}} \times {a_{m1}}) - {t_1}}\\ {...}\\ {{b_{m1}} - ({R_{m1}} \times {a_{11}} + ... + {R_{mm}} \times {a_{m1}}) - {t_m}} \end{bmatrix}||^2 +...+ ||\begin{bmatrix} {{b_{1n}} - ({R_{11}} \times {a_{1n}} + ... + {R_{1m}} \times {a_{mn}}) - {t_1}}\\ {...}\\ {{b_{mn}} - ({R_{m1}} \times {a_{1n}} + ... + {R_{mm}} \times {a_{mn}}) - {t_m}} \end{bmatrix}||^2
左式=i=1∑N∣∣bi−Rai−t∣∣2=∣∣⎣⎡b11−(R11×a11+...+R1m×am1)−t1...bm1−(Rm1×a11+...+Rmm×am1)−tm⎦⎤∣∣2+...+∣∣⎣⎡b1n−(R11×a1n+...+R1m×amn)−t1...bmn−(Rm1×a1n+...+Rmm×amn)−tm⎦⎤∣∣2
化简得左式=右式。
2Frobenius范数的性质
∣
∣
A
∣
∣
F
2
=
∑
i
=
1
m
∑
j
=
1
n
∣
a
i
j
2
∣
=
t
r
a
c
e
(
A
∗
A
)
=
∑
i
=
1
m
i
n
{
m
,
n
}
σ
i
2
(
A
)
||A||_F^2=\sqrt{\sum \limits_{i=1}^m{\sum \limits_{j=1}^n}{|a_{ij}^2|}}=\sqrt{trace(A^*A)}=\sqrt{\sum \limits_{i=1}^{min\{m,n\}}{\sigma_i^2(A)}}
∣∣A∣∣F2=i=1∑mj=1∑n∣aij2∣=trace(A∗A)=i=1∑min{m,n}σi2(A)
Frobenius范数分解
∣
∣
A
+
B
∣
∣
F
2
=
∣
∣
A
∣
∣
F
2
+
∣
∣
B
∣
∣
F
2
+
2
⟨
A
,
B
⟩
F
||A+B||_F^2=||A||_F^2+||B||_F^2+2\left\langle {A,B} \right\rangle_F
∣∣A+B∣∣F2=∣∣A∣∣F2+∣∣B∣∣F2+2⟨A,B⟩F
其中
⟨
A
,
B
⟩
F
=
∑
i
j
A
i
j
B
i
j
=
t
r
(
A
T
B
)
=
t
r
(
A
B
T
)
\left\langle {A,B} \right\rangle_F=\sum \limits_{ij}{A_{ij}B_{ij}}=tr(A^TB)=tr(AB^T)
⟨A,B⟩F=ij∑AijBij=tr(ATB)=tr(ABT)
3矩阵的一些求导性质
∂
∂
X
T
r
(
X
)
=
I
∂
∂
X
T
r
(
X
A
)
=
A
T
∂
∂
X
T
r
(
A
X
B
)
=
A
T
B
T
∂
∂
X
T
r
(
A
X
T
B
)
=
B
A
∂
∂
X
T
r
(
X
T
A
)
=
A
∂
∂
X
T
r
(
A
X
T
)
=
A
∂
∂
X
T
r
(
A
)
⊗
X
=
T
r
(
A
)
I
∂
∂
X
β
T
X
=
β
∂
∂
X
X
T
X
=
2
X
∂
∂
X
X
T
A
X
=
(
A
+
A
T
)
X
\begin {aligned} \frac{\partial }{{\partial X}}{Tr(X)}&=I\\ \frac{\partial }{{\partial X}}{Tr(XA)}&=A^T\\ \frac{\partial }{{\partial X}}{Tr(AXB)}&=A^TB^T\\ \frac{\partial }{{\partial X}}{Tr(AX^TB)}&=BA\\ \frac{\partial }{{\partial X}}{Tr(X^TA)}&=A\\ \frac{\partial }{{\partial X}}{Tr(AX^T)}&=A\\ \frac{\partial }{{\partial X}}{Tr(A)\otimes X}&=Tr(A)I\\ \\ \frac{\partial }{{\partial X}}{\beta^T X}&=\beta\\ \frac{\partial }{{\partial X}}{X^T X} &=2X \\ \frac{\partial }{{\partial X}}{X^TAX} &=(A+A^T)X \end {aligned}
∂X∂Tr(X)∂X∂Tr(XA)∂X∂Tr(AXB)∂X∂Tr(AXTB)∂X∂Tr(XTA)∂X∂Tr(AXT)∂X∂Tr(A)⊗X∂X∂βTX∂X∂XTX∂X∂XTAX=I=AT=ATBT=BA=A=A=Tr(A)I=β=2X=(A+AT)X
上式公式的推导主要使用的是
∂
∂
X
T
r
(
A
X
T
)
=
A
∂
∂
X
X
T
X
=
2
X
\frac{\partial }{{\partial X}}{Tr(AX^T)}=A \\ \frac{\partial }{{\partial X}}{X^T X} =2X
∂X∂Tr(AXT)=A∂X∂XTX=2X
4证明
B
−
R
A
=
[
b
1
.
.
.
b
n
]
−
[
R
11
.
.
.
R
1
m
.
.
.
.
.
.
.
.
.
R
m
1
.
.
.
R
m
m
]
×
[
a
11
.
.
.
a
1
n
.
.
.
.
.
.
.
.
.
a
m
1
.
.
.
a
m
n
]
=
[
b
1
.
.
.
b
n
]
−
[
[
R
11
.
.
.
R
1
m
.
.
.
.
.
.
.
.
.
R
m
1
.
.
.
R
m
m
]
[
a
11
.
.
.
a
m
1
]
.
.
.
[
R
11
.
.
.
R
1
m
.
.
.
.
.
.
.
.
.
R
m
1
.
.
.
R
m
m
]
[
a
1
n
.
.
.
a
m
n
]
]
=
[
b
1
.
.
.
b
n
]
−
[
R
a
1
.
.
.
R
a
n
]
=
[
b
1
−
R
a
1
.
.
.
b
n
−
R
a
n
]
\begin {aligned} B-RA&=\begin{bmatrix} {{b_1}}&{...}&{{b_n}} \end{bmatrix} - \begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix} \times \begin{bmatrix} {{a_{11}}}&{...}&{{a_{1n}}}\\ {...}&{...}&{...}\\ {{a_{m1}}}&{...}&{{a_{mn}}} \end{bmatrix}\\ &=\begin{bmatrix} {{b_1}}&{...}&{{b_n}} \end{bmatrix} - \begin{bmatrix} {\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}\begin{bmatrix} {{a_{11}}}\\ {...}\\ {{a_{m1}}} \end{bmatrix}} &{...} &{\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}\begin{bmatrix} {{a_{1n}}}\\ {...}\\ {{a_{mn}}} \end{bmatrix}} \end{bmatrix}\\ &=\begin{bmatrix} {{b_1}}&{...}&{{b_n}} \end{bmatrix} - \begin{bmatrix} {R{a_1}}&{...}&{R{a_n}} \end{bmatrix}\\ &=\begin{bmatrix} {{b_1} - R{a_1}}&{...}&{{b_n} - R{a_n}} \end{bmatrix} \end {aligned}
B−RA=[b1...bn]−⎣⎡R11...Rm1.........R1m...Rmm⎦⎤×⎣⎡a11...am1.........a1n...amn⎦⎤=[b1...bn]−⎣⎡⎣⎡R11...Rm1.........R1m...Rmm⎦⎤⎣⎡a11...am1⎦⎤...⎣⎡R11...Rm1.........R1m...Rmm⎦⎤⎣⎡a1n...amn⎦⎤⎦⎤=[b1...bn]−[Ra1...Ran]=[b1−Ra1...bn−Ran]
5矩阵迹的性质
迹的定义
t
r
(
A
)
=
∑
i
=
1
n
a
i
i
=
a
11
+
a
22
+
.
.
.
+
a
n
n
tr(A)=\sum \limits_{i=1}^n{a_{ii}}=a_{11}+a_{22}+...+a_{nn}
tr(A)=i=1∑naii=a11+a22+...+ann
迹的线性性质
t
r
(
A
+
B
)
=
t
r
(
A
)
+
t
r
(
B
)
t
r
(
c
A
)
=
c
t
r
(
A
)
t
r
(
A
)
=
t
r
(
A
T
)
tr(A+B)=tr(A)+tr(B)\\ tr(cA)=ctr(A)\\ tr(A)=tr(A^T)
tr(A+B)=tr(A)+tr(B)tr(cA)=ctr(A)tr(A)=tr(AT)
迹的高级性质
t
r
(
A
T
B
)
=
t
r
(
A
B
T
)
=
t
r
(
B
T
A
)
=
t
r
(
B
A
T
)
=
∑
i
,
j
A
i
j
B
i
j
t
r
(
b
a
T
)
=
a
T
b
(
其
中
b
a
T
为
向
量
的
外
积
,
a
T
b
为
向
量
的
内
积
)
tr(A^TB)=tr(AB^T)=tr(B^TA)=tr(BA^T)=\sum \limits_{i,j}{A_{ij}B_{ij}}\\ tr(ba^T)=a^Tb(其中ba^T为向量的外积,a^Tb为向量的内积)
tr(ATB)=tr(ABT)=tr(BTA)=tr(BAT)=i,j∑AijBijtr(baT)=aTb(其中baT为向量的外积,aTb为向量的内积)
迹的循环性质
t
r
(
A
B
C
D
)
=
t
r
(
B
C
D
A
)
=
t
r
(
C
D
A
B
)
=
t
r
(
D
A
B
C
)
tr(ABCD)=tr(BCDA)=tr(CDAB)=tr(DABC)
tr(ABCD)=tr(BCDA)=tr(CDAB)=tr(DABC)