本文主要介绍两种基于直接匹配的前端里程计方案，ICP以及NDT，重点介绍公式推导以及代码实现。

ICP

ICP即迭代最近点$（Iterative\ Closest\ Point）$的实现以及公式推导在视觉SLAM中已经提及，并进行了相关的公式推导和代码实现，详细查看：求解ICP，这里不做具体展示。

经典NDT

由于ICP迭代法一般般需要提供一个较好的初值，也就是需要粗配准，最终可能导致迭代结果可能会陷入局部最优，导致配准失败，往往达不到我们想要的效果。目前，实际工程之中，大多数使用另一种比较好的配准算法，NDT配准。

所谓NDT就是正态分布变换，作用与ICP一样用来估计两个点云之间的刚体变换。NDT算法主要的思路就是将目标点云刻画成多个概率分布，然后通过位姿变换关系将待配准点云转换到目标点云坐标系下，计算转换后待配准点云的总概率，并将此概率的负值作为目标函数，通过高斯牛顿迭代法优化该目标函数以求获得负的最小概率值(即最大概率值)．

由于其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。这个配准算法耗时稳定，跟初值相关不大，初值误差大时，也能很好的纠正过来。因此，NDT算法因其具有较强的鲁棒性而被广泛的应用。

对点云进行相应的预处理之后，我们将空间划分为栅格（2D图像中的正方形或3D中的立方体），并统计落在栅格中的点，如图所示

同样的，我们设上一帧点集和当前点集分别为：

$$ X=\{x_1,x_2,...,x_{N_x}\} \\ Y=\{y_1,y_2,...,y_{N_y}\} $$

求$X$的均值：

$$ \mu=\frac{1}{N_x}\sum^{N_x}_{i=1}x_i $$

求$X$的协方差：

$$ \Sigma=\frac{1}{N_x-1}\sum^{N_x}_{i=1}(x_i-\mu)(x_i-\mu)^T $$

根据预测的位姿$R,t$，对点进行旋转和平移，得到上一帧的点云投影到当前帧下的点云$y_i^{\prime}$：

$$ y_i^{\prime}=T(p,y_i)=Ry_i+t $$

其中对于2D模型的点：$p=[t_x\ t_y\ \phi_z]^T$，对于3D模型的点：$p=[t_x\ t_y\ t_z\ \phi_x\ \phi_y\ \phi_z]^T$

将当前点集$X$的均值$\mu$和协方差$\sum$，与投影点集$y_i^\prime$的每一个点做联系，构建如下的概率分布：

$$ f(X,y_i^\prime)=\frac{1}{\sqrt{2\pi}\sqrt{|\Sigma|} }\exp(-\frac{(y_i^\prime-\mu)^T\Sigma^{-1}(y_i^\prime-\mu)}{2})\tag{1} $$

然后求所有点的联合概率分布：

$$ \begin{align*} \Psi&=\prod_{i=1}^{N_y}f(X,y_i^\prime)\\ &=\prod_{i=1}^{N_y}\frac{1}{\sqrt{2\pi}\sqrt{|\Sigma|} }\exp(-\frac{(y_i^\prime-\mu)^T\Sigma^{-1}(y_i^\prime-\mu)}{2}) \end{align*}\tag{2} $$

取对数，简化问题：

$$ \ln\Psi=\sum^{N_y}_{i=1}(-\frac{(y_i^\prime-\mu)^T\Sigma^{-1}(y_i^\prime-\mu)}{2}+\ln(\frac{1}{\sqrt{2\pi}\sqrt{|\Sigma|} }))\tag{3} $$

去除常数项：

$$ \max\Psi=\max\ln\Psi=\min\Psi_1=\min\sum^{N_y}_{i=1}(y_i^\prime-\mu)^T{\Sigma}^{-1}(y_i^\prime-\mu)\tag{4} $$

所以我们的目标函数就变成了：$\min\sum^{N_y}_{i=1}(y_i^\prime-\mu)^T{\Sigma}^{-1}(y_i^\prime-\mu)$，而接下来的任务就是根据这个目标函数求得对应的参数$R,t$。

令：

$$ \begin{align*} e_i(p)&=y_i^\prime-\mu\\ F_i(p)&=\sum^{N_y}_{i=1}(y_i^\prime-\mu)^T{\Sigma}^{-1}(y_i^\prime-\mu)\\ &=e_i(p)^T\Sigma^{-1}e_i(p) \end{align*}\tag{5} $$

因此目标函数为：

$$ \begin{align*} &\min\sum^{N_y}_{i=1}(y_i^\prime-\mu)^T{\Sigma}^{-1}(y_i^\prime-\mu)\\ &=\min\sum^{N_y}_{i=1}F_i(p) \end{align*}\tag{6} $$

迭代优化，即找到$\Delta p$使得下式中的值达到最小：

$$ \min\sum^{N_y}_{i=1}F_i(p+\Delta p)=\sum^{N_y}_{i=1}e_i(p+\Delta p)^T\Sigma^{-1}e_i(p+\Delta p)\tag{7} $$

