对齐两段不同坐标系的轨迹,实际就是已知两两3D点的对应关系,求取它们之间的R,T。这实际就是简化的ICP算法(省去了寻找点的对应关系这一步)。
设一段轨迹的3d点为pt, 一段为ps,那么它们之间的关系为:
s
∗
p
t
=
R
∗
p
s
+
T
s*p_t = R*p_s + T
s∗pt=R∗ps+T
假设两段轨迹的尺度是一样的,则s=1.
将R,T去耦合,我们知道点云的平均值也是满足旋转关系的,即
p
t
ˉ
=
R
∗
p
s
ˉ
+
T
\bar{p_t} = R * \bar{p_s} +T
ptˉ=R∗psˉ+T
故:
p
t
−
p
t
ˉ
=
R
∗
(
p
s
−
p
s
ˉ
)
p_t -\bar{p_t} = R*(p_s - \bar{p_s})
pt−ptˉ=R∗(ps−psˉ)
简写为:
q
t
=
R
∗
q
s
q_t = R * q_s
qt=R∗qs
对于所有的点
s
u
m
=
∑
i
N
∣
∣
q
t
i
−
R
q
s
i
∣
∣
2
sum = \sum^{N}_i{||q_t^i - Rq^i_s||^2}
sum=i∑N∣∣qti−Rqsi∣∣2
即求R使得sum最小。
s
u
m
=
∑
i
N
(
q
t
i
−
R
q
s
i
)
T
(
q
t
i
−
R
q
s
i
)
=
∑
i
N
(
q
t
i
T
∗
q
t
i
+
q
s
i
T
∗
q
s
i
−
q
t
i
T
∗
R
∗
q
s
i
−
q
s
i
T
R
t
∗
q
t
i
)
sum = \sum^N_i(q_t^i-Rq_s^i)^T(q_t^i-Rq_s^i) \\ = \sum^N_i({q_t^i}^T*q_t^i + {q_s^i}^T*q_s^i - {q_t^i}^T*R*q_s^i - {q_s^i}^TR^t*q_t^i)
sum=i∑N(qti−Rqsi)T(qti−Rqsi)=i∑N(qtiT∗qti+qsiT∗qsi−qtiT∗R∗qsi−qsiTRt∗qti)
R是单位对称矩阵,有:
R
=
R
T
a
T
∗
R
∗
b
=
b
T
∗
R
∗
a
R = R^T \\ a^T*R*b = b^T*R*a
R=RTaT∗R∗b=bT∗R∗a
故上面可以简化为:
∑
i
N
(
q
t
i
T
∗
q
t
i
+
q
s
i
T
∗
q
s
i
−
2
q
t
i
T
∗
R
∗
q
s
i
\sum^N_i({q_t^i}^T*q_t^i + {q_s^i}^T*q_s^i - 2{q_t^i}^T*R*q_s^i
i∑N(qtiT∗qti+qsiT∗qsi−2qtiT∗R∗qsi
即求R使得F最大
F
=
∑
i
N
q
t
i
T
∗
R
∗
q
s
i
=
T
r
a
c
e
(
∑
i
N
R
q
s
i
q
t
i
T
)
=
T
r
a
c
e
(
R
H
)
H
=
∑
i
N
(
q
s
i
q
t
i
T
)
F = \sum^N_i{q_t^i}^T*R*q_s^i \\ =Trace(\sum^N_i{Rq_s^i{q_t^i}^T}) \\ =Trace(RH) \\ H = \sum^N_i({q_s^i{q_t^i}^T})
F=i∑NqtiT∗R∗qsi=Trace(i∑NRqsiqtiT)=Trace(RH)H=i∑N(qsiqtiT)
上面转换是由于
T
r
a
c
e
(
A
B
)
=
T
r
a
c
e
(
B
A
)
Trace(AB) = Trace(BA)
Trace(AB)=Trace(BA)
当det(X) = -1 时,
因此我们最终的结果:
i
f
d
e
t
(
V
U
T
)
=
1
R
=
V
U
T
i
f
d
e
t
(
V
U
T
)
=
−
1
R
=
V
′
U
T
V
′
=
[
v
1
,
v
2
,
−
v
3
]
T
=
p
t
ˉ
−
R
∗
p
s
ˉ
if det(VU^T) = 1 \\ R = VU^T \\ if det(VU^T) = -1 \\ R = V'U^T \\ V' = [v1,v2,-v3] \\ T = \bar{p_t} - R * \bar{p_s}
ifdet(VUT)=1R=VUTifdet(VUT)=−1R=V′UTV′=[v1,v2,−v3]T=ptˉ−R∗psˉ
补充一点线性知识:
1.Trace迹,是矩阵主对角线的和。
对于方阵A,
A
=
Q
−
1
Λ
Q
A = Q^{-1} \Lambda Q
A=Q−1ΛQ
t
r
a
c
e
(
A
)
=
λ
1
+
λ
2
+
.
.
.
d
e
t
(
A
)
=
λ
1
∗
λ
2
∗
.
.
.
trace(A) = \lambda_1 + \lambda_2 +... \\ det(A) = \lambda_1 * \lambda_2 *...
trace(A)=λ1+λ2+...det(A)=λ1∗λ2∗...
如果A和B是相似矩阵
B
=
P
−
1
A
P
B = P^{-1} AP
B=P−1AP
那么
trace(A) = trace(B)
det(A) = det(B)
实际是因为A和B的特征值是一样的。
A可以看做是imu坐标系的旋转矩阵,B可以看做是wheel坐标系下的旋转矩阵
B= R_wheel2b.transpose() * A * R_wheel2b
哈哈,就是我们代码里常用的公式了
参考论文:Least-Squares Fitting of Two 3-D Point Sets