第二篇 语义元素定位方法

一背景概述

语义定位比起点云定位越来越流行，并且具有诸多有点，如运算量少，特征稳定，类人思维，便于融合等。

二 定位方法

2.1 ICP方法

数学建模如下。

2.1.1 字符定义

同一组点在坐标系a和坐标系b 下分别表达成如下点集。
$$\left\{\begin{array}{c}{P}_i^a\end{array}\right\},\left\{\begin{array}{c}{P}_i^b\end{array}\right\},$$
在a系中的重心和b系中的重心分别如下。
$$\left\{\begin{array}{c}{P}^a\end{array}\right\},\left\{\begin{array}{c}{P}^b\end{array}\right\},$$
扣除在a系中的重心和b系中的重心分别如下。
$$\begin{gathered} \left\{\begin{array}{c}{Q}_i^a\end{array}\right\}=\left\{\begin{array}{c}{P}_i^a-P^a\end{array}\right\} \\ \left\{\begin{array}{c}{Q}_i^b\end{array}\right\}=\left\{\begin{array}{c}{P}_i^b-P^b\end{array}\right\} \end{gathered}$$

2.1.2 目标函数

$$\begin{array}{rl}MinF& =\Sigma \parallel P_i^a-(R\ast P_i^b+t){\parallel }^2\\ & =\Sigma \parallel (P_i^a-P^a)-(R\ast (P_i^b-P^b)+t)+P^a-R\ast P^b{\parallel }^2\\ & =\Sigma \parallel Q_i^a-R\ast Q_i^b+P^a-R\ast P^b-t{\parallel }^2\\ & =\Sigma \parallel Q_i^a-R\ast Q_i^b{\parallel }^2+\Sigma \parallel P^a-R\ast P^b-t{\parallel }^2+2\ast \Sigma (Q_i^a-R\ast Q_i^b{)}^T\ast (P^a-R\ast P^b-t)\end{array}$$
第三项类累加为0.于是目标函数简化为下式。
$$\begin{array}{rl}MinF& =\Sigma \parallel Q_i^a-R\ast Q_i^b{\parallel }^2+\Sigma \parallel P^a-R\ast P^b-t{\parallel }^2\end{array}$$
上式第二项对于任意的R均可以求出t使得第二项为0.
因此最终的目标函数如下。
$$\begin{array}{rl}MinF& =\Sigma \parallel Q_i^a-R\ast Q_i^b{\parallel }^2\end{array}$$

2.1.3 求解函数

$$\begin{array}{rl}MinF& =\Sigma (Q_i^a-R\ast Q_i^b{)}^T\ast (Q_i^a-R\ast Q_i^b)\\ & =\Sigma {Q_i^a}^T\ast Q_i^a+\Sigma {Q_i^b}^T\ast Q_i^b-\Sigma {Q_i^a}^T\ast R\ast Q_i^b-\Sigma {Q_i^b}^T\ast R^T\ast Q_i^a\end{array}$$
前两项是常数，后两项相等。因此可以化简得到下式。
$$\begin{array}{rl}MinF& =-2\ast \Sigma ({Q_i^a}^T\ast R\ast Q_i^b)\\ & =-2\ast \Sigma tr(R\ast Q_i^b\ast {Q_i^a}^T)\\ & =-2\ast tr(R\ast \Sigma (Q_i^b\ast {Q_i^a}^T))\end{array}$$
令
$$W=\Sigma (Q_i^b\ast {Q_i^a}^T)=U\ast \sigma \ast V^T$$
于是利用矩阵迹的交换一致性质，目标函数
$$\begin{array}{rl}MinF& =-tr(R\ast U\ast \sigma \ast V^T)\\ & =-tr(V^T\ast R\ast U\ast \sigma )\end{array}$$
容易知道
$$V^T\ast R\ast U$$
是一个旋转矩阵，当它为单位矩阵时，取得最大迹值，即
$$\begin{gathered} V^T\ast R\ast U=E \\ R=V\ast U^T=(U\ast V^T{)}^T \end{gathered}$$

2.2 改造ICP方法

在语义定位中，每个语义的权重不一样，并且存在多种形式的语义，包括点语义、线语义和形状语义。
点语义的表达2.1节已经描述。

2.2.1 线语义可以表达为下式。

$$\begin{array}{rl}MinF& =\Sigma (w_i^2\ast \parallel L_i^a-R\ast L_i^b{\parallel }^2)+\Sigma \parallel (P_i^a-R\ast P_i^b-t)\times L_i^a{\parallel }^2\\ & =\Sigma (\parallel w_i\ast L_i^a-R\ast w_i\ast L_i^b{\parallel }^2)+\Sigma \parallel (P_i^a\times L_i^a-R\ast P_i^b\times L_i^a-t\times L_i^a){\parallel }^2\\ & =\Sigma (\parallel w_i\ast L_i^a-R\ast w_i\ast L_i^b{\parallel }^2)+\Sigma \parallel (P{L}_i^a-R\ast P{L}_i^b-tL){\parallel }^2\\ & =\Sigma (\parallel w_i\ast L_i^a-R\ast w_i\ast L_i^b{\parallel }^2)+\Sigma \parallel (Q{L}_i^a-R\ast Q{L}_i^b){\parallel }^2+\Sigma \parallel (P{L}^a-R\ast P{L}^b-tL){\parallel }^2\end{array}$$
QL_i代表PL_i(P叉乘L结果)去重心，PL表示{PL_i}点集的重心。对于任意R，都能有tL使第三项为0.可以参考2.1节以及目标函数求解过程得出。
$$\begin{array}{r}W=\Sigma (w_i\ast L_i^b)\ast (w_i\ast L_i^a{)}^T+\Sigma (Q{L}_i^b)\ast (Q{L}_i^a{)}^T=U\ast \sigma \ast V^T\end{array}$$
对W 进行SVD分解可以求出R。
$$\begin{gathered} V^T\ast R\ast U=E \\ R=V\ast U^T=(U\ast V^T{)}^T \end{gathered}$$