其中对$e_i(p+\Delta p)$进行一阶泰勒展开：

$$ \begin{align*} e_i(p+\Delta p)&\approx e_i(p)+\frac{de_i}{dp}\Delta p\\ &=e_i(p)+J_i\Delta p \end{align*}\tag{8} $$

故而：

$$ \begin{align*} F_i(p+\Delta p)&=e_i(p+\Delta p)^T\Sigma^{-1}e_i(p+\Delta p)\\ &\approx (e_i(p)+J_i\Delta p)^T\Sigma^{-1}(e_i(p)+J_i\Delta p)\\ &=e_i(p)^T\Sigma^{-1}e_i(p)+2e_i(p)^T\Sigma^{-1}J_i\Delta p+\Delta p^TJ_i^T\Sigma^{-1}J_i\Delta p\\ &=F_i(p)+2b_i^T\Delta p+\Delta p^TH_i\Delta p \end{align*}\tag{9} $$

其中：

$$ b_i^T=e_i(p)^T\Sigma^{-1}J_i，\ H_i=J_i^T\Sigma^{-1}J_i\tag{10} $$

因此，目标函数随自变量的变化为：

$$ \begin{align*} \Delta F_i(p) &=F_i(p+\Delta p)-F_i(p)\\ &=2b_i^T\Delta p+\Delta p^TH_i\Delta p \end{align*}\tag{11} $$

上述优化问题转换为：找到$\Delta p$使得$\Delta F_i(p)$取得极小值，令其导数为零：

$$ \begin{align*} \frac{d\Delta F_i(p)}{d\Delta p}&=2b_i+2H_i\Delta p=0\\ 即：H_i\Delta p&=-b_i \end{align*}\tag{12} $$

根据$(10)$中的定义，只需要求$J_i$，即可求得$\Delta p $

$$ J_i=\frac{de_i}{dp} $$

2D场景

已知对于2D场景$p=[t_x\ t_y\ \phi_z]^T$，

$$ \begin{align*} y_i^{\prime}&=T(p,y_i)\\ &=Ry_i+t\\ &=\begin{bmatrix} \cos\phi_z&-\sin\phi_z\\ \sin\phi_z&\cos\phi_z \end{bmatrix}y_i+\begin{bmatrix}t_x\\t_y\end{bmatrix}\\ e_i&=y_i^{\prime}-\mu \end{align*} $$

雅可比矩阵：

$$ J_i=\begin{bmatrix} 1&0&-y_{i1}\sin\phi_z-y_{i2}\cos\phi_z\\ 0&1&y_{i1}\cos\phi_z-y_{i2}\sin\phi_z\end{bmatrix} $$

3D场景

已知对于3D场景$p=[t_x\ t_y\ t_z\ \phi_x\ \phi_y\ \phi_z]^T$，

$$ \begin{align*} y_i^{\prime}&=T(p,y_i)\\ &=Ry_i+t\\ &=R_xR_yR_zy_i+t\\ &= \begin{bmatrix} c_yc_z&-c_ys_z&s_y\\ c_xs_z+s_xs_yc_z&c_xc_z-s_xs_ys_z&-s_xc_y\\ s_xs_z-c_xs_yc_z&c_xs_ys_z+s_xc_z&c_xc_y \end{bmatrix}y_i+\begin{bmatrix}t_x\\t_y\\t_z\end{bmatrix}\\ e_i&=y_i^{\prime}-\mu \end{align*} $$

雅可比矩阵：

$$ J_i=\begin{bmatrix} 1&0&0&0&c&f\\ 0&1&0&a&d&g\\ 0&0&1&b&e&h \end{bmatrix} $$

其中：

$$ \begin{align*} a&=y_{i1}(-s_xs_z+c_xs_yc_z)+y_{i2}(-s_xc_z-c_xs_ys_z)+y_{i3}(-c_xc_y)\\ b&=y_{i1}(c_xs_z+s_xs_yc_z)+y_{i2}(c_xc_z-s_xs_ys_z)+y_{i3}(-s_xc_y)\\ c&=y_{i1}(-s_yc_z)+y_{i2}(s_ys_z)+y_{i3}(c_y)\\ d&=y_{i1}(s_xc_yc_z)+y_{i2}(-s_xc_ys_z)+y_{i3}(s_xs_y)\\ e&=y_{i1}(-c_xc_yc_z)+y_{i2}(c_xc_ys_z)+y_{i3}(-c_xs_y)\\ f&=y_{i1}(-c_ys_z)+y_{i2}(-c_yc_z)\\ g&=y_{i1}(c_xc_z-s_xs_ys_z)+y_{i2}(-c_xs_z-s_xs_yc_z)\\ h&=y_{i1}(s_xc_z+c_xs_ys_z)+y_{i2}(c_xs_yc_z-s_xs_z) \end{align*} $$

其他NDT