对应点集配准算法

什么是配准

对应点集配准算法(四元数)

已知点集初始位置$A$与运动后的位置$B$，其中$a_i$是点集$A$的任意点，$b_i$是点集$B$的任意点。

1. 先求两个点集的中心$\bar{a}$和$\bar{b}$
2. 再求点集的协方差$D_{AB}$
   2.1 A的任意点的偏差${\Delta }_{ai}=a_i-\bar{a}=(\delta a_{xi},\delta a_{yi},\delta a_{zi})$
   2.2 B的任意点的偏差${\Delta }_{bi}=a_i-\bar{b}=(\delta b_{xi},\delta b_{yi},\delta b_{zi})$
   2.3 AB的协方差:
   $$D_{AB}=\left(\begin{array}{ccc}...& \Delta a_{ix}& ...\\ ...& \Delta a_{iy}& ...\\ ...& \Delta a_{iz}& ...\end{array}\right)\left(\begin{array}{ccc}...& ...& ...\\ \Delta b_{ix}& \Delta a_{iy}& \Delta a_{iz}\\ ...& ...& ...\end{array}\right)$$
3. 构建差模矩阵
   $$Q=\left(\begin{array}{cccc}tr(D)& \Delta D_{23}& \Delta D_{31}& \Delta D_{12}\\ \Delta D_{23}& |& -& |\\ \Delta D_{31}& |& M& |\\ \Delta D_{12}& |& -& |\end{array}\right)$$
   其中:
   $M=[D+D^T-tr(D)I_{33}]$
   $\Delta D=D-D^T$
4. 求$Q$的主特征向量就是从$A$到$B$的旋转四元数，将该四元数转换成旋转矩阵$R$
5. 平移向量为$T=\bar{b}-\bar{a}R$

可参考

ICP算法在点云配准的应用.
matlab练习程序（对应点集配准的四元数法）.

对应点集配准算法(奇异值分解)

已知点集初始位置$A$与运动后的位置$B$，其中$a_i$是点集$A$的任意点，$b_i$是点集$B$的任意点。

1. 先求两个点集的中心$\bar{a}$和$\bar{b}$
2. 再求点集的张量和$H_{AB}$
3. 将张量和进行奇异值分解$H=U\Sigma V^T$
4. 最佳旋转矩阵为
   $$R=\{\begin{array}{ll}V{U}^T& |V{U}^T|>0\\ -V{U}^T& |V{U}^T|<0\end{array}$$
5. 最佳平移$T=\bar{b}-\bar{a}R$

张量的迹

两个矢量的内积是一个标量$a\cdot b=a{b}^T$
两个矢量的外积是一个向量$a⨂b=a^{T}b$
所有矢量为行的形式
以三维为例
$$a\cdot b=\left(\begin{array}{ccc}{x}_a& y_a& z_a\end{array}\right)\cdot \left(\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right)=x_a{x}_b+y_a{y}_b+z_a{z}_b$$
$$a⨂b=\left(\begin{array}{c}{x}_a\\ y_a\\ z_a\end{array}\right)\cdot \left(\begin{array}{ccc}{x}_b& y_b& z_b\end{array}\right)=\left(\begin{array}{ccc}{x}_a{x}_b& x_a{y}_b& x_a{z}_b\\ y_a{x}_b& y_a{y}_b& y_a{z}_b\\ y_a{x}_b& y_a{y}_b& y_a{z}_b\end{array}\right)$$
可知$a\cdot b=tr(a⨂b)$，也就是说矢量的内积就是矢量外积的迹。
对应点群之间的距离可以如下表示
$$\begin{gathered} d=\sum _{k=1}^n(a_k-b_k{)}^2=\sum _{k=1}^n(a_k-b_k)\cdot (a_k-b_k{)}^T/n \\ =\sum _{k=1}^n(a_k{a}_k^T-b_k{a}_k^T-a_k{b}_k^T+b_k{b}_k^T)/n \\ =\sum _{k=1}^n{a}_k{a}_k^T/n+\sum _{k=1}^n{b}_k{b}_k^T/n-2\sum _{k=1}^n{a}_k{b}_k^T/n \end{gathered}$$
也就是说${\sum }_{k=1}^n{a}_k{b}_k^T$越大，两个对应点群的距离就越小。${\sum }_{k=1}^n{a}_k{b}_k^T$其实就是两个点群对应点的内积之和，根据前面的所述内积与外积的关系，他也就是外积和的迹
$$\begin{gathered} \sum _{k=1}^n{a}_k{b}_k^T=tr(\sum _{k=1}^n{a}_k^T{b}_k) \\ =tr(\sum _{k=1}^n\left(\begin{array}{ccc}{x}_{ak}{x}_{bk}& x_{ak}{y}_{bk}& x_{ak}{z}_{bk}\\ y_{ak}{x}_{bk}& y_{ak}{y}_{bk}& y_{ak}{z}_{bk}\\ y_{ak}{x}_{bk}& y_{ak}{y}_{bk}& y_{ak}{z}_{bk}\end{array}\right)) \end{gathered}$$
可以证明，当外积等于其奇异值矩阵时，他的迹最大。也就是说，如果外积$a\otimes b$奇异值为$\Sigma$
$$\begin{gathered} \Sigma =\left(\begin{array}{ccc}{\sigma }_x& 0& 0\\ 0& {\sigma }_y& 0\\ 0& 0& {\sigma }_z\end{array}\right) \\ max(tr(a\otimes b))={\sigma }_x+{\sigma }_y+{\sigma }_z \end{gathered}$$
如何让$a$和$b$的外积变成其奇异值呢?我们知道
$$H=U\Sigma V^T$$
若有个旋转矩阵为$R=V^{T}U$时，外积就是其奇异值矩阵。
我们还可以构建一个重合度的概念
$$\begin{gathered} \rho =\frac{2{\sum }_{k=1}^n{a}_k{b}_k^T}{{\sum }_{k=1}^n{a}_k{a}_k^T/n+{\sum }_{k=1}^n{b}_k{b}_k^T/n} \\ =\frac{2A^{T}B}{A^{T}A+B^{T}B} \end{gathered}$$
当点群就一个点时
$$\rho =\frac{2a^{T}b}{a^{T}a+b^{T}b}=\frac{a\cdot b}{a^2+b^2}=cos<a,b>$$
也就是说，对于一个点时，重合度就是矢量之间夹角的余弦值，可以用下图表示

直观的理解就是当a与b的夹角越小时，两个矢量越接近重合。同理当两个点群的重合度越小时，两个点群越重合。
三维点云配准 – ICP 算法原理及推导.