正態分布變換（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
協方差矩陣描述了隨機點的概率密度的分布情況，顏色越深的地方表示概率密度值越大
import matplotlib.pyplot as plt import numpy as np import scipy.stats def plot_cov_ellipse_colormap(cov=[[1,1],[1,1]]): side = np.linspace(-3, 3, 200) X,Y = np.meshgrid(side,side) pos = np.empty(X.shape + (2,)) pos[:, :, 0] = X; pos[:, :, 1] = Y plt.axes(xticks=[], yticks=[], frameon=True) rv = scipy.stats.multivariate_normal((0,0), cov) plt.gca().grid(b=False) plt.gca().imshow(rv.pdf(pos), cmap=plt.cm.Greys, origin='lower') plt.show() plot_cov_ellipse_colormap(cov=[[1, 0.6], [0.6, 2]])
-
向量求導
$x$為列向量，$A$是一個矩陣，則有：$$\frac{dAx}{dx}=A, \quad \frac{d(x^TAx)}{dx}=x^T(A^T+A)$$
-
多元函數的泰勒展開式
多元目標函數可能是很復雜的函數，為了便於研究函數極值問題，必須用簡單函數作局部逼近，通常采用泰勒展開式作為函數在某點附近的表達式。
由高等數學知識可知，對於一元函數$f(x)$在$k$點，即$x=x^{(k)}$的泰勒展開式為：$$f(x)=f(x^{(k)})+{f}'(x^{(k)})(x-x^{(k)})+\frac{1}{2!}{f}''(x^{(k)})(x-x^{(k)})^2+...+\frac{1}{n!}f^{(n)}(x^{(k)})(x-x^{(k)})^n+R_n$$
式中的余項$R_n$為$$R_n=\frac{1}{(n+1)!}f^{(n+1)}(\xi)(x-x^{(k)})^{(n+1)}$$
$\xi$在$x$、$x^{(k)}$之間。在$x^{(k)}$點充分小的鄰域內，余項$R_n$的極限為零，因此可以用多項式來逼近函數$f(x)$。
二元函數的泰勒展開式可由一元函數泰勒展開推廣得到。
函數$f(X)=f(x_1,x_2)$在點$X^{(k)}=(x_{1}^{(k)},x_{2}^{(k)})$附近的泰勒展開，若只取到二次項可寫為\begin{align*}
f(X)\approx &f(X^{(k)})+\frac{\partial f}{\partial x_1}\bigg|_{X=X^{(k)}} (x_1-x_{1}^{(k)})+\frac{\partial f}{\partial x_2}\bigg|_{X=X^{(k)}} (x_2-x_{2}^{(k)})\\ &+\frac{1}{2!} \left[\frac{\partial^2 f}{\partial x_1^2}\bigg|_{X=X^{(k)}}(x_1-x_{1}^{(k)})^2+ 2 \frac{\partial^2 f}{\partial x_1 \partial x_2 }\bigg|_{X=X^{(k)}}(x_1-x_{1}^{(k)})(x_2-x_{2}^{(k)})+\frac{\partial^2 f}{\partial x_2^2}\bigg|_{X=X^{(k)}}(x_2-x_{2}^{(k)})^2 \right]
\end{align*}
上式可寫成矩陣形式，即$$f(X)\approx f(X^{(k)})+\left(\frac{\partial f}{\partial x_1},\frac{\partial f}{\partial x_2} \right )\begin{pmatrix}
x_1-x_1^{(k)}\\ x_2-x_2^{(k)})\end{pmatrix}+\frac{1}{2}(x_1-x_1^{(k)},x_2-x_2^{(k)})\begin{pmatrix}
\frac{\partial^2f }{\partial x_1^2} & \frac{\partial^2f }{\partial x_1 \partial x_2}\\ \frac{\partial^2f }{\partial x_1 \partial x_2} & \frac{\partial^2f }{\partial x_2^2}\end{pmatrix}\begin{pmatrix}
x_1-x_1^{(k)}\\ x_2-x_2^{(k)})\end{pmatrix}$$
式中$\left(\frac{\partial f}{\partial x_1},\frac{\partial f}{\partial x_2} \right )={\nabla f}$，$\begin{pmatrix}x_1-x_1^{(k)}\\ x_2-x_2^{(k)})\end{pmatrix}=X-X^{(k)}$，$\nabla^2 f=\begin{pmatrix}\frac{\partial^2f }{\partial x_1^2} & \frac{\partial^2f }{\partial x_1 \partial x_2}\\ \frac{\partial^2f }{\partial x_1 \partial x_2} & \frac{\partial^2f }{\partial x_2^2}\end{pmatrix}$
$\nabla^2f$是函數$f(X)$在$X^{(k)}$點的二階偏導數矩陣，稱為海塞（Hessian）矩陣，可以用$H(X)$表示，Hessian矩陣是對稱矩陣。引用上述符號后，二元函數泰勒展開式可簡寫為$$f(X)\approx f(X^{(k)}) +\nabla f(X^{(k)})(X-X^{(k)})+\frac{1}{2}(X-X^{(k)})^T H(X^{(k)})(X-X^{(k)})$$
-
無約束優化中的牛頓法
牛頓法是梯度法的進一步發展，梯度法在確定搜索方向時只考慮目標函數在選擇迭代點的局部性質，即利用一階偏導數的信息，而牛頓法進一步利用了目標函數的二階偏導數，考慮了梯度的變化趨勢，因而可更全面地確定合適的搜索方向，以便更快地搜索到極小點。
以一維問題來說明牛頓法的過程。設已知一維目標函數$f(x)$的初始點$A(x^{(k)},f(x^{{k}}))$，過$A$點做一與原目標函數$f(x)$相切的二次曲線$\varphi(x)$，求拋物線的極小點的坐標$x^{(k+1)}$，將$x^{(k+1)}$代入原目標函數$f(x)$，求得$f(x^{(k+1)})$得到$B$點。過$B$點再做一條與$f(x)$相切的二次曲線，得到下一個最小點$x^{(k+2)}$，解得$C$點。重復做下去直到找到原目標函數的極小點的坐標$x^*$為止，如下圖所示。
在逼近的過程中所用的二次函數是以原目標函數在迭代點附近的泰勒展開式的二次項。一維目標函數$f(x)$在$x^{(k)}$點逼近所用的二次函數為：$$\varphi(x)=f(x^{(k)}) +{f}'(x^{(k)})(x-x^{(k)})+\frac{1}{2}{f}''(x^{(k)})(x-x^{(k)})^2$$
該二次函數的極小點可由極值存在的必要條件${\varphi}'(x^{(k)})=0$求得，也即求得$x^{(k+1)}$
把上述過程推廣到$n$維問題，即是$$\varphi(X)=f(X^{(k)}) +\nabla f(X^{(k)})(X-X^{(k)})+\frac{1}{2}(X-X^{(k)})^T H(X^{(k)})(X-X^{(k)})$$
當$\nabla \varphi(X)=0$時，可求得二次函數$\varphi(X)$的極值點，對上式矩陣方程求導，可得到$$\nabla \varphi(X)=\nabla f(X^{(k)})+ H(X^{(k)})(X-X^{(k)})=0$$
若$H(X^{(k)})$為可逆矩陣，從而得到這次逼近的二次函數的最小點$X^{(k+1)}$ $$X = X^{(k)}- H(X^{(k)})^{-1}\nabla f(X^{(k)})$$
在每次逼近的極小點可由上式計算出。由於$\varphi(X)$是$f(X)$的一個近似表達式，所求得的極小點$X^*$並不是目標函數真正的極小點。只有當目標函數$f(X)$本身是二次函數時，所求的極小點$X^*$才是目標函數的真正極小點，這種情況一次迭代就可以求出目標函數的極值點。
在一般情況下，$f(X)$不一定是二次函數，為了求得$f(X)$的極小值，可以把由上式求得的$X$作為下一個迭代點，通過反復循環迭代，不斷逼近到$f(X)$的極小點。於是得到牛頓法的迭代格式為：$$X^{(k+1)} = X^{(k)}- H(X^{(k)})^{-1}\nabla f(X^{(k)})$$
或者寫成$$X^{(k+1)} = X^{(k)}+\alpha_k S^{(k)}$$
式中，$S^{(k)}=- H(X^{(k)})^{-1}\nabla f(X^{(k)})$稱為牛頓搜索方向；$\alpha_k=1$.
牛頓法主要缺點是每次迭代過程都要計算函數二階導數矩陣（Hessian矩陣），並且要對該矩陣求逆。這樣工作量相當大，特別是矩陣求逆計算，當設計變量維數較高時工作量更大。因此，牛頓法很少被直接采用，但是對於那些容易計算一階導數和海塞矩陣及其逆的二次函數采用牛頓法還是很方便的。
-
數值計算中的病態問題與條件數
求解過程涉及到計算協方差矩陣$\Sigma$的逆，理論上沒什么問題，但是實際中可能出現$\Sigma$接近奇異的情況。對於一個數值問題本身如果輸入數據有微小擾動（即誤差），引起輸出數據（即問題解）相對誤差很大，這就是病態問題。在計算機上解線性方程組或計算矩陣的逆時要分析問題是否病態，例如解線性方程組，如果輸入數據有微小誤差引起解的巨大誤差，就認為是病態方程組。
條件數定義：$cond(A)=\left \| A \right \|_p \left \| A^{-1} \right \|_p$，$\left \| \cdot \right \|_p $表示矩陣$A$的某種范數，若矩陣$A$的條件數較大，則稱$A$為病態矩陣。
矩陣$A=(a_{ij})_{m \times n}$有3種常見的范數：
1-范數： $ \left \| A \right \|_1= \mathop {\min }\limits_{1\leq j \leq n}\sum_{i=1}^{m}\left |a_{ij} \right |$ （列和范數，A每一列元素絕對值之和的最大值）
∞-范數：$\left \| A \right \|_\infty= \mathop {\min }\limits_{1\leq i \leq m}\sum_{j=1}^{n}\left |a_{ij} \right |$ （行和范數，A每一行元素絕對值之和的最大值）
2-范數：$\left \| A \right \|_2= \sqrt{\lambda_{max}}$ ，$\lambda_{max}$是矩陣$A^TA$的最大特征值，也稱為譜范數
根據上面的定義，矩陣的譜條件數為$$cond(A)_2=\left \| A \right \|_2 \left \| A^{-1} \right \|_2=\sqrt{\frac{\lambda_{max}(A^TA)}{\lambda_{min}(A^TA)}}$$
當$A$為對稱矩陣時$$cond(A)_2=\frac{|\lambda_1|}{|\lambda_n|}$$
其中$\lambda_1$，$\lambda_n$為$A$的絕對值最大和最小的特征值。
若條件數$cond(A)$較小（接近1），就稱$A$關於求逆矩陣或解線性方程組為良態的；若條件數$cond(A)$較大，就稱$A$關於求逆矩陣或解線性方程組為病態的。當矩陣$A$十分病態時，就說明$A$已十分接近一個奇異矩陣。要判別一個矩陣是否病態需要計算條件數$cond(A)=\left \| A \right \| \left \| A^{-1} \right \|$，而計算$A^{-1}$是比較費勁的，那么在實際中如何發現病態情況呢？通常來說，如果系數矩陣的行列式值相對很小，或系數矩陣某些行近似線性相關，這時$A$可能病態。或者矩陣$A$元素間數量級相差很大，並且無一定規則，$A$可能病態。
例如矩陣$A=\begin{pmatrix}1 & 2\\ 2.001 & 4\end{pmatrix}$，可以看出矩陣的第一行和第二行非常接近線性相關（矩陣接近奇異）。用MATALB中的inv函數求逆，結果為[[-2000, 1000], [1000.5, -500]]，如果將$A$中的元素2.001改為2.002，此時求逆的結果為[[-1000, 500], [500.5, -250]]，可以看出微小的擾動引起結果巨大的變化。
通過SVD分解來計算矩陣$A$的奇異值
[u, s, v] = svd(A)
結果為：
s =
5.0004 0
0 0.0004
則矩陣$A$的譜條件數為$cond(A)_2=\frac{5.0004}{0.0004}=12502\gg 1$，可以看出矩陣$A$非常病態。
基於數值計算的考慮，計算協方差矩陣的逆$\Sigma^{-1}$之前，可以先用奇異值分解來檢查矩陣是否接近奇異。如果最大奇異值比最小的大1000倍以上（條件數大於1000），則將條件數限制在1000，用此時對應的協方差矩陣求逆。
% Prevent covariance matrix from going singular (and not be invertible)
[U, S, V] = svd(xyCov);
if S(2,2) < 0.001 * S(1,1)
S(2,2) = 0.001 * S(1,1);
xyCov = U * S * V';
end
NDT算法
NDT算法的基本思想是先根據參考數據（reference scan）來構建多維變量的正態分布，如果變換參數能使得兩幅激光數據匹配的很好，那么變換點在參考系中的概率密度將會很大。因此，可以考慮用優化的方法求出使得概率密度之和最大的變換參數，此時兩幅激光點雲數據將匹配的最好。
算法的基本步驟如下：
1. 將參考點雲（reference scan）所占的空間划分成指定大小（CellSize）的網格或體素（Voxel）；並計算每個網格的多維正態分布參數：
-
- 均值 $\textbf q=\frac{1}{n}\sum_i \textbf{x}_i$
- 協方差矩陣 $\Sigma=\frac{1}{n}\sum_i(\textbf{x}_i-\textbf q)(\textbf{x}_i-\textbf q)^T$
2. 初始化變換參數$\textbf p=(t_x,t_y,\phi)^T$（賦予零值或者使用里程計數據賦值）
3. 對於要配准的點雲（second scan），通過變換$T$將其轉換到參考點雲的網格中$\textbf x_i'=T(\textbf x_i, \textbf p)$ $$T:\begin{pmatrix}x'\\ y'\end{pmatrix}=\begin{pmatrix}cos\phi & -sin\phi\\ sin\phi & cos\phi\end{pmatrix}\begin{pmatrix}x\\ y\end{pmatrix}+\begin{pmatrix}t_x\\t_y\end{pmatrix}$$
4. 根據正態分布參數計算每個轉換點的概率密度$$p(\textbf x)\sim exp(-\frac{(\textbf x-\textbf q)^T\Sigma^{-1}(\textbf x- \textbf q)}{2})$$
5. NDT配准得分（score）通過對每個網格計算出的概率密度相加得到$$score(\textbf p)= \sum_i exp(-\frac{(\textbf {x}_i'-\textbf q_i)^T\Sigma_i^{-1}(\textbf {x}_i'-\textbf q_i)}{2})$$
6. 根據牛頓優化算法對目標函數$-score$進行優化，即尋找變換參數$\textbf p$使得$score$的值最大。優化的關鍵步驟是要計算目標函數的梯度和Hessian矩陣：
目標函數$-score$由每個格子的值$s$累加得到，令$\textbf q=\textbf x_i'-\textbf q_i$，則 $$s=-exp(\frac{-\textbf q^T \Sigma^{-1} \textbf q}{2})$$
根據鏈式求導法則以及向量、矩陣求導的公式，$s$梯度方向為$$\frac{\partial s}{\partial p_i}= \frac{\partial s}{\partial q}\frac{\partial q}{\partial p_i} = \textbf q^T \Sigma^{-1} \frac{\partial q}{\partial p_i}exp(\frac{-\textbf q^T \Sigma^{-1} \textbf q}{2})$$
其中$\textbf q$對變換參數$p_i$的偏導數$\frac{\partial q}{\partial p_i}$即為變換$T$的雅克比矩陣：$$\frac{\partial q}{\partial p_i}=\textbf J_T=\begin{pmatrix}
1 & 0 & -xsin\phi-ycos\phi\\
0 & 1 & xcos\phi-ysin\phi
\end{pmatrix}$$
根據上面梯度的計算結果，繼續求$s$關於變量$p_i$、$p_j$的二階偏導：
$$\begin{align*} H_{ij}&=\frac{\partial^2 s}{\partial p_i \partial p_j}= \partial(\frac{\partial s}{\partial p_i})/\partial p_j=\partial(\textbf q^T \Sigma^{-1} \frac{\partial \textbf q}{\partial p_i}exp(\frac{-\textbf q^T \Sigma^{-1} \textbf q}{2}))/\partial p_j\\
&=\partial(\textbf q^T \Sigma^{-1} \frac{\partial \textbf q}{\partial p_i})/\partial p_j)exp(\frac{-\textbf q^T \Sigma^{-1} \textbf q}{2})+ (\textbf q^T \Sigma^{-1} \frac{\partial \textbf q}{\partial p_i})(\partial exp(\frac{-\textbf q^T \Sigma^{-1} \textbf q}{2})/ \partial p_j)\\
&=-exp(\frac{-\textbf q^T \Sigma^{-1} \textbf q}{2})\left[(\textbf q^T\Sigma^{-1}\frac{\partial \textbf q}{\partial p_i})(\textbf q^T\Sigma^{-1}\frac{\partial \textbf q}{\partial p_j})-(\frac{\partial \textbf q^T}{\partial p_j}\Sigma^{-1}\frac{\partial \textbf q}{\partial p_i})-(\textbf q^T \Sigma^{-1}\frac{\partial^2 \textbf q}{\partial p_i\partial p_j})\right]\end{align*}$$
根據變換方程，向量$\textbf q$對變換參數$p$的二階導數的向量為：
$$\frac{\partial^2 \textbf q}{\partial p_i\partial p_j}=\left\{\begin{matrix}
\begin{pmatrix}
-xcos\phi+ysin\phi\\
-xsin\phi-ycos\phi
\end{pmatrix} & i=j=3\\
\begin{pmatrix}
0 \\ 0\end{pmatrix}& otherwise
\end{matrix}\right.$$
7. 跳轉到第3步繼續執行，直到達到收斂條件為止
NDT算法的MATLAB實現
preNDT函數將激光數據點划分到指定大小的網格中：
function [xgridcoords, ygridcoords] = preNDT(laserScan, cellSize)
%preNDT Calculate cell coordinates based on laser scan
% [XGRIDCOORDS, YGRIDCOORDS] = preNDT(LASERSCAN, CELLSIZE) calculated the x
% (XGRIDCOORDS) and y (YGRIDCOORDS) coordinates of the cell center points that are
% used to discretize the given laser scan.
%
% Inputs:
% LASERSCAN - N-by-2 array of 2D Cartesian points
% CELLSIZE - Defines the side length of each cell used to build NDT.
% Each cell is square
%
% Outputs:
% XGRIDCOORDS - 4-by-K, the discretized x coordinates using cells with size
% equal to CELLSIZE.
% YGRIDCOORDS: 4-by-K, the discretized y coordinates using cells with size
% equal to CELLSIZE.
xmin = min(laserScan(:,1));
ymin = min(laserScan(:,2));
xmax = max(laserScan(:,1));
ymax = max(laserScan(:,2));
halfCellSize = cellSize/2;
lowerBoundX = floor(xmin/cellSize)*cellSize-cellSize;
upperBoundX = ceil(xmax/cellSize)*cellSize+cellSize;
lowerBoundY = floor(ymin/cellSize)*cellSize-cellSize;
upperBoundY = ceil(ymax/cellSize)*cellSize+cellSize;
% To minimize the effects of discretization,use four overlapping
% grids. That is, one grid with side length cellSize of a single cell is placed
% first, then a second one, shifted by cellSize/2 horizontally, a third
% one, shifted by cellSize/2 vertically and a fourth one, shifted by
% cellSize/2 horizontally and vertically.
xgridcoords = [ lowerBoundX:cellSize:upperBoundX;... % Grid of cells in position 1
lowerBoundX+halfCellSize:cellSize:upperBoundX+halfCellSize;... % Grid of cells in position 2 (X Right, Y Same)
lowerBoundX:cellSize:upperBoundX; ... % Grid of cells in position 3 (X Same, Y Up)
lowerBoundX+halfCellSize:cellSize:upperBoundX+halfCellSize]; % Grid of cells in position 4 (X Right, Y Up)
ygridcoords = [ lowerBoundY:cellSize:upperBoundY;... % Grid of cells in position 1
lowerBoundY:cellSize:upperBoundY;... % Grid of cells in position 2 (X Right, Y Same)
lowerBoundY+halfCellSize:cellSize:upperBoundY+halfCellSize;... % Grid of cells in position 3 (X Same, Y Up)
lowerBoundY+halfCellSize:cellSize:upperBoundY+halfCellSize]; % Grid of cells in position 4 (X Right, Y Up)
end
為了減小離散化的影響，用4個尺寸都為cellSize的相互重疊的小格子組成一個大格子（下面示意圖中藍色邊框的格子）來計算目標函數值等信息。將左下角的第1個小格子向右平移cellSize/2個單位得到第2個小格子；將第1個小格子向上平移cellSize/2個單位得到第3個小格子；將第1個小格子向右、向上平移cellSize/2個單位得到第4個小格子：
buildNDT函數根據划分好的網格，來計算每一個小格子中的二維正態分布參數（均值、協方差矩陣以及協方差矩陣的逆）：
function [xgridcoords, ygridcoords, meanq, covar, covarInv] = buildNDT(laserScan, cellSize)
%buildNDT Build Normal Distributions Transform from laser scan
% [XGRIDCOORDS, YGRIDCOORDS, MEANQ, COVAR, COVARINV] = buildNDT(LASERSCAN, CELLSIZE)
% discretizes the laser scan points into cells and approximates each cell
% with a Normal distribution.
%
% Inputs:
% LASERSCAN - N-by-2 array of 2D Cartesian points
% CELLSIZE - Defines the side length of each cell used to build NDT.
% Each cell is a square area used to discretize the space.
%
% Outputs:
% XGRIDCOORDS - 4-by-K, the discretized x coordinates of the grid of cells,
% with each cell having a side length equal to CELLSIZE.
% Note that K increases when CELLSIZE decreases.
% The second row shifts the first row by CELLSIZE/2 to
% the right. The third row shifts the first row by CELLSIZE/2 to the
% top. The fourth row shifts the first row by CELLSIZE/2 to the right and
% top. The same row semantics apply to YGRIDCOORDS, MEANQ, COVAR, and COVARINV.
% YGRIDCOORDS: 4-by-K, the discretized y coordinates of the grid of cells,
% with each cell having a side length equal to CELLSIZE.
% MEANQ: 4-by-K-by-K-by-2, the mean of the points in cells described by
% XGRIDCOORDS and YGRIDCOORDS.
% COVAR: 4-by-K-by-K-by-2-by-2, the covariance of the points in cells
% COVARINV: 4-by-K-by-K-by-2-by-2, the inverse of the covariance of
% the points in cells.
%
% [XGRIDCOORDS, YGRIDCOORDS, MEANQ, COVAR, COVARINV] describe the NDT statistics.
% Copyright 2016 The MathWorks, Inc.
% When the scan contains ONLY NaN values (no valid range readings),
% the input laserScan is empty. Explicitly
% initialize empty variables to support code generation.
if isempty(laserScan)
xgridcoords = zeros(4,0);
ygridcoords = zeros(4,0);
meanq = zeros(4,0,0,2);
covar = zeros(4,0,0,2,2);
covarInv = zeros(4,0,0,2,2);
return;
end
% Discretize the laser scan into cells
[xgridcoords, ygridcoords] = preNDT(laserScan, cellSize);
xNumCells = size(xgridcoords,2);
yNumCells = size(ygridcoords,2);
% Preallocate outputs
meanq = zeros(4,xNumCells,yNumCells,2);
covar = zeros(4,xNumCells,yNumCells,2,2);
covarInv = zeros(4,xNumCells,yNumCells,2,2);
% For each cell, compute the normal distribution that can approximately
% describe the distribution of the points within the cell.
for cellShiftMode = 1:4
% Find the points in the cell
% First find all points in the xgridcoords and ygridcoords separately and then combine the result.
% indx的值表示laserScan的x坐標分別在xgridcoords划分的哪個范圍中(例如1就表示落在第1個區間;若不在范圍中,則返回0)
[~, indx] = histc(laserScan(:,1), xgridcoords(cellShiftMode, :));
[~, indy] = histc(laserScan(:,2), ygridcoords(cellShiftMode, :));
for i = 1:xNumCells
xflags = (indx == i); % xflags is a logical vector
for j = 1:yNumCells
yflags = (indy == j);
xyflags = logical(xflags .* yflags);
xymemberInCell = laserScan(xyflags,:); % laser points in the cell
% If there are more than 3 points in the cell, compute the
% statistics. Otherwise, all statistics remain zero.
% See reference [1], section III.
if size(xymemberInCell, 1) > 3
% Compute mean and covariance
xymean = mean(xymemberInCell);
xyCov = cov(xymemberInCell, 1);
% Prevent covariance matrix from going singular (and not be
% invertible). See reference [1], section III.
[U,S,V] = svd(xyCov);
if S(2,2) < 0.001 * S(1,1)
S(2,2) = 0.001 * S(1,1);
xyCov = U*S*V';
end
[~, posDef] = chol(xyCov);
if posDef ~= 0
% If the covariance matrix is not positive definite,
% disregard the contributions of this cell.
continue;
end
% Store statistics
meanq(cellShiftMode,i,j,:) = xymean;
covar(cellShiftMode,i,j,:,:) = xyCov;
covarInv(cellShiftMode,i,j,:,:) = inv(xyCov);
end
end
end
end
end
objectiveNDT函數根據變換參數計算目標函數值及其梯度和Hessian矩陣，objectiveNDT的輸出參數將作為目標函數信息傳入優化函數中：
function [score, gradient, hessian] = objectiveNDT(laserScan, laserTrans, xgridcoords, ygridcoords, meanq, covar, covarInv)
%objectiveNDT Calculate objective function for NDT-based scan matching
% [SCORE, GRADIENT, HESSIAN] = objectiveNDT(LASERSCAN, LASERTRANS, XGRIDCOORDS,
% YGRIDCOORDS, MEANQ, COVAR, COVARINV) calculates the NDT objective function by
% matching the LASERSCAN transformed by LASERTRANS to the NDT described
% by XGRIDCOORDS, YGRIDCOORDS, MEANQ, COVAR, and COVARINV.
% The NDT score is returned in SCORE, along with the optionally
% calculated score GRADIENT, and score HESSIAN.
% Copyright 2016 The MathWorks, Inc.
% Create rotation matrix
theta = laserTrans(3);
sintheta = sin(theta);
costheta = cos(theta);
rotm = [costheta -sintheta;
sintheta costheta];
% Create 2D homogeneous transform
trvec = [laserTrans(1); laserTrans(2)];
tform = [rotm, trvec
0 0 1];
% Create homogeneous points for laser scan
hom = [laserScan, ones(size(laserScan,1),1)];
% Apply homogeneous transform
trPts = hom * tform'; % Eqn (2)
% Convert back to Cartesian points
laserTransformed = trPts(:,1:2);
hessian = zeros(3,3);
gradient = zeros(3,1);
score = 0;
% Compute the score, gradient and Hessian according to the NDT paper
for i = 1:size(laserTransformed,1) % 對每一個轉換點進行處理
xprime = laserTransformed(i,1);
yprime = laserTransformed(i,2);
x = laserScan(i,1);
y = laserScan(i,2);
% Eqn (11)
jacobianT = [1 0 -x*sintheta - y*costheta;
0 1 x*costheta - y*sintheta];
% Eqn (13)
qp3p3 = [-x*costheta + y*sintheta;
-x*sintheta - y*costheta];
for cellShiftMode = 1:4
[~,m] = histc(xprime, xgridcoords(cellShiftMode, :)); % 轉換點落在(m,n)格子中
[~,n] = histc(yprime, ygridcoords(cellShiftMode, :));
if m == 0 || n == 0
continue
end
meanmn = reshape(meanq(cellShiftMode,m,n,:),2,1);
covarmn = reshape(covar(cellShiftMode,m,n,:,:),2,2);
covarmninv = reshape(covarInv(cellShiftMode,m,n,:,:),2,2);
if ~any([any(meanmn), any(covarmn)])
% Ignore cells that contained less than 3 points
continue
end
% Eqn (3)
q = [xprime;yprime] - meanmn;
% As per the paper, this term should represent the probability of
% the match of the point with the specific cell
gaussianValue = exp(-q'*covarmninv*q/2);
score = score - gaussianValue;
for j = 1:3
% Eqn (10)
gradient(j) = gradient(j) + q'*covarmninv*jacobianT(:,j)*gaussianValue;
% Eqn (12)
qpj = jacobianT(:,j);
for k = j:3 % Hessian矩陣為對稱矩陣,只需要計算對角線上的部分
qpk = jacobianT(:,k);
if j == 3 && k == 3
hessian(j,k) = hessian(j,k) + gaussianValue*(-(q'*covarmninv*qpj)*(q'*covarmninv*qpk) +(q'*covarmninv*qp3p3) + (qpk'*covarmninv*qpj));
else
hessian(j,k) = hessian(j,k) + gaussianValue*(-(q'*covarmninv*qpj)*(q'*covarmninv*qpk) + (qpk'*covarmninv*qpj));
end
end
end
end
end
% 補全Hessian矩陣
for j = 1:3
for k = 1:j-1
hessian(j,k) = hessian(k,j);
end
end
score = double(score);
gradient = double(gradient);
hessian = double(hessian);
end
最后，可以利用MATLAB中的優化函數來計算最優變換參數。具體使用方法可以參考：fminunc(Find minimum of unconstrained multivariable function) 以及 Including Gradients and Hessians.
以上過程就是MATLAB Robotics System Toolbox工具箱中的函數matchScans的主要內容。matchScans函數用於匹配兩幀激光雷達數據，輸出兩幀之間的姿態變換。
參考：
多元正態分布(Multivariate normal distribution)
The Normal Distributions Transform: A New Approach to Laser Scan Matching