問題背景
近來從事毫米波雷達的定位與建圖工作，想拓展下工作思路，研究autoware公司的激光點雲定位與建圖。期間正好發現autoware的激光點雲配准算法是NDT(Normal-Distributions Transform)，相比ICP算法，它能更快速高效地確定兩個大型點雲的剛性變換。這裏分別介紹下2003年經典的2D NDT算法，以及autoware日本團隊在2006年提出的3D NDT算法。
“The Normal Distributions Transform: A New Approach to Laser Scan Matching”
“A 3-D Scan Matching using Improved 3-D Normal Distributions Transform for Mobile Robotic Mapping”
二維ND
2D NDT數學描述
假設小車安裝了只有一線的2D激光雷達，只能表達一個平面內是否有障礙物。把小車周圍的區域劃分成大小相同的單元格，我們可以用NDT(Normal Distribution Transform)對掃描的2D點雲分佈進行建模，用高斯分佈表達每個單元格內2D點的聚集情況。若每個單元格包含3個以上的2D點，則計算該單元格內所有點Xi=1,…,n的座標均值及方差。
不同於於柵格圖occupancy grid，NDT分佈用連續可微的概率密度描述細分區域網格平面內的2D點。柵格圖表達的是單元格內包含障礙物的概率，NDT分佈本身表示參考幀中特定網格內2D點的位置分佈情況。假設下圖爲參考幀，左側時柵格圖表達空與不空，中間是識別到的2D激光雷達點雲，右圖表達每個單元格內二維正態概率分佈，每個單元格大小相同，越紅的地方概率值越大。
我們可以NDT分佈計算當前幀新觀測到的2D點座標相對於參考幀單元格的概率（不同單元格有不同的均值和方差，均值區域表示參考幀單元格內2D點最密集的區域）。下式表示當前幀2D點x，屬於參考幀中某個單元格的概率
如何劃分網格並避免離散化的影響
個人認爲按照上圖那樣劃分網格，破壞了2D點雲結構，比如屬於同一個物體的點雲，被刻意分開到不同單元格計算了。這種離散化不利於地圖地描述，也不利於後期點雲配準。我們分別看看建圖和定位時，如何劃分網格，計算概率密度。
建圖時，給定參考幀laser scan的2D點雲，根據點雲座標上下界以及cell size確定外側bounding box下圖藍色邊框所示。對於邊框內部採用4個單元格相互重疊的，兩兩覆蓋50%，分別計算每個子網格的點雲正態分佈。因此這種劃分包含了4種均值和方差。下右圖只是示意，實際上藍色框圖內將有多個單元格，可以認爲藍色框圖內大部分的2D點會同時落入四種單元格內。
定位時，計算前後兩幀pose關係，給定當前幀laser scan的2D點雲，根據T(待優化的參數)轉換後的座標查找落入哪個單元格內，同樣當前幀每個2d點將對應四種分佈。累計所有2d點對應的四種概率密度之和，通過選擇合理的T，使得概率密度之和最大。
如何計算兩片點雲相對位姿
假設用T表達2D空間內，兩個時刻機器人的位姿變換關係。下面公式表達了當前幀的2D點雲在上一幀座標系的座標
(1)用第一幀laser scan，按照上述過程計算初始NDT分佈
(2)初始化位姿變換參數T，全零或者用odometry初始化
(3)根據位姿變換參數T，將第二幀的laser scan點雲轉換到第一幀座標系下
(4)爲第二幀的每個2D點分配網格，計算點雲對應的四種概率分佈
(5)累計點雲的概率分佈，計算總體得分
p是第二幀點雲，x’是第二幀點雲p經T投影在第一幀的座標，q和sigma是x’所屬第一幀的網格概率分佈。
(6)用牛頓法優化參數T (任意非線性優化算法均可)，使得score得分最高 [對於牛頓法，迭代步長的要點是求score函數的一階和二階偏導數，此處不擴展]
(7)回到步驟(3)，重複上述步驟，優化算法收斂
2D NDT在SLAM中的應用
以下內容是我基於2003年那篇論文的理解，給出大致思路。
建圖：
Slam地圖由圖結構表達，每個頂點表示當前關鍵幀機器人絕對位姿，頂點之間的邊表示前後兩幀關鍵幀之間2D點雲經過NDT配準推算出來的relative pose（可以看作是odometry，如果有IMU或輪速傳感器，就不用NDT推算前後航跡了）。地圖元素就是2D 點雲的正態分佈(均值看作2D點雲的位置，方差可看作2D點雲覆蓋範圍)，並且這些分佈根據每個頂點的全局座標，也轉換到全局座標下了。
每新增一個頂點，或者叫新增關鍵幀，理論上所有其他頂點都要參與全局優化。關於全局目標函數，每個頂點的全局位姿可以得到兩個頂點之間相對位姿，而兩個頂點對應的laser scan根據NDT匹配也能推算出相對位姿。由於全局位姿未知，我們希望通過估算兩種相對位姿的差(這樣能反推全局位姿)，使得整體score得分最小，所以優化的目標函數是關於位姿之差的二次函數（關於score函數在NDT配準的相對位姿處二階泰勒展開，一次項在左側，
另外隨着關鍵幀增加，全局優化的頂點越來越多，無法實時計算。因此每次新增頂點，只優化三條連接邊以內的子圖。
定位：
假設k-n時刻的測量是關鍵幀，給定k-n與k時刻的里程計估計
下面時matlab 工具箱內關於2D NDT Matching的代碼
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
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 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;
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
三維NDT
三維NDT分佈數學描述
接下來看看NDT如何處理三維激光數據，假設有參考幀3D點集
它們將被分配到稱作ND voxel的三維網格中，對於第k個voxel，裏面有
那麼該網格的概率密度函數爲：
如下圖所示，reference點集會被三維網格包圍構造多維正態分佈。爲了克服離散化問題，不同於2D點集，這裏每個3D點會被8個三維網格包圍。
3D NDT分佈更新
隨着地圖擴張，每次三維網格內新增了參考點，均值和協方差(關於x,y,z)都會重複計算且計算量會逐漸增加。爲了減少計算量，對於第k個voxel，其均值p和協方差sigma採用如下方式增量更新，計算複雜度與網格中3D點的個數無關。
下圖是3D點雲對應網格的NDT可視化
如何進行3D點雲NDT配準
給定兩個3D點集，如何通過NDT分佈計算二者的相對位姿呢？先看看三維空間的座標變換，點集X通過(R,t)變換，R是旋轉矩陣，t是平移向量
給定參考幀點雲以及當前幀點雲，配準問題就變成了(R,t)參數搜索問題。根據參考幀點雲構造NDT分佈，通過尋找合適的座標變換參數使得當前幀點雲對應的NDT概率密度之和最大。
這裏同樣使用牛頓優化法求解參數R和t
NDT三維網格的設計
如果三維網格voxel大小不當，那麼NDT配準時會出問題。如下圖所示，
圖a中黑點爲參考幀，白點爲當前幀，如果voxel網格尺寸太小，當前幀左上角點雲沒有對應的參考幀點雲，對NDT配準位置修正沒有貢獻，反而增加了牛頓法收斂時間。另外，如果網格內點雲數量過小，無法形成正態分佈。
圖b中，如果voxel網格尺寸較大，白色點和黑色點本不屬於同一物體，雖然收斂速度很快，但是最終匹配結果是錯誤的。
圖c中，如果voxel網格尺寸過大，網格中有兩種參考點的分佈，單核正態分佈(單峯)不足以描述它們，所以當前幀同這種分佈配準時，往往也是失敗的。
如何改進
總之，網格越小，計算量越大，消耗內存大，但是匹配更精確。網格越大，計算量越小，匹配越不精確。
NDT TKU作者在不同計算階段定義不同網格大小。在初始匹配時，或者叫收斂階段，對於機器人較近激光測量採用較小的網格，而距離機器人較遠的，採用較大的網格。在初始匹配完成後，進行修正階段時，對於遠處的網格將同近處測量一樣採用較小的網格。因爲機器人朝向角的細微差異，會導致遠處測量的巨大不確定性。因此，在初始搜索匹配時，對於遠處的激光點雲，採用較大的網格，對於近處的點雲採用較小的網格。並且最終匹配計算，將近採用較小的網格。
下圖爲autoware NDT定位截圖
