6. 根据牛顿优化算法对目标函数−score$-score$进行优化，即寻找变换参数$\mathbf{\text{p}}$使得$score$的值最大。优化的关键步骤是要计算目标函数的梯度和Hessian矩阵：

　　目标函数−score由每个格子的值$s$累加得到，令$\mathbf{\text{q}}={\mathbf{\text{x}}}_i^{\prime }-{\mathbf{\text{q}}}_i$，则

　根据链式求导法则以及向量、矩阵求导的公式，$s$梯度方向为

　其中q对变换参数$p_i$的偏导数即为变换$T$的雅克比矩阵：

根据上面梯度的计算结果，继续求s关于变量$p_i$、$p_j$的二阶偏导：

根据变换方程，向量$\mathbf{\text{q}}$对变换参数$p$的二阶导数的向量为：

　　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函数用于匹配两帧激光雷达数据，输出两帧之间的姿态变换。

参考：

Matrix calculus

Covariance matrix

协方差与协方差矩阵

矩阵、向量求导法则

Multivariate Gaussians

NDT方法在SLAM中的应用

多元正态分布(Multivariate normal distribution)

NDT 算法（与ICP对比）和一些常见配准算法

The Normal Distributions Transform: A New Approach to Laser Scan Matching