參考論文：The normal distributions transform: a new approach to laser scan matching

NDT演算法原理

MATLAB 程式設計

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)<v(2,2)
    l=v(2,2),s=v(1,1)
    theta=atan(d(2,2)/d(1,2));
else
    l=v(1,1)
    s=v(2,2)
    theta=atan(d(2,1)/d(1,1));
end


ellipse(l,s,theta,mu(1),mu(2))
end
% c=(abs(v(2,2)^2-v(1,1)^2))^0.5;
% c1=mu+[c*cos(theta),c*sin(theta)]
% c2=mu-[c*cos(theta),c*sin(theta)]
% 
 
[email protected](((x-c1(1))^2+(y-c1(1))^2)^0.5+(x-c2(1))^2+(y-c2(1))^2)^0.5-2*v(i,i);
% ezplot(f,[])

程式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;