icp.m

国外网站下载的比较经典的ICP算法

function [TR, TT] = icp(model,data,max_iter,min_iter,fitting,thres,init_flag,tes_flag,refpnt)
% ICP Iterative Closest Point Algorithm. Takes use of
% Delaunay tesselation of points in model.
%
%   Ordinary usage:
%
%   [R, T] = icp(model,data)
%
%   ICP fit points in data to the points in model.
%   Fit with respect to minimize the sum of square
%   errors with the closest model points and data points.
%
%   INPUT:
%
%   model - matrix with model points, [Pm_1 Pm_2 ... Pm_nmod]
%   data - matrix with data points,   [Pd_1 Pd_2 ... Pd_ndat]
%
%   OUTPUT:
%
%   R - rotation matrix and
%   T - translation vector accordingly so
%
%           newdata = R*data + T .
%
%   newdata are transformed data points to fit model
%
%
%   Special usage:
%
%   icp(model)  or icp(model,tes_flag)
%
%   ICP creates a Delaunay tessellation of points in
%   model and save it as global variable Tes. ICP also
%   saves two global variables ir and jc for tes_flag=1 (default) or
%   Tesind and Tesver for tes_flag=2, which
%   makes it easy to find in the tesselation. To use the global variables
%   in icp, put tes_flag to 0.
%
%
%   Other usage:
%
%   [R, T] = icp(model,data,max_iter,min_iter,...
%                         fitting,thres,init_flag,tes_flag)
%
%   INPUT:
%
% 	max_iter - maximum number of iterations. Default=104
%
% 	min_iter - minimum number of iterations. Default=4
%
%   fitting  -  =2 Fit with respect to minimize the sum of square errors. (default)
%                  alt. =[2,w], where w is a weight vector corresponding to data.
%                  w is a vector of same length as data.
%                  Fit with respect to minimize the weighted sum of square errors.
%               =3 Fit with respect to minimize the sum to the amount 0.95
%                  of the closest square errors.
%                  alt. =[3,lambda], 0.0<lambda<=1.0, (lambda=0.95 default)
%                  In each iteration only the amount lambda of the closest
%                  points will affect the translation and rotation.
%                  If 1<lambda<=size(data,2), lambda integer, only the number lambda
%                  of the closest points will affect the translation and
%                  rotation in each iteration.
%
% 	thres - error differens threshold for stop iterations. Default 1e-5
%
% 	init_flag  -  =0 no initial starting transformation
%                 =1 transform data so the mean value of
%                     data is equal to mean value of model.
%                     No rotation. (init_flag=1 default)
%
% 	tes_flag  -  =0 No new tesselation has to be done. There
%                   alredy exists one for the current model points.
%                =1 A new tesselation of the model points will
%                   be done. (default)
%                =2 A new tesselation of the model points will
%                   be done. Another search strategy than tes_flag=1
%                =3 The closest point will be find by testing
%                   all combinations. No Delaunay tesselation will be done.
%
%   refpnt - (optional) (An empty vector is default.) refpnt is a point corresponding to the
%                 set of model points wich correspondig data point has to be find.
%                 How the points are weighted depends on the output from the
%                 function weightfcn found in the end of this m-file. The input in weightfcn is the
%                 distance between the closest model point and refpnt.
%
%   To clear old global tesselation variables run: "clear global Tes ir jc" (tes_flag=1)
%   or run: "clear global Tes Tesind Tesver" (tes_flag=2) in Command Window.
%
%   m-file can be downloaded for free at
%   http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=12627&objectType=FILE
%
%   icp version 1.4
%
%   written by Per Bergstr鰉 2007-03-07

if nargin<1

    error('To few input arguments!');

elseif or(nargin==1,nargin==2)

    bol=1;
    refpnt=[];
    if nargin==2
        if isempty(data)
            tes_flag=1;
        elseif isscalar(data)
            tes_flag=data;
            if not(tes_flag==1 | tes_flag==2)
                tes_flag=1;
            end
        else
            bol=0;
        end
    else
        tes_flag=1;
    end

    if bol

        global MODEL

        if isempty(model)
            error('Model can not be an empty matrix.');
        end

        if (size(model,2)<size(model,1))
            MODEL=model';
            TR=eye(size(model,2));
            TT=zeros(size(model,2),1);
        else
            MODEL=model;
            TR=eye(size(model,1));
            TT=zeros(size(model,1),1);
        end

        if (size(MODEL,2)==size(MODEL,1))
            error('This icp method demands the number of MODEL points to be greater then the dimension.');
        end

        icp_struct(tes_flag);

        return
    end
end

global MODEL DATA TR TT

if isempty(model)
    error('Model can not be an empty matrix.');
end

if (size(model,2)<size(model,1))
    MODEL=model';
else
    MODEL=model;
end

if (size(data,2)<size(data,1))
    data=data';
    DATA=data;
else
    DATA=data;
end

if size(DATA,1)~=size(MODEL,1)
    error('Different dimensions of DATA and MODEL!');
end