2.2.2 形状语义的表达式可以表达为下式。

$$\begin{array}{rl}MinF& =\Sigma (w_i^2\ast \parallel L_i^a-R\ast L_i^b{\parallel }^2)+\Sigma \parallel P_i^a-R\ast P_i^b-t{\parallel }^2\end{array}$$
令Q_i是形状语义的位置，L_i是形状语义决定的方向。一个形状语义又位置和方向唯一确定。Q_i代表P_i去重心，P表示{P_i}点集的重心。
$$\begin{array}{r}W=\Sigma (w_i\ast L_i^b)\ast (w_i\ast L_i^a{)}^T+\Sigma (Q_i^b)\ast (Q_i^a{)}^T=U\ast \sigma \ast V^T\end{array}$$
对W 进行SVD分解可以求出R。
$$\begin{gathered} V^T\ast R\ast U=E \\ R=V\ast U^T=(U\ast V^T{)}^T \end{gathered}$$

2.2.3 多语义融合求解

如果同时存在三种语义，只需要三种语义W矩阵相加再求SVD分解就可以。
$$W=W_{point}+W_{line}+W_{shape}$$

三 扩展：SVD分解应用

3.1 求最佳旋转矩阵

3.1.1 求旋转矩阵R

第二节已经描述

3.1.2 求四元数q

在SLAM中经常遇到求姿态问题描述如下。
$$A\ast q=0,\parallel q\parallel =1$$
上述问题相当于是带条件最优化的问题，可以使用拉格朗日乘子法。 但是上述求姿态问题也可以使用SVD分解。
$$\begin{gathered} A=U\ast \Sigma \ast V^T \\ A\ast q=U\ast \Sigma \ast V^T\ast q=0 \end{gathered}$$
上式求得q应该使左边的式子得到最小的非零值，U和V是旋转矩阵并不会影响最终向量的模。V*q仍然是一个模值的单位向量。
$$min(\parallel A\ast q\parallel )=min(\parallel \Sigma \ast V^T\ast q\parallel )=min(\parallel \Sigma \ast n\parallel ）$$

$$\begin{gathered} \Sigma =\left[\begin{array}{cccc}{\sigma }_1& 0& 0& 0\\ 0& {\sigma }_2& 0& 0\\ 0& 0& {\sigma }_3& 0\\ 0& 0& 0& {\sigma }_4\end{array}\right] \\ 满足条件{\sigma }_1\ge {\sigma }_2\ge {\sigma }_3\ge {\sigma }_4 \end{gathered}$$
是一个对角阵，其几何含义是将右边所乘向量沿着各个分量进行伸缩变换，当向量模一定时，向量方向指向最小sigma4的那个维度时，模的伸缩比例最小，根据SVD的性质，最右下角的sigma最小。即 n = [0 0 0 1]'。进而推出q值位V的最后一列。
$$\begin{gathered} V^T\ast q=n=单位向量 \\ q=V^T(4,:)=V(:,4) \end{gathered}$$
从数学维度也可以看出来：
$$\parallel \Sigma \ast n\parallel =\parallel [{\sigma }_1\ast n1,{\sigma }_2\ast n2,{\sigma }_3\ast n3,{\sigma }_4\ast n4{]}^{\prime }\parallel )\ge \parallel [{\sigma }_4\ast n1,{\sigma }_4\ast n2,{\sigma }_4\ast n3,{\sigma }_4\ast n4{]}^{\prime }\parallel$$
当且仅当n1=n2=n3=0, n4=1时满足等号条件。求出n,进而求出q。

3.2 求最小二乘法

最小二乘数学问题利用SVD分解求解描述如下
$$\begin{array}{rl}minf& =min\parallel Ax-y{\parallel }^2\\ & =min\parallel U_{m\ast m}\ast {\Sigma }_{m\ast n}\ast V_{n\ast n}^T\ast x_n-y_m{\parallel }^2\\ & =\parallel \left[\begin{array}{c}{\Sigma }_{n\ast n}\\ 0_{(m-n)\ast n}\end{array}\right]\ast V_{n\ast n}^T\ast x_n-{\left[\begin{array}{c}{U}_{m\ast n}{U}_{m\ast (m-n)}\end{array}\right]}^T\ast y_m{\parallel }^2\\ & =\parallel \left[\begin{array}{c}{\Sigma }_{n\ast n}\ast V_{n\ast n}^T\ast x_n-U_{m\ast n}^T\ast y_m\\ -U_{m\ast (m-n)}^T\ast y_m\end{array}\right]{\parallel }^2\\ & =\parallel \left[\begin{array}{c}-U_{m\ast (m-n)}^T\ast y_m\end{array}\right]{\parallel }^2\end{array}$$
取得最小值时
$$\begin{gathered} {\Sigma }_{n\ast n}\ast V_{n\ast n}^T\ast x_n-U_{m\ast n}^T\ast y_m=0 \\ x=V_{n\ast n}\ast {\Sigma }_{n\ast n}^{-1}\ast U_{m\ast n}^T\ast y_m \end{gathered}$$

3.3 图像压缩算法

仅保留大于门限的奇异值和奇异值对应的向量。
当图片大小为2560*1440时，仅U中第一个列向量，S的第一个元素，V^H的第一个行向量，就能恢复几乎原图中的所有信息！压缩率非常大