激光雷达SLAM激光的前端配准算法

ICP匹配方法

两组三维点集的最小二乘拟合

给定两个三维点集：

{ x i } = { x 1 , x 2 , … , x N } { p i } = { p 1 , p 2 , … , p N } \{x_i\} = \{x_1, x_2, \dots, x_{N}\} \\ \{p_i\} = \{p_1, p_2, \dots, p_{N}\} {xi​}={x1​,x2​,…,xN​}{pi​}={p1​,p2​,…,pN​}

点集 { x i } \{x_i\} {xi​}和点集 { p i } \{p_i\} {pi​}的集合转换关系如下所示：

$$p_i=R{x}_i+T+N_i$$

其中，$R$是一个$3\times 3$的旋转矩阵，$T$是一个三维的平移向量，$N_i$是噪声项。

我们可以通过最小化下式，求得旋转矩阵$R$和平移向量$T$。

${\sum }^2=\sum _{i=1}^N{||p_i-(R{x}_i+T)||}^2$

为了实现最小二乘过程的$R$、$T$变量解耦，做去中心化处理，假设$\hat{R}$和$\hat{T}$是我们的最小二乘解,，定义以下变量。

$$\begin{gathered} x_i^{\prime }=x_i-\frac{1}{N}\sum _{i=1}^N{x}_i \\ {p}_i^{\prime }=p_i-\frac{1}{N}\sum _{i=1}^N{p}_1 \end{gathered}$$

此时我们有
${\sum }^2=\sum _{i=1}^N{||p_i^{\prime }-R{x}_i^{\prime }||}^2$

首先我们可以求得最小二乘解$\hat{R}$,然后由下式求得$\hat{T}$

$$\hat{T}=p^{\prime }-\hat{R}p$$

$$\begin{gathered} \sum _{i=1}^N{||p_i^{\prime }-R{x}_i^{\prime }||}^2=\sum _{i=1}^N(p_i^{\prime }-R{x}_i^{\prime }{)}^t(p_i^{\prime }-R{x}_i^{\prime }) \\ =\sum _{i=1}^N({p_i^{\prime }}^t{p}_i^{\prime }+{x_i^{\prime }}^t{x}_i^{\prime }-2{p_i^{\prime }}^{t}R{x}_i) \end{gathered}$$

所以最小化

$$\begin{gathered} F=\sum _{i=1}^N{p_i^{\prime }}^{t}R{x}_i \\ =Trace(\sum _{i=1}^{N}R{x}_i^{\prime }{p_i^{\prime }}^t)=Trace(RH) \end{gathered}$$

由定理：
对于任意的对称正定矩阵$A{A}^T$和任意的正交矩阵$B$，都有

$$Trace(A{A}^T)\ge Trace(BA{A}^T)$$

对$H$进行$SVD$分解得到$H=UM{V}^T$，令$X=V{U}^T$，则有，
$$XH=VM{V}^T$$

所以，

$$Trace(XH)\ge Trace(BXH)$$
因此，如果$X$矩阵的行列式为1，则$\hat{T}=X$