NDT算法 matlab实现过程

参考论文：The normal distributions transform: a new approach to laser scan matching

目录： 

1.原理解释

2.matlab 代码

NDT算法原理

MATLAB 编程

程序1：主函数

close all
% load('C:\Program Files\MATLAB\R2017a\toolbox\robotics\robotexamples\robotalgs\data\scanMatchingData.mat')
referenceScan = laserMsg{150};
currentScan = laserMsg{162};


currScanRanges = currentScan.Ranges;
currScanAngles = readScanAngles(currentScan);
refScanRanges = referenceScan.Ranges;
refScanAngles = readScanAngles(referenceScan);
% 把读取到的雷达数据转化成笛卡尔坐标系
currScanCart = readCartesian(currentScan);
refScanCart = readCartesian(referenceScan);
plot(currScanCart(:,1),currScanCart(:,2),'r.');
hold on;
plot(refScanCart(:,1),refScanCart(:,2),'b.');
% c=sum222(1,2)
% [R,t]=NDT_ScanMatch(currScanCart,refScanCart);


currCart=currScanCart;
refCart=refScanCart;
grid_num=5;
% 这里需要把边界放宽一点
x_up=    max([currCart(:,1);refCart(:,1)]);
x_low=    min([currCart(:,1);refCart(:,1)]);
y_up=    max([currCart(:,2);refCart(:,2)]);
y_low=    min([currCart(:,2);refCart(:,2)]);
gridSize=[(x_up-x_low)/(grid_num-1),(y_up-y_low)/(grid_num-1)];
grid_dist_ref=cell(grid_num,grid_num);
grid_dist_cur=cell(grid_num,grid_num);
for j=1:size(refCart,1)
    Pos=(refCart(j,:)-[x_low,y_low]);
    Pos=[Pos(1)/gridSize(1),Pos(2)/gridSize(2)];
    row=Pos(1)+1;
    col=Pos(2)+1;
    grid_dist_ref{ceil(row),ceil(col)}=[grid_dist_ref{ceil(row),ceil(col)};refCart(j,:)];
    grid_dist_ref{ceil(row),floor(col)}=[grid_dist_ref{ceil(row),floor(col)};refCart(j,:)];
    grid_dist_ref{floor(row),ceil(col)}=[grid_dist_ref{flo