if nargin<9
    refpnt=[];
    if nargin<8
        tes_flag=1;
        if nargin<7
            init_flag=1;
            if nargin<6
                thres=1e-5;                     % threshold to icp iterations
                if nargin<5
                    fitting=2;                  % fitting method
                    if nargin<4
                        min_iter=4;             % min number of icp iterations
                        if nargin<3
                            max_iter=104;       % max number of icp iterations
                        end
                    end
                end
            end
        end
    end
elseif nargin>9
    warning('Too many input arguments!');
end

if isempty(tes_flag)
    tes_flag=1;
elseif not(tes_flag==0 | tes_flag==1 | tes_flag==2 | tes_flag==3)
    init_flag=1;
    warning('init_flag has been changed to 1');
end

if and((size(MODEL,2)==size(MODEL,1)),tes_flag~=0)
    error('This icp method demands the number of model points to be greater then the dimension.');
end

if isempty(min_iter)
    min_iter=4;
end

if isempty(max_iter)
    max_iter=100+min_iter;
end

if max_iter<min_iter;
    max_iter=min_iter;
    warning('max_iter<min_iter , max_iter has been changed to be equal min_iter');
end

if min_iter<0;
    min_iter=0;
    warning('min_iter<0 , min_iter has been changed to be equal 0');
end

if isempty(thres)
    thres=1e-5;
elseif thres<0
    thres=abs(thres);
    warning('thres negative , thres have been changed to -thres');
end

if isempty(fitting)

    fitting=2;

elseif fitting(1)==2

    [fi1,fi2]=size(fitting);
    lef=max([fi1,fi2]);
    if lef>1
        if fi1<fi2
            fitting=fitting';
        end

        if lef<(size(data,2)+1)
            warning('Illegeal size of fitting! Unweighted minimization will be used.');
            fitting=2;
        elseif min(fitting(2:(size(data,2)+1)))<0
            warning('Illegeal value of the weights! Unweighted minimization will be used.');
            fitting=2;
        elseif max(fitting(2:(size(data,2)+1)))==0
            warning('Illegeal values of the weights! Unweighted minimization will be used.');
            fitting=2;
        else
            su=sum(fitting(2:(size(data,2)+1)));
            fitting(2:(size(data,2)+1))=fitting(2:(size(data,2)+1))/su;
            thres=thres/su;
        end
    end

elseif fitting(1)==3
    if length(fitting)<2
        fitting=[fitting,round(0.95*size(data,2))];
    elseif fitting(2)>1

        if fitting(2)>floor(fitting(2))
            fitting(2)=floor(fitting(2));
            warning(['lambda has been changed to ',num2str(fitting(2)),'!']);
        end
        if fitting(2)>size(data,2)
            fitting(2)=size(data,2);
            warning(['lambda has been changed to ',num2str(fitting(2)),'!']);
        end

    elseif fitting(2)>0

        if fitting(2)<=0.5
            warning('lambda small. Troubles might occur!');
        end

        fitting(2)=round(fitting(2)*size(data,2));

    elseif fitting(2)<=0
        fitting(2)=round(0.95*size(data,2));
        warning(['lambda has been changed to ',num2str(fitting(2)),'!']);
    end

else
    fitting=2;
    warning('fitting has been changed to 2');
end

if isempty(init_flag)
    init_flag=1;
elseif not(init_flag==0 | init_flag==1)
    init_flag=1;
    warning('init_flag has been changed to 1');
end

if (size(refpnt,2)>size(refpnt,1))
    refpnt=refpnt';
end
if (size(refpnt,1)~=size(DATA,1))
    if not(isempty(refpnt))
        refpnt=[];
        warning('Dimension of refpnt dismatch. refpnt is put to [] (empty matrix).');
    end
end
if (size(refpnt,2)>1)
    refpnt=refpnt(:,1);
    warning('Only the first point in refpnt is used.');
end

% Start the ICP algorithm

N = size(DATA,2);

icp_init(init_flag,fitting);                    % initiate a starting rotation matrix and starting translation vector

tes_flag=icp_struct(tes_flag);                  % construct a Delaunay tesselation and two vectors make it easy to find in Tes

ERROR=icp_closest_start(tes_flag,fitting);      % initiate a vector with indices of closest MODEL points

icp_transformation(fitting,[]);                 % find transformation

DATA = TR*data;                                 % apply transformation
DATA=DATA+repmat(TT,1,N);                       %

for iter=1:max_iter
    olderror = ERROR;
    ERROR=icp_closest(tes_flag,fitting);        % find indices of closest MODEL points and total error

    if iter<min_iter
        icp_transformation(fitting,[]);         % find transformation
    else
        icp_transformation(fitting,refpnt);     % find transformation
    end

    DATA = TR*data;                             % apply transformation
    DATA=DATA+repmat(TT,1,N);                   %

    if iter>=min_iter
        if abs(olderror-ERROR) < thres
            break
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function icp_init(init_flag,fitting)
%function icp_init(init_flag)
%ICP_INIT Initial alignment for ICP.

