大家好，又见面了，我是你们的朋友全栈君。如果您正在找激活码,请点击查看最新教程,关注关注公众号 “全栈程序员社区” 获取激活教程,可能之前旧版本教程已经失效.最新Idea2022.1教程亲测有效,一键激活。

Jetbrains全家桶1年46，售后保障稳定

问题背景

  近来从事毫米波雷达的定位与建图工作，想拓展下工作思路，研究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点云在上一帧坐标系的坐标

表示位移，表示旋转角度。Scan alignment目的就是通过前后两帧的laser scan复原位移和角度。假设给定两帧激光点云的测量，具体步骤如下：

(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时刻的里程计估计，将k时刻测量到的2D点云根据 投影到k-n时刻。通过计算k-n时刻与k时刻的NDT score，通过优化得到的位姿作为k时刻位姿。如果k时刻的测量距离k-n时刻的关键帧较远，则把最近一次NDT匹配成功的测量作为新的关键帧。

下面时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

Jetbrains全家桶1年46，售后保障稳定

　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，里面有 个3D点。对于该网格的NDT分布的均值和方差：

那么该网格的概率密度函数为：

  如下图所示，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定位截图