icp.m

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

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

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

                    else

                        for k=2:nT

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

                                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,2)

                    if nT==2

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

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

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

                        if Tes(j,1)>Tes(i,2)
                            break;
                        end

                    elseif all(Tes(i,3:end)==Tes(j,3:end))

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

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

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

                    end

                elseif Tes(j,1)>Tes(i,2)
                    break;
                end
            end
        end
    end
end

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

function ERROR=icp_closest_start(tes_flag,fitting)
% Usage:
%           ERROR = icp_closest_start(tes_flag)
%
% ERROR=sum of all errors between points in MODEL and points in DATA.
%
% ICP_CLOSEST_START finds indexes of closest MODEL points for each point in DATA.
% The _start version allocates memory for iclosest and finds the closest MODEL points
% to the DATA points

if tes_flag==3

    global MODEL DATA iclosest
    md=size(DATA,2);
    mm=size(MODEL,2);

    iclosest=zeros(1,md);

    ERROR=0;

    for id=1:md
        dist=Inf;
        for im=1:mm
            dista=norm(MODEL(:,im)-DATA(:,id));
            if dista<dist
                iclosest(id)=im;
                dist=dista;
            end
        end
        ERROR=ERROR+err(dist,fitting,id);
    end

elseif tes_flag==1

    global MODEL DATA Tes ir jc iclosest

    md=size(DATA,2);
    iclosest=zeros(1,md);
    mid=round(md/2);

    iclosest(mid)=round(size(MODEL,2)/2);

    bol=logical(1);
    while bol
        bol=not(bol);
        distc=norm(DATA(:,mid)-MODEL(:,iclosest(mid)));
        distcc=2*distc;
        for in=ir(jc(iclosest(mid)):(jc(iclosest(mid)+1)-1))
            for ind=Tes(in,:)
                distcc=norm(DATA(:,mid)-MODEL(:,ind));
                if distcc<distc
                    distc=distcc;
                    bol=not(bol);
                    iclosest(mid)=ind;
                    break;
                end
            end
            if bol
                break;
            end
        end
    end

    ERROR=err(distc,fitting,mid);

    for id = (mid+1):md
        iclosest(id)=iclosest(id-1);
        bol=not(bol);
        while bol
            bol=not(bol);
            distc=norm(DATA(:,id)-MODEL(:,iclosest(id)));
            distcc=2*distc;
            for in=ir(jc(iclosest(id)):(jc(iclosest(id)+1)-1))
                for ind=Tes(in,:)
                    distcc=norm(DATA(:,id)-MODEL(:,ind));
                    if distcc<distc
                        distc=distcc;
                        bol=not(bol);
                        iclosest(id)=ind;
                        break;
                    end
                end
                if bol
                    break;
                end
            end
        end
        ERROR=ERROR+err(distc,fitting,id);
    end

    for id=(mid-1):-1:1
        iclosest(id)=iclosest(id+1);
        bol=not(bol);
        while bol
            bol=not(bol);
            distc=norm(DATA(:,id)-MODEL(:,iclosest(id)));
            distcc=2*distc;
            for in=ir(jc(iclosest(id)):(jc(iclosest(id)+1)-1))
                for ind=Tes(in,:)
                    distcc=norm(DATA(:,id)-MODEL(:,ind));
                    if distcc<distc
                        distc=distcc;
                        bol=not(bol);
                        iclosest(id)=ind;
                        break;
                    end
                end
                if bol
                    break;
                end
            end
        end
        ERROR=ERROR+err(distc,fitting,id);
    end
