1. 经典NDT

在这里插入图片描述
NDT 核心思想：基于概率的匹配。目标是将点集 Y 匹配到固定的点集 X 中。这里的联合概率说的是将 X 划分成栅格(如上图)，每个栅格里面都有很多点，这些点可以计算一个均值/协方差，这是一个概率的概念。而联合概率是：Y 往 X 上旋转的时候，它落在栅格中的哪个格子里知道的。根据落在格子里的点，本身 X 格子已经有了一个均值/协方差，而 Y 的这些落在格子里的点可以形成一个联合概率。这个联合概率会作为有没匹配好的一个指标。
点集：
$\begin{aligned} & X=\left\{x_1, x_2, \cdots, x_{N_x}\right\} \\ & Y=\left\{y_1, y_2, \cdots, y_{N_y}\right\} \end{aligned}$

目标: $\max \Psi=\max \prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right)$ (与ICP这里有些区别，ICP的是点到点的距离，这里变成了联合概率)
2D模型: $\quad p=p_3=\left[\begin{array}{lll}t_x & t_y & \phi_z\end{array}\right]^{\mathrm{T}}$
3D模型: $\quad p=p_6=\left[\begin{array}{llllll}t_x & t_y & t_z & \phi_x & \phi_y & \phi_z\end{array}\right]^{\mathrm{T}}$

在这里插入图片描述

2. 计算方式

均值和协方差：
$\mu=\frac{1}{N_x} \sum_{i=1}^{N_x} x_i， \boldsymbol{\Sigma}=\frac{1}{N_x-1} \sum_{i=1}^{N_x}\left(x_i-\mu\right)\left(x_i-\mu\right)^{\mathrm{T}}$

根据预测的位姿，对点进行旋转和平移(这里的旋转和平移是一个初始预测值)：
$y_i^{\prime}=T\left(p, y_i\right)=R y_i+t$

旋转和平移后的点与目标点集中的点在同一坐标系下，此时可计算各点的联合概率：
$f\left(X, y_i^{\prime}\right)=\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right)$

所有点的联合概率(就是所有点的概率乘一起)：
$\begin{aligned} \Psi & =\prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right) \\ & =\prod_{i=1}^{N_y} \frac{1}{\sqrt{2 \pi} \sqrt{|\mathbf{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right) \end{aligned}$
目标是让所有点的联合概率最大。但是上式中的 R 和 t 是在 $y'_i$ 内的。这里还有一个exp，这个指数项会让公式变得复杂，需要消掉指数项。这时去对数可以解决。
取对数，简化问题(目标是让所有点的联合概率最大)：
$\ln \Psi=\sum_{i=1}^{N_y}\left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}+\ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right)\right)$

去除常数项 $\ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right)$ ，得到：
$\max \Psi=\max \ln \Psi=\min \Psi_1=\min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)$

因为之前是负数，我们上式只需要求最小值就行。

这时就变成了一个优化问题了：
目标函数： $\min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)$ $y_i^{\prime}=T\left(p, y_i\right)=R y_i+t$
待求参数： $R, t$
定义残差函数： $f_i(p)=y_i^{\prime}-\mu$
按照高斯牛顿法的流程，只需计算残差函数关于待求参数的雅可比，便可迭代优化。
$J_i=\frac{d f_i(p)}{d p}$

2.1 2D场景求解:

$\begin{aligned} p & =\left[\begin{array}{lll} t_x & t_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ \\ y_i^{\prime} & =T\left(p, y_i\right) \\ & =\left[\begin{array}{cc} \cos \phi_z & -\sin \phi_z \\ \sin \phi_z & \cos \phi_z \end{array}\right] y_i+\left[\begin{array}{l} t_x \\ t_y \end{array}\right] \end{aligned}$

雅可比：
$J_i=\frac{d f_i(p)}{d p}=\left[\begin{array}{ccc}1 & 0 & -y_{i 1} \sin \phi_z-y_{i 2} \cos \phi_z \\ 0 & 1 & y_{i 1} \cos \phi_z-y_{i 2} \sin \phi_z\end{array}\right]$

2.2 3D场景求解：

$\begin{aligned} & p=\left[\begin{array}{llllll} t_x & t_y & t_z & \phi_x & \phi_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ & y_i^{\prime}=T\left(p, y_i\right)=R_x R_y R_z y_i+t=\left[\begin{array}{ccc} c_y c_z & -c_y s_z & s_y \\ c_x s_z+s_x s_y c_z & c_x c_z-s_x s_y s_z & -s_x c_y \\ s_x s_z-c_x s_y c_z & c_x s_y s_z+s_x c_z & c_x c_y \end{array}\right] y_i+\left[\begin{array}{c} t_x \\ t_y \\ t_z \end{array}\right] \end{aligned}$
雅可比：
$\begin{aligned} & \text { 雅可比 } J_i=\left[\begin{array}{llllll} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \end{array}\right] \end{aligned}$

其中：
$\begin{aligned} & a=y_{i 1}\left(-s_x s_z+c_x s_y c_z\right)+y_{i 2}\left(-s_x c_z-c_x s_y s_z\right)+y_{i 3}\left(-c_x c_y\right) \\ & b=y_{i 1}\left(c_x s_z+s_x s_y c_z\right)+y_{i 2}\left(-s_x s_y s_z+c_x c_z\right)+y_{i 3}\left(-s_x c_y\right) \\ & c=y_{i 1}\left(-s_y c_z\right)+y_{i 2}\left(s_y s_z\right)+y_{i 3}\left(c_y\right) \\ & d=y_{i 1}\left(s_x c_y c_z\right)+y_{i 2}\left(-s_x c_y s_z\right)+y_{i 3}\left(s_x s_y\right) \\ & e=y_{i 1}\left(-c_x c_y c_z\right)+y_{i 2}\left(c_x c_y s_z\right)+y_{i 3}\left(-c_x s_y\right) \\ & f=y_{i 1}\left(-c_y s_z\right)+y_{i 2}\left(-c_y c_z\right) \\ & g=y_{i 1}\left(c_x c_z-s_x s_y s_z\right)+y_{i 2}\left(-c_x s_z-s_x s_y c_z\right) \\ & h=y_{i 1}\left(s_x c_z+c_x s_y s_z\right)+y_{i 2}\left(c_x s_y c_z-s_x s_z\right) \end{aligned}$