一背景概述
语义定位比起点云定位越来越流行,并且具有诸多有点,如运算量少,特征稳定,类人思维,便于融合等。
二 定位方法
2.1 ICP方法
数学建模如下。
2.1.1 字符定义
同一组点在坐标系a和坐标系b 下分别表达成如下点集。
{
P
i
a
}
,
{
P
i
b
}
,
\begin{Bmatrix}\ P_i^a \end{Bmatrix},\begin{Bmatrix}\ P_i^b \end{Bmatrix},
{ Pia},{ Pib},
在a系中的重心和b系中的重心分别如下。
{
P
a
}
,
{
P
b
}
,
\begin{Bmatrix}\ P^a \end{Bmatrix} , \begin{Bmatrix} P^b \end{Bmatrix},
{ Pa},{Pb},
扣除在a系中的重心和b系中的重心分别如下。
{
Q
i
a
}
=
{
P
i
a
−
P
a
}
{
Q
i
b
}
=
{
P
i
b
−
P
b
}
\begin{Bmatrix}\ Q_i^a \end{Bmatrix} = \begin{Bmatrix}\ P_i^a- P^a \end{Bmatrix}\\ \begin{Bmatrix} Q_i^b \end{Bmatrix} = \begin{Bmatrix}\ P_i^b- P^b \end{Bmatrix}\\
{ Qia}={ Pia−Pa}{Qib}={ Pib−Pb}
2.1.2 目标函数
M
i
n
F
=
Σ
∥
P
i
a
−
(
R
∗
P
i
b
+
t
)
∥
2
=
Σ
∥
(
P
i
a
−
P
a
)
−
(
R
∗
(
P
i
b
−
P
b
)
+
t
)
+
P
a
−
R
∗
P
b
∥
2
=
Σ
∥
Q
i
a
−
R
∗
Q
i
b
+
P
a
−
R
∗
P
b
−
t
∥
2
=
Σ
∥
Q
i
a
−
R
∗
Q
i
b
∥
2
+
Σ
∥
P
a
−
R
∗
P
b
−
t
∥
2
+
2
∗
Σ
(
Q
i
a
−
R
∗
Q
i
b
)
T
∗
(
P
a
−
R
∗
P
b
−
t
)
\begin{aligned} Min\quad F &=\Sigma \parallel P_i^a-(R*P_i^b+t) \parallel ^2 \\ &=\Sigma \parallel (P_i^a-P^a)-(R*(P_i^b-P^b)+t) + P^a - R*P^b\parallel ^2 \\ &= \Sigma \parallel Q_i^a-R*Q_i^b + P^a - R*P^b-t\parallel^2 \\ &=\Sigma \parallel Q_i^a-R*Q_i^b \parallel^2 + \Sigma \parallel P^a - R*P^b-t\parallel^2 + 2* \Sigma(Q_i^a-R*Q_i^b )^T*(P^a - R*P^b-t) \\ \end{aligned}
MinF=Σ∥Pia−(R∗Pib+t)∥2=Σ∥(Pia−Pa)−(R∗(Pib−Pb)+t)+Pa−R∗Pb∥2=Σ∥Qia−R∗Qib+Pa−R∗Pb−t∥2=Σ∥Qia−R∗Qib∥2+Σ∥Pa−R∗Pb−t∥2+2∗Σ(Qia−R∗Qib)T∗(Pa−R∗Pb−t)
第三项类累加为0.于是目标函数简化为下式。
M
i
n
F
=
Σ
∥
Q
i
a
−
R
∗
Q
i
b
∥
2
+
Σ
∥
P
a
−
R
∗
P
b
−
t
∥
2
\begin{aligned} Min\quad F &=\Sigma \parallel Q_i^a-R*Q_i^b \parallel^2 + \Sigma \parallel P^a - R*P^b-t\parallel^2 \end{aligned}
MinF=Σ∥Qia−R∗Qib∥2+Σ∥Pa−R∗Pb−t∥2
上式第二项对于任意的R均可以求出t使得第二项为0.
因此最终的目标函数如下。
M
i
n
F
=
Σ
∥
Q
i
a
−
R
∗
Q
i
b
∥
2
\begin{aligned} Min\quad F &=\Sigma \parallel Q_i^a-R*Q_i^b \parallel^2 \end{aligned}
MinF=Σ∥Qia−R∗Qib∥2
2.1.3 求解函数
M
i
n
F
=
Σ
(
Q
i
a
−
R
∗
Q
i
b
)
T
∗
(
Q
i
a
−
R
∗
Q
i
b
)
=
Σ
Q
i
a
T
∗
Q
i
a
+
Σ
Q
i
b
T
∗
Q
i
b
−
Σ
Q
i
a
T
∗
R
∗
Q
i
b
−
Σ
Q
i
b
T
∗
R
T
∗
Q
i
a
\begin{aligned} Min\quad F &=\Sigma (Q_i^a-R*Q_i^b)^T *(Q_i^a-R*Q_i^b) \\ &=\Sigma {Q_i^a}^T*Q_i^a + \Sigma {Q_i^b}^T*Q_i^b - \Sigma {Q_i^a}^T*R*Q_i^b - \Sigma {Q_i^b}^T*R^T*Q_i^a \end{aligned}
MinF=Σ(Qia−R∗Qib)T∗(Qia−R∗Qib)=ΣQiaT∗Qia+ΣQibT∗Qib−ΣQiaT∗R∗Qib−ΣQibT∗RT∗Qia
前两项是常数,后两项相等。因此可以化简得到下式。
M
i
n
F
=
−
2
∗
Σ
(
Q
i
a
T
∗
R
∗
Q
i
b
)
=
−
2
∗
Σ
t
r
(
R
∗
Q
i
b
∗
Q
i
a
T
)
=
−
2
∗
t
r
(
R
∗
Σ
(
Q
i
b
∗
Q
i
a
T
)
)
\begin{aligned} Min\quad F &= - 2*\Sigma( {Q_i^a}^T*R*Q_i^b )\\ &= - 2*\Sigma tr(R*Q_i^b*{Q_i^a}^T) \\ &= -2*tr(R*\Sigma (Q_i^b*{Q_i^a}^T)) \end{aligned}
MinF=−2∗Σ(QiaT∗R∗Qib)=−2∗Σtr(R∗Qib∗QiaT)=−2∗tr(R∗Σ(Qib∗QiaT))
令
W
=
Σ
(
Q
i
b
∗
Q
i
a
T
)
=
U
∗
σ
∗
V
T
W=\Sigma (Q_i^b*{Q_i^a}^T)=U*\sigma*V^T
W=Σ(Qib∗QiaT)=U∗σ∗VT
于是利用矩阵迹的交换一致性质,目标函数
M
i
n
F
=
−
t
r
(
R
∗
U
∗
σ
∗
V
T
)
=
−
t
r
(
V
T
∗
R
∗
U
∗
σ
)
\begin{aligned} Min\quad F &=-tr(R*U* \sigma*V^T) \\ &=-tr(V^T*R*U* \sigma) \\ \end{aligned}
MinF=−tr(R∗U∗σ∗VT)=−tr(VT∗R∗U∗σ)
容易知道
V
T
∗
R
∗
U
V^T*R*U
VT∗R∗U
是一个旋转矩阵,当它为单位矩阵时,取得最大迹值,即
V
T
∗
R
∗
U
=
E
R
=
V
∗
U
T
=
(
U
∗
V
T
)
T
V^T*R*U = E\\ R=V*U^T=(U*V^T)^T
VT∗R∗U=ER=V∗UT=(U∗VT)T
2.2 改造ICP方法
在语义定位中,每个语义的权重不一样,并且存在多种形式的语义,包括点语义、线语义和形状语义。
点语义的表达2.1节已经描述。
2.2.1 线语义可以表达为下式。
M
i
n
F
=
Σ
(
w
i
2
∗
∥
L
i
a
−
R
∗
L
i
b
∥
2
)
+
Σ
∥
(
P
i
a
−
R
∗
P
i
b
−
t
)
×
L
i
a
∥
2
=
Σ
(
∥
w
i
∗
L
i
a
−
R
∗
w
i
∗
L
i
b
∥
2
)
+
Σ
∥
(
P
i
a
×
L
i
a
−
R
∗
P
i
b
×
L
i
a
−
t
×
L
i
a
)
∥
2
=
Σ
(
∥
w
i
∗
L
i
a
−
R
∗
w
i
∗
L
i
b
∥
2
)
+
Σ
∥
(
P
L
i
a
−
R
∗
P
L
i
b
−
t
L
)
∥
2
=
Σ
(
∥
w
i
∗
L
i
a
−
R
∗
w
i
∗
L
i
b
∥
2
)
+
Σ
∥
(
Q
L
i
a
−
R
∗
Q
L
i
b
)
∥
2
+
Σ
∥
(
P
L
a
−
R
∗
P
L
b
−
t
L
)
∥
2
\begin{aligned} Min\quad F &=\Sigma (w_i^2*\parallel L_i^a-R*L_i^b\parallel ^2) +\Sigma \parallel (P_i^a-R*P_i^b-t) \times L_i^a \parallel^2 \\ &=\Sigma (\parallel w_i*L_i^a-R*w_i*L_i^b\parallel ^2) +\Sigma \parallel (P_i^a\times L_i^a-R*P_i^b \times L_i^a-t\times L_i^a) \parallel^2 \\ &=\Sigma (\parallel w_i*L_i^a-R*w_i*L_i^b\parallel ^2) +\Sigma \parallel (PL_i^a-R*PL_i^b-tL) \parallel^2 \\ &=\Sigma (\parallel w_i*L_i^a-R*w_i*L_i^b\parallel ^2) +\Sigma \parallel (QL_i^a-R*QL_i^b) \parallel^2 +\Sigma \parallel (PL^a-R*PL^b-tL) \parallel^2\\ \end{aligned}
MinF=Σ(wi2∗∥Lia−R∗Lib∥2)+Σ∥(Pia−R∗Pib−t)×Lia∥2=Σ(∥wi∗Lia−R∗wi∗Lib∥2)+Σ∥(Pia×Lia−R∗Pib×Lia−t×Lia)∥2=Σ(∥wi∗Lia−R∗wi∗Lib∥2)+Σ∥(PLia−R∗PLib−tL)∥2=Σ(∥wi∗Lia−R∗wi∗Lib∥2)+Σ∥(QLia−R∗QLib)∥2+Σ∥(PLa−R∗PLb−tL)∥2
QL_i代表PL_i(P叉乘L结果)去重心,PL表示{PL_i}点集的重心。对于任意R,都能有tL使第三项为0.可以参考2.1节以及目标函数求解过程得出。
W
=
Σ
(
w
i
∗
L
i
b
)
∗
(
w
i
∗
L
i
a
)
T
+
Σ
(
Q
L
i
b
)
∗
(
Q
L
i
a
)
T
=
U
∗
σ
∗
V
T
\begin{aligned} W = \Sigma (w_i*L_i^b)*(w_i*L_i^a)^T + \Sigma (QL_i^b)*(QL_i^a)^T=U*\sigma*V^T \end{aligned}
W=Σ(wi∗Lib)∗(wi∗Lia)T+Σ(QLib)∗(QLia)T=U∗σ∗VT
对W 进行SVD分解可以求出R。
V
T
∗
R
∗
U
=
E
R
=
V
∗
U
T
=
(
U
∗
V
T
)
T
V^T*R*U = E\\ R=V*U^T=(U*V^T)^T
VT∗R∗U=ER=V∗UT=(U∗VT)T
2.2.2 形状语义的表达式可以表达为下式。
M
i
n
F
=
Σ
(
w
i
2
∗
∥
L
i
a
−
R
∗
L
i
b
∥
2
)
+
Σ
∥
P
i
a
−
R
∗
P
i
b
−
t
∥
2
\begin{aligned} Min\quad F &=\Sigma (w_i^2*\parallel L_i^a-R*L_i^b\parallel ^2) +\Sigma \parallel P_i^a-R*P_i^b-t\parallel^2 \end{aligned}
MinF=Σ(wi2∗∥Lia−R∗Lib∥2)+Σ∥Pia−R∗Pib−t∥2
令Q_i是形状语义的位置,L_i是形状语义决定的方向。一个形状语义又位置和方向唯一确定。Q_i代表P_i去重心,P表示{P_i}点集的重心。
W
=
Σ
(
w
i
∗
L
i
b
)
∗
(
w
i
∗
L
i
a
)
T
+
Σ
(
Q
i
b
)
∗
(
Q
i
a
)
T
=
U
∗
σ
∗
V
T
\begin{aligned} W = \Sigma (w_i*L_i^b)*(w_i*L_i^a)^T + \Sigma (Q_i^b)*(Q_i^a)^T=U*\sigma*V^T \end{aligned}
W=Σ(wi∗Lib)∗(wi∗Lia)T+Σ(Qib)∗(Qia)T=U∗σ∗VT
对W 进行SVD分解可以求出R。
V
T
∗
R
∗
U
=
E
R
=
V
∗
U
T
=
(
U
∗
V
T
)
T
V^T*R*U = E\\ R=V*U^T=(U*V^T)^T
VT∗R∗U=ER=V∗UT=(U∗VT)T
2.2.3 多语义融合求解
如果同时存在三种语义,只需要三种语义W矩阵相加再求SVD分解就可以。
W
=
W
p
o
i
n
t
+
W
l
i
n
e
+
W
s
h
a
p
e
W = W_{point} + W_{line} + W_{shape}
W=Wpoint+Wline+Wshape
三 扩展:SVD分解应用
3.1 求最佳旋转矩阵
3.1.1 求旋转矩阵R
第二节已经描述
3.1.2 求四元数q
在SLAM中经常遇到求姿态问题描述如下。
A
∗
q
=
0
,
∥
q
∥
=
1
A*q=0, \parallel q \parallel = 1
A∗q=0,∥q∥=1
上述问题相当于是带条件最优化的问题,可以使用拉格朗日乘子法。 但是上述求姿态问题也可以使用SVD分解。
A
=
U
∗
Σ
∗
V
T
A
∗
q
=
U
∗
Σ
∗
V
T
∗
q
=
0
A = U*\Sigma*V^T \\ A*q=U*\Sigma*V^T*q=0
A=U∗Σ∗VTA∗q=U∗Σ∗VT∗q=0
上式求得q应该使左边的式子得到最小的非零值,U和V是旋转矩阵并不会影响最终向量的模。V*q仍然是一个模值的单位向量。
m
i
n
(
∥
A
∗
q
∥
)
=
m
i
n
(
∥
Σ
∗
V
T
∗
q
∥
)
=
m
i
n
(
∥
Σ
∗
n
∥
)
min (\parallel A*q \parallel)=min (\parallel \Sigma*V^T*q \parallel)=min (\parallel \Sigma*n \parallel)
min(∥A∗q∥)=min(∥Σ∗VT∗q∥)=min(∥Σ∗n∥)
Σ
=
[
σ
1
0
0
0
0
σ
2
0
0
0
0
σ
3
0
0
0
0
σ
4
]
满足条件
σ
1
≥
σ
2
≥
σ
3
≥
σ
4
\Sigma= \begin{bmatrix} \sigma_1 &0 &0 &0 \\ 0 &\sigma_2 &0 &0 \\ 0 &0 &\sigma_3 &0 \\ 0 &0 &0 &\sigma_4 \end{bmatrix} \\ 满足条件\sigma_1 \geq \sigma_2 \geq \sigma_3 \geq \sigma_4
Σ=
σ10000σ20000σ30000σ4
满足条件σ1≥σ2≥σ3≥σ4
是一个对角阵,其几何含义是将右边所乘向量沿着各个分量进行伸缩变换,当向量模一定时,向量方向指向最小sigma4的那个维度时,模的伸缩比例最小,根据SVD的性质,最右下角的sigma最小。即 n = [0 0 0 1]'。进而推出q值位V的最后一列。
V
T
∗
q
=
n
=
单位向量
q
=
V
T
(
4
,
:
)
=
V
(
:
,
4
)
V^T*q=n = 单位向量\\ q=V^T(4,:)=V(:,4)
VT∗q=n=单位向量q=VT(4,:)=V(:,4)
从数学维度也可以看出来:
∥
Σ
∗
n
∥
=
∥
[
σ
1
∗
n
1
,
σ
2
∗
n
2
,
σ
3
∗
n
3
,
σ
4
∗
n
4
]
′
∥
)
≥
∥
[
σ
4
∗
n
1
,
σ
4
∗
n
2
,
σ
4
∗
n
3
,
σ
4
∗
n
4
]
′
∥
\parallel \Sigma*n \parallel = \parallel [ \sigma_1*n1, \sigma_2*n2, \sigma_3*n3, \sigma_4*n4]' \parallel)\geq \parallel [ \sigma_4*n1, \sigma_4*n2, \sigma_4*n3, \sigma_4*n4]' \parallel
∥Σ∗n∥=∥[σ1∗n1,σ2∗n2,σ3∗n3,σ4∗n4]′∥)≥∥[σ4∗n1,σ4∗n2,σ4∗n3,σ4∗n4]′∥
当且仅当n1=n2=n3=0, n4=1时满足等号条件。求出n,进而求出q。
3.2 求最小二乘法
最小二乘数学问题利用SVD分解求解描述如下
m
i
n
f
=
m
i
n
∥
A
x
−
y
∥
2
=
m
i
n
∥
U
m
∗
m
∗
Σ
m
∗
n
∗
V
n
∗
n
T
∗
x
n
−
y
m
∥
2
=
∥
[
Σ
n
∗
n
0
(
m
−
n
)
∗
n
]
∗
V
n
∗
n
T
∗
x
n
−
[
U
m
∗
n
U
m
∗
(
m
−
n
)
]
T
∗
y
m
∥
2
=
∥
[
Σ
n
∗
n
∗
V
n
∗
n
T
∗
x
n
−
U
m
∗
n
T
∗
y
m
−
U
m
∗
(
m
−
n
)
T
∗
y
m
]
∥
2
=
∥
[
−
U
m
∗
(
m
−
n
)
T
∗
y
m
]
∥
2
\begin{aligned} min f &= min \parallel Ax-y \parallel ^2 \\ &=min\parallel U_{m*m}*\Sigma_{m*n}*V_{n*n}^T*x_n-y_m \parallel ^2 \\ &= \parallel \begin{bmatrix}\Sigma_{n*n}\\ 0_{(m-n)*n}\end{bmatrix}*V_{n*n}^T*x_n -\begin{bmatrix} U_{m*n} \quad U_{m*(m-n)} \end{bmatrix} ^T*y_m \parallel ^2 \\ &= \parallel \begin{bmatrix} \Sigma_{n*n}*V_{n*n}^T*x_n -U_{m*n} ^T*y_m \\ -U_{m*(m-n)}^T * y_m \end{bmatrix} \parallel ^2 \\ &=\parallel \begin{bmatrix} -U_{m*(m-n)}^T * y_m \end{bmatrix} \parallel ^2 \end{aligned}
minf=min∥Ax−y∥2=min∥Um∗m∗Σm∗n∗Vn∗nT∗xn−ym∥2=∥[Σn∗n0(m−n)∗n]∗Vn∗nT∗xn−[Um∗nUm∗(m−n)]T∗ym∥2=∥[Σn∗n∗Vn∗nT∗xn−Um∗nT∗ym−Um∗(m−n)T∗ym]∥2=∥[−Um∗(m−n)T∗ym]∥2
取得最小值时
Σ
n
∗
n
∗
V
n
∗
n
T
∗
x
n
−
U
m
∗
n
T
∗
y
m
=
0
x
=
V
n
∗
n
∗
Σ
n
∗
n
−
1
∗
U
m
∗
n
T
∗
y
m
\Sigma_{n*n}*V_{n*n}^T*x_n-U_{m*n} ^T*y_m=0\\ x= V_{n*n}*\Sigma_{n*n}^{-1}*U_{m*n} ^T*y_m
Σn∗n∗Vn∗nT∗xn−Um∗nT∗ym=0x=Vn∗n∗Σn∗n−1∗Um∗nT∗ym
3.3 图像压缩算法
仅保留大于门限的奇异值和奇异值对应的向量。
当图片大小为2560*1440时,仅U中第一个列向量,S的第一个元素,V^H的第一个行向量,就能恢复几乎原图中的所有信息!压缩率非常大