else  % tes_flag==2

    global MODEL DATA Tes Tesind Tesver icTesind iclosest

    md=size(DATA,2);
    iclosest=zeros(1,md);
    icTesind=zeros(1,md);

    [mTes,nTes]=size(Tes);

    mid=round(md*0.5);

    icTesind(mid)=round(mTes*0.5);
    iclosest(mid)=max(Tesind(icTesind(mid),:));

    visited=logical(zeros(1,mTes));

    visited(icTesind(mid))=1;

    di2vec=sqrt(sum((repmat(DATA(:,mid),1,nTes)-MODEL(:,Tes(icTesind(mid),:))).^2,1));
    bol=logical(1);

    while bol

        [so,in]=sort(di2vec);

        for ii=nTes:-1:2
            Ti=Tesind(icTesind(mid),in(ii));
            if Ti>0
                if not(visited(Ti))
                    break;
                end
            end
        end

        if Ti==0
            bol=not(bol);
        elseif visited(Ti)
            bol=not(bol);
        else
            Tv=Tesver(icTesind(mid),in(ii));
            visited(Ti)=1;
            icTesind(mid)=Ti;
            di2vec([1:(Tv-1),(Tv+1):nTes])=di2vec([1:(in(ii)-1),(in(ii)+1):nTes]);
            di2vec(Tv)=norm(DATA(:,mid)-MODEL(:,Tes(Ti,Tv)));
        end
    end
    iclosest(mid)=Tes(icTesind(mid),in(1));
    ERROR=err(so(1),fitting,mid);

    for id = (mid+1):md

        iclosest(id)=iclosest(id-1);
        icTesind(id)=icTesind(id-1);

        visited(:)=0;
        visited(icTesind(id))=1;

        di2vec=sqrt(sum((repmat(DATA(:,id),1,nTes)-MODEL(:,Tes(icTesind(id),:))).^2,1));
        bol=not(bol);

        while bol

            [so,in]=sort(di2vec);

            for ii=nTes:-1:2
                Ti=Tesind(icTesind(id),in(ii));
                if Ti>0
                    if not(visited(Ti))
                        break;
                    end
                end
            end

            if Ti==0
                bol=not(bol);
            elseif visited(Ti)
                bol=not(bol);
            else
                Tv=Tesver(icTesind(id),in(ii));
                visited(Ti)=1;
                icTesind(id)=Ti;
                di2vec([1:(Tv-1),(Tv+1):nTes])=di2vec([1:(in(ii)-1),(in(ii)+1):nTes]);
                di2vec(Tv)=norm(DATA(:,id)-MODEL(:,Tes(Ti,Tv)));
            end
        end
        iclosest(id)=Tes(icTesind(id),in(1));
        ERROR=ERROR+err(so(1),fitting,id);
    end

    for id=(mid-1):-1:1

        iclosest(id)=iclosest(id+1);
        icTesind(id)=icTesind(id+1);

        visited(:)=0;
        visited(icTesind(id))=1;

        di2vec=sqrt(sum((repmat(DATA(:,id),1,nTes)-MODEL(:,Tes(icTesind(id),:))).^2,1));
        bol=not(bol);

        while bol

            [so,in]=sort(di2vec);

            for ii=nTes:-1:2
                Ti=Tesind(icTesind(id),in(ii));
                if Ti>0
                    if not(visited(Ti))
                        break;
                    end
                end
            end

            if Ti==0
                bol=not(bol);
            elseif visited(Ti)
                bol=not(bol);
            else
                Tv=Tesver(icTesind(id),in(ii));
                visited(Ti)=1;
                icTesind(id)=Ti;
                di2vec([1:(Tv-1),(Tv+1):nTes])=di2vec([1:(in(ii)-1),(in(ii)+1):nTes]);
                di2vec(Tv)=norm(DATA(:,id)-MODEL(:,Tes(Ti,Tv)));
            end
        end
        iclosest(id)=Tes(icTesind(id),in(1));
        ERROR=ERROR+err(so(1),fitting,id);
    end

end

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

function ERROR=icp_closest(tes_flag,fitting)
% Usage:
%           ERROR = icp_closest(tes_flag,fitting)
%
% ERROR=sum of all errors between points in model and points in data.
%
% ICP_CLOSEST finds indexes of closest model points for each point in data.

if tes_flag==3

    global MODEL DATA iclosest
    md=size(DATA,2);
    mm=size(MODEL,2);

    ERROR=0;

    for id=1:md
        dist=Inf;
        for im=1:mm
            dista=norm(MODEL(:,im)-DATA(:,id));
            if dista<dist
                iclosest(id)=im;
                dist=dista;
            end
        end
        ERROR=ERROR+err(dist,fitting,id);
    end

elseif tes_flag==1

    global MODEL DATA Tes ir jc iclosest

    [mTes,nTes]=size(Tes);
    ERROR=0;
    bol=logical(0);

    for id=1:size(DATA,2)

        bol=not(bol);
        while bol
            bol=not(bol);
            distc=norm(DATA(:,id)-MODEL(:,iclosest(id)));
            distcc=2*distc;
            for in=ir(jc(iclosest(id)):(jc(iclosest(id)+1)-1))
                for ind=Tes(in,:)
                    distcc=norm(DATA(:,id)-MODEL(:,ind));
                    if distcc<distc
                        distc=distcc;
                        bol=not(bol);
                        iclosest(id)=ind;
                        break;
                    end
                end
                if bol
                    break;
                end
            end
        end
        ERROR=ERROR+err(distc,fitting,id);
    end