global MODEL DATA TR TT

if init_flag==0

    TR=eye(size(MODEL,1));
    TT=zeros(size(MODEL,1),1);

elseif init_flag==1

    N = size(DATA,2);

    if fitting(1)==2

        if length(fitting)==1
            mem=mean(MODEL,2); med=mean(DATA,2);
        else
            mem=MODEL*fitting(2:(N+1)); med=DATA*fitting(2:(N+1));
        end

    else
        mem=mean(MODEL,2); med=mean(DATA,2);
    end

    TR=eye(size(MODEL,1));
    TT=mem-med;
    DATA=DATA+repmat(TT,1,N);                         % apply transformation

else
    error('Wrong init_flag');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function tes_flag=icp_struct(tes_flag)

if tes_flag==0
    global ir
    if isempty(ir)
        global Tesind
        if isempty(Tesind)
            error('No tesselation system exists');
        else
            tes_flag=2;
        end
    else
        tes_flag=1;
    end
elseif tes_flag==3
    return
else
    global MODEL Tes

    [m,n]=size(MODEL);

    if m==1
        [so1,ind1]=sort(MODEL);
        Tes=zeros(n,2);
        Tes(1:(n-1),1)=ind1(2:n)';
        Tes(2:n,2)=ind1(1:(n-1))';
        Tes(1,2)=Tes(1,1);
        Tes(n,1)=Tes(n,2);
        Tes(ind1,:)=Tes(:,:);
    else
        Tes=delaunayn(MODEL');

        if isempty(Tes)

            mem=mean(MODEL,2);
            MODELm=MODEL-repmat(mem,1,n);
            [U,S,V]=svd(MODELm*MODELm');
            onbasT=U(:,diag(S)>1e-8)'
            MODELred=onbasT*MODEL;

            if size(MODELred,1)==1
                [so1,ind1]=sort(MODELred);
                Tes=zeros(n,2);
                Tes(1:(n-1),1)=ind1(2:n)';
                Tes(2:n,2)=ind1(1:(n-1))';
                Tes(1,2)=Tes(1,1);
                Tes(n,1)=Tes(n,2);
                Tes(ind1,:)=Tes(:,:);
            else
                Tes=delaunayn(MODELred');
            end
        end
    end
    Tes=sortrows(sort(Tes,2));
    [mT,nT]=size(Tes);

    if tes_flag==1
        global ir jc

        num=zeros(1,n);

        for i=1:mT
            for j=1:nT
                num(Tes(i,j))=num(Tes(i,j))+1;
            end
        end

        num=cumsum(num);

        jc=ones(1,n+1);
        jc(2:end)=num+jc(2:end);

        ir=zeros(1,num(end));
        ind=jc(1:(end-1));

        %% calculate ir;
        for i=1:mT
            for j=1:nT
                ir(ind(Tes(i,j)))=i;
                ind(Tes(i,j))=ind(Tes(i,j))+1;
            end
        end
    else    % tes_flag==2
        global Tesind Tesver

        Tesind=zeros(mT,nT);
        Tesver=zeros(mT,nT);

        couvec=zeros(mT,1);

        for i=1:(mT-1)
            for j=(i+1):mT

                if couvec(i)==nT
                    break;
                elseif Tes(i,1)==Tes(j,1)

                    if nT==2

                        Tesind(i,2)=j;
                        Tesind(j,2)=i;

                        Tesver(i,2)=2;
                        Tesver(j,2)=2;

                        couvec(i)=couvec(i)+1;
                        couvec(j)=couvec(j)+1;

                    else

                        for k=2:nT
                            for kk=k:nT
                                if all(Tes(i,[2:(k-1),(k+1):nT])==Tes(j,[2:(kk-1),(kk+1):nT]))
                                    Tesind(i,k)=j;
                                    Tesind(j,kk)=i;

                                    Tesver(i,k)=kk;
                                    Tesver(j,kk)=k;

                                    couvec(i)=couvec(i)+1;
                                    couvec(j)=couvec(j)+1;
                                end
                            end

                            if or(couvec(i)==nT,couvec(j)==nT)
                                break
                            end
                        end

                    end

                elseif Tes(i,1)==Tes(j,2)

                    if nT==2

                        Tesind(i,2)=j;
                        Tesind(j,1)=i;

                        Tesver(i,2)=1;
                        Tesver(j,1)=2;

                        couvec(i)=couvec(i)+1;
                        couvec(j)=couvec(j)+1;

                    else
                        for k=2:nT

                            if all(Tes(i,[2:(k-1),(k+1):nT])==Tes(j,3:nT))
                                Tesind(i,k)=j;
                                Tesind(j,1)=i;

                                Tesver(i,k)=1;
                                Tesver(j,1)=k;

                                couvec(i)=couvec(i)+1;
                                couvec(j)=couvec(j)+1;
                            end

                            if or(couvec(i)==nT,couvec(j)==nT)
                                break
                            end
                        end
                    end

                elseif Tes(i,2)==Tes(j,1)

                    if nT==2