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{floor(row),ceil(col)};refCart(j,:)];
grid_dist_ref{floor(row),floor(col)}=[grid_dist_ref{floor(row),floor(col)};refCart(j,:)];
end
for j=1:size(currCart,1)
Pos=(currCart(j,:)-[x_low,y_low]);
Pos=[Pos(1)/gridSize(1),Pos(2)/gridSize(2)];
row=Pos(1)+1;
col=Pos(2)+1;
grid_dist_cur{ceil(row),ceil(col)}=[grid_dist_cur{ceil(row),ceil(col)};currCart(j,:)];
grid_dist_cur{ceil(row),floor(col)}=[grid_dist_cur{ceil(row),floor(col)};currCart(j,:)];
grid_dist_cur{floor(row),ceil(col)}=[grid_dist_cur{floor(row),ceil(col)};currCart(j,:)];
grid_dist_cur{floor(row),floor(col)}=[grid_dist_cur{floor(row),floor(col)};currCart(j,:)];
end
figure
hold on
for i=1:grid_num
for j=1:grid_num
Ref=grid_dist_ref{i,j};
Cur=grid_dist_cur{i,j};
if length( Ref)>0
plot(Ref(:,1),Ref(:,2),'*')
end
if length(Cur>0)
plot(Cur(:,1),Cur(:,2),'o')
end
end
end
figure,
cellplot(grid_dist_ref)
figure
% 求出每一个网格的分布
cell_param=cell(grid_num,grid_num);
for i=1:grid_num
for j=1:grid_num
Ref=grid_dist_ref{i,j};
if length( Ref)>0
cell_param{i,j}.mu=mean(Ref,1);
cell_param{i,j}.sigma=cov(Ref(:,1),Ref(:,2));
while 1
eigen_val=eig(cell_param{i,j}.sigma);
if(eigen_val(1)<=0.00001|| eigen_val(2)<=0.00001)
cell_param{i,j}.sigma=cell_param{i,j}.sigma+ eye(2)*0.0001;
else
break
end
end
eigen_val=eig(cell_param{i,j}.sigma);
plot(Ref(:,1),Ref(:,2),'.')
mu_=cell_param{i,j}.mu;
sig_=cell_param{i,j}.sigma;
drawEllipse(mu_,sig_);
Cur=grid_dist_cur{i,j};
if ~isempty(Cur)
P=[P;mvnpdf(Cur,mu_,sig_)];
end
% ezplot('((x-mu_(1))/eigen_val(1))^2+((y-mu_(2))/eigen_val(2))^2 = 5');
end
end
end
% 变换向量
T = [0;0];
r=0;
R=[cos(r) -sin(r);
sin(r) cos(r)]
JT=[1 0 -T(1)*sin(r)-T(2)*cos(r)
0 1 T(1)*cos(r)-T(2)*sin(r)];
d2ij=[zeros(2,8),[-cos(r)*T(1)+sin(r)*T(2) ; -sin(r)*T(1)-cos(r)*T(2)]];
H_sum=zeros(3,3);
g_sum=zeros(3,1);
for i=1:grid_num
for j=1:grid_num
per_grid = grid_dist_cur{i,j};
for k=i:size(per_grid)
mu_=cell_param{i,j}.mu';
sig_=cell_param{i,j}.sigma;
x_pie= R*per_grid(i,:)'+T;
q= x_pie-mu_;
g=[];
H=[];
for h=1:3 %
g=[g;q'* inv(sig_)*JT(:,h)*exp(-q'*inv(sig_)*q/2)];
H_temp=[];
for f=1:3
H_temp=[H_temp,exp(-q'*inv(sig_)*q/2)+(-q'* inv(sig_)*JT(:,h))*(q'* inv(sig_)*JT(:,h))+(-q'*inv(sig_)*d2ij(:,h*f))+(-JT(:,h)'*inv(sig_)*JT(:,h))];
end
H=[H;H_temp];
end
H_sum=H_sum+H;
g_sum=g_sum+g;
end
end
end
d_p=H_sum\(-g_sum)程序2：画椭圆function drawEllipse(mu,sigma)
% mu=[0,0];
% sigma=[10,-3;
% -3,10]
% 计算 长短轴
[d,v]=eig(sigma)
% 计算焦点坐标
v=v*3;
if v(1,1)程序4：椭圆function h=ellipse(ra,rb,ang,x0,y0,C,Nb)
% Ellipse adds ellipses to the current plot
%
% ELLIPSE(ra,rb,ang,x0,y0) adds an ellipse with semimajor axis of ra,
% a semimajor axis of radius rb, a semimajor axis of ang, centered at
% the point x0,y0.
%
% The length of ra, rb, and ang should be the same.
% If ra is a vector of length L and x0,y0 scalars, L ellipses
% are added at point x0,y0.
% If ra is a scalar and x0,y0 vectors of length M, M ellipse are with the same
% radii are added at the points x0,y0.
% If ra, x0, y0 are vectors of the same length L=M, M ellipses are added.
% If ra is a vector of length L and x0, y0 are vectors of length
% M~=L, L*M ellipses are added, at each point x0,y0, L ellipses of radius ra.
%
% ELLIPSE(ra,rb,ang,x0,y0,C)
% adds ellipses of color C. C may be a string ('r','b',...) or the RGB value.
% If no color is specified, it makes automatic use of the colors specified by
% the axes ColorOrder property. For several circles C may be a vector.
%
% ELLIPSE(ra,rb,ang,x0,y0,C,Nb), Nb specifies the number of points
% used to draw the ellipse. The default value is 300. Nb may be used
% for each ellipse individually.
%
% h=ELLIPSE(...) returns the handles to the ellipses.
%
% as a sample of how ellipse works, the following produces a red ellipse
% tipped up at a 45 deg axis from the x axis
% ellipse(1,2,pi/8,1,1,'r')
%
% note that if ra=rb, ELLIPSE plots a circle
%
% written by D.G. Long, Brigham Young University, based on the
% CIRCLES.m original
% written by Peter Blattner, Institute of Microtechnology, University of
% Neuchatel, Switzerland, [email protected]
% Check the number of input arguments
if nargin<1,
ra=[];
end;
if nargin<2,
rb=[];
end;
if nargin<3,
ang=[];
end;
%if nargin==1,
% error('Not enough arguments');
%end;
if nargin<5,
x0=[];
y0=[];
end;
if nargin<6,
C=[];
end
if nargin<7,
Nb=[];
end
% set up the default values
if isempty(ra),ra=1;end;
if isempty(rb),rb=1;end;
if isempty(ang),ang=0;end;
if isempty(x0),x0=0;end;
if isempty(y0),y0=0;end;
if isempty(Nb),Nb=300;end;
if isempty(C),C=get(gca,'colororder');end;
% work on the variable sizes
x0=x0(:);
y0=y0(:);
ra=ra(:);
rb=rb(:);
ang=ang(:);
Nb=Nb(:);
if isstr(C),C=C(:);end;
if length(ra)~=length(rb),
error('length(ra)~=length(rb)');
end;
if length(x0)~=length(y0),
error('length(x0)~=length(y0)');
end;
% how many inscribed elllipses are plotted
if length(ra)~=length(x0)
maxk=length(ra)*length(x0);
else
maxk=length(ra);
end;
% drawing loop
for k=1:maxk
if length(x0)==1
xpos=x0;
ypos=y0;
radm=ra(k);
radn=rb(k);
if length(ang)==1
an=ang;
else
an=ang(k);
end;
elseif length(ra)==1
xpos=x0(k);
ypos=y0(k);
radm=ra;
radn=rb;
an=ang;
elseif length(x0)==length(ra)
xpos=x0(k);
ypos=y0(k);
radm=ra(k);
radn=rb(k);
an=ang(k)
else
rada=ra(fix((k-1)/size(x0,1))+1);
radb=rb(fix((k-1)/size(x0,1))+1);
an=ang(fix((k-1)/size(x0,1))+1);
xpos=x0(rem(k-1,size(x0,1))+1);
ypos=y0(rem(k-1,size(y0,1))+1);
end;
co=cos(an);
si=sin(an);
the=linspace(0,2*pi,Nb(rem(k-1,size(Nb,1))+1,:)+1);
% x=radm*cos(the)*co-si*radn*sin(the)+xpos;
% y=radm*cos(the)*si+co*radn*sin(the)+ypos;
figure(20);
hold on
h(k)=line(radm*cos(the)*co-si*radn*sin(the)+xpos,radm*cos(the)*si+co*radn*sin(the)+ypos);
set(h(k),'color',C(rem(k-1,size(C,1))+1,:));
end;