else  % tes_flag==2

    global MODEL DATA Tes Tesind Tesver iclosest icTesind

    [mTes,nTes]=size(Tes);
    ERROR=0;
    bol=logical(0);
    visited=logical(zeros(1,mTes));

    for id=1:size(DATA,2)
        visited(:)=0;
        visited(icTesind(id))=1;

        di2vec=sqrt(sum((repmat(DATA(:,id),1,nTes)-MODEL(:,Tes(icTesind(id),:))).^2,1));
        bol=not(bol);

        while bol

            [so,in]=sort(di2vec);

            for ii=nTes:-1:2
                Ti=Tesind(icTesind(id),in(ii));
                if Ti>0
                    if not(visited(Ti))
                        break;
                    end
                end
            end

            if Ti==0
                bol=not(bol);
            elseif visited(Ti)
                bol=not(bol);
            else
                Tv=Tesver(icTesind(id),in(ii));
                visited(Ti)=1;
                icTesind(id)=Ti;
                di2vec([1:(Tv-1),(Tv+1):nTes])=di2vec([1:(in(ii)-1),(in(ii)+1):nTes]);
                di2vec(Tv)=norm(DATA(:,id)-MODEL(:,Tes(Ti,Tv)));
            end
        end
        iclosest(id)=Tes(icTesind(id),in(1));
        ERROR=ERROR+err(so(1),fitting,id);
    end
end

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

function icp_transformation(fitting,refpnt)
% Finds the rotation and translation of the DATA points

global MODEL DATA iclosest TR TT

M=size(DATA,1);
N=size(DATA,2);

bol=0;

if not(isempty(refpnt))
    bol=1;
    dis=sqrt(sum((MODEL(:,iclosest)-repmat(refpnt,1,N)).^2,1));
    weights=weightfcn(dis');
end

if bol

    if fitting(1)==2

        if length(fitting)>1
            weights=fitting(2:(N+1)).*weights;
            weights=weights/sum(weights);
        end

        med=DATA*weights; mem=MODEL(:,iclosest)*weights;
        A=DATA-repmat(med,1,N);
        B=MODEL(:,iclosest)-repmat(mem,1,N);

        for i=1:N
            A(:,i)=weights(i)*A(:,i);
        end

    elseif fitting(1)==3

        V=sum((DATA-MODEL(:,iclosest)).^2,1);
        [soV,in]=sort(V);
        ind=in(1:fitting(2));

        weights(ind)=weights(ind)/sum(weights(ind));

        med=DATA(:,ind)*weights(ind); mem=MODEL(:,iclosest(ind))*weights(ind);
        A=DATA(:,ind)-repmat(med,1,fitting(2));
        B=MODEL(:,iclosest(ind))-repmat(mem,1,fitting(2));

        for i=1:fitting(2)
            A(:,i)=weights(ind(i))*A(:,ind(i));
        end

    end

else

    if fitting(1)==2

        if length(fitting)==1

            med=mean(DATA,2); mem=mean(MODEL(:,iclosest),2);
            A=DATA-repmat(med,1,N);
            B=MODEL(:,iclosest)-repmat(mem,1,N);

        else

            med=DATA*fitting(2:(N+1)); mem=MODEL(:,iclosest)*fitting(2:(N+1));
            A=DATA-repmat(med,1,N);
            B=MODEL(:,iclosest)-repmat(mem,1,N);

            for i=1:N
                A(:,i)=fitting(i+1)*A(:,i);
            end

        end

    elseif fitting(1)==3

        V=sum((DATA-MODEL(:,iclosest)).^2,1);
        [soV,in]=sort(V);
        ind=in(1:fitting(2));

        med=mean(DATA(:,ind),2); mem=mean(MODEL(:,iclosest(ind)),2);
        A=DATA(:,ind)-repmat(med,1,fitting(2));
        B=MODEL(:,iclosest(ind))-repmat(mem,1,fitting(2));

    end

end

[U,S,V] = svd(B*A');
U(:,end)=U(:,end)*det(U*V');
R=U*V';

% Compute the translation
T=(mem-R*med);
TR=R*TR;  TT=R*TT+T;

function ERR=err(dist,fitting,ind)

if fitting(1)==2
    if (ind+1)>length(fitting)
        ERR=dist^2;
    else
        ERR=fitting(ind+1)*dist^2;
    end
elseif fitting(1)==3
    ERR=dist^2;
else
    ERR=0;
    warning('Unknown value of fitting!');
end

function weights=weightfcn(distances)

maxdistances=max(distances);
mindistances=min(distances);

if maxdistances>1.1*mindistances
    weights=1+mindistances/(maxdistances-mindistances)-distances/(maxdistances-mindistances);
else
    weights=maxdistances+mindistances-distances;
end

weights=weights/sum(weights);