NDT（Normal Distributions Transform）算法原理与公式推导

　　正态分布变换（NDT）算法是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。下面的公式推导和MATLAB程序编写都参考论文：The Normal Distributions Transform: A New Approach to Laser Scan Matching

　　先回顾一下算法推导和实现过程中涉及到的几个知识点：

• 协方差矩阵

　　在概率论和统计中，协方差是对两个随机变量联合分布线性相关程度的一种度量。对多维随机变量$\textbf X=[X_1, X_2, X_3, ..., X_n]^T$ ，我们往往需要计算各维度两两之间的协方差，这样各协方差组成了一个$n\times n$的矩阵，称为协方差矩阵。协方差矩阵是个对称矩阵，而且是半正定的（证明可以参考这里）。对角线上的元素是各维度上随机变量的方差。我们定义协方差矩阵为$\Sigma$，矩阵内的元素$\Sigma_{ij}$为$$\Sigma_{ij}=\operatorname{cov}(X_i,X_j)=\operatorname{E}\big[(X_i-\operatorname{E}[X_i])(X_j-\operatorname{E}[X_j])\big]$$

　　这样这个矩阵为$$\begin{align*} 
\Sigma&=\operatorname{E}\big[(\textbf X-\operatorname{E}[\textbf X]\big)(\textbf X-\operatorname{E}[\textbf X])^T]\\
&=\begin{bmatrix} 
\operatorname{cov}(X_1, X_1) &
\operatorname{cov}(X_1, X_2) &
\cdots &
\operatorname{cov}(X_1, X_n) \\
\operatorname{cov}(X_2, X_1) &
\operatorname{cov}(X_2, X_2) &
\cdots &
\operatorname{cov}(X_2, X_n) \\
\vdots &
\vdots &
\ddots &
\vdots \\
\operatorname{cov}(X_n, X_1) &
\operatorname{cov}(X_n, X_2) &
\cdots &
\operatorname{cov}(X_n, X_n) 
\end{bmatrix}
\end{align*}$$

  　　样本的协方差是样本集的一个统计量，可作为联合分布总体参数的一个估计，在实际中计算的通常是样本的协方差。样本的协方差矩阵与上面的协方差矩阵相同，只是矩阵内各元素以样本的协方差替换。样本集合为$\{\textbf x_{\cdot j}=[x_{1j},x_{2j},...,x_{nj}]^T|1\leqslant j\leqslant m\}$，$m$为样本数量，所有样本可以表示成一个$n \times m$的矩阵。我们以$\hat \Sigma$表示样本的协方差矩阵，与$\Sigma$区分。

$$\begin{align*}
\hat \Sigma&=\begin{bmatrix}
q_{11} & q_{12} & \cdots & q_{1n} \\
q_{21} & q_{21} & \cdots & q_{2n} \\
\vdots & \vdots & \ddots & \vdots \\
q_{n1} & q_{n2} & \cdots & q_{nn}
\end{bmatrix}\\
&=\frac {1}{m-1}
\begin{bmatrix}
{\sum_{j=1}^m{(x_{1j}-\bar x_1)(x_{1j}-\bar x_1)}} &
{\sum_{j=1}^m{(x_{1j}-\bar x_1)(x_{2j}-\bar x_2)}} &
\cdots &
{\sum_{j=1}^m{(x_{1j}-\bar x_1)(x_{nj}-\bar x_n)}} \\
{\sum_{j=1}^m{(x_{2j}-\bar x_2)(x_{1j}-\bar x_1)}} &
{\sum_{j=1}^m{(x_{2j}-\bar x_2)(x_{2j}-\bar x_2)}} &
\cdots &
{\sum_{j=1}^m{(x_{2j}-\bar x_2)(x_{nj}-\bar x_n)}} \\
\vdots &
\vdots &
\ddots &
\vdots \\
{\sum_{j=1}^m{(x_{nj}-\bar x_n)(x_{1j}-\bar x_1)}} &
{\sum_{j=1}^m{(x_{nj}-\bar x_n)(x_{2j}-\bar x_2)}} &
\cdots &
{\sum_{j=1}^m{(x_{nj}-\bar x_n)(x_{nj}-\bar x_n)}}
\end{bmatrix}\\
&=\frac {1}{m-1} \sum_{j=1}^m (\textbf x_{\cdot j} - \bar {\textbf x}) (\textbf x_{\cdot j} - \bar {\textbf x})^T
\end{align*}$$

　　公式中$m$为样本数量，$\bar{\textbf x}$为样本均值，是一个列向量。$\textbf x_{\cdot j}$为第$j$个样本，也是一个列向量。     

 

• 多元正态分布

　　若随机向量的概率密度函数为$$\begin{align*}
f_{\textbf x}(x_1,...,x_k)&=\frac{exp(-\frac{1}{2}(\mathbf x- \mu)^T \Sigma^{-1}(\textbf x-\mu))}{\sqrt{(2\pi)^k|\Sigma|}}\\
&=\frac{exp(-\frac{1}{2}(\mathbf x- \mu)^T \Sigma^{-1}(\textbf x-\mu))}{\sqrt{|2\pi\Sigma|}}
\end{align*}$$

　　则称$\textbf x$服从$k$元正态分布，记作$\textbf x \sim N_k(\mu, \Sigma)$，其中$\textbf x$是一个$k$维列向量，参数$\mu$和$\Sigma$分别为$\textbf x$的均值和协方差矩阵，$|\Sigma|=det\Sigma$是协方差矩阵的行列式（Determinant）。

　　二维正态分布概率密度函数为钟形曲面，等高线是椭圆线族，并且二维正态分布的两个边缘分布都是一维正态分布的形式：$$f(x)=\frac{1}{\sqrt{2\pi}\sigma}e^{-\frac{(x-\mu)^2}{2\sigma^2}} \quad ,-\infty<x<\infty$$

　　下面是服从$\mu=\begin{bmatrix}0\\ 0\end{bmatrix}$，$\Sigma=\begin{bmatrix}1 & 3/5\\ 3/5 & 2\end{bmatrix}$二维正态分布的概率密度及两个边缘分布的概率密度图：

　　用Mathematica可以画出二维正态分布概率密度函数的等高线：

ContourPlot[PDF[MultinormalDistribution[{
    0, 0}, {
   {
   1, 3/5}, {
   3/5, 2}}], {x, y}], {x, -3, 3}, {y, -3, 3}]

 　　可以看出等高线是一族椭圆线：

　　也可以使用Python的FilterPy（Kalman filters and other optimal and non-optimal estimation filters in Python）库来计算多元正态分布概率密度，首先用pip安装filterpy

pip install filterpy

 　　下面的代码计算x=[1.2, 2.5]处的概率密度

from filterpy.stats import multivariate_gaussian

x = [1.2, 2.5]
mu = [0, 0]
cov = [[1,0.6],[0.6,2]]

pdf = multivariate_gaussian(x, mu, cov)  # output is 0.02302

View Code

 　　协方差矩阵描述了随机点的概率密度的分布情况，颜色越深的地方表示概率密度值越大