function [TR, TT] =icp(model,data,max_iter,min_iter,fitting,thres,init_flag,tes_flag,refpn t)% 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%/matlabcentral/fileexchange/loadFile.do?objectId=12627&objectType=FILE%% icp version 1.4%% written by Per Bergström 2007-03-07if nargin<1error('To few input arguments!');elseif or(nargin==1,nargin==2)bol=1;refpnt=[];if nargin==2if isempty(data)tes_flag=1;elseif isscalar(data)tes_flag=data;if not(tes_flag==1 | tes_flag==2)tes_flag=1;endelsebol=0;endelsetes_flag=1;endif bolglobal MODELif isempty(model)error('Model can not be an empty matrix.');endif (size(model,2)<size(model,1))MODEL=model';TR=eye(size(model,2));TT=zeros(size(model,2),1);elseMODEL=model;TR=eye(size(model,1));TT=zeros(size(model,1),1);endif (size(MODEL,2)==size(MODEL,1))error('This icp method demands the number of MODEL points to be greater then the dimension.');endicp_struct(tes_flag);returnendendglobal MODEL DATA TR TTif isempty(model)error('Model can not be an empty matrix.');endif (size(model,2)<size(model,1))MODEL=model';elseMODEL=model;endif (size(data,2)<size(data,1))data=data';DATA=data;elseDATA=data;endif size(DATA,1)~=size(MODEL,1)error('Different dimensions of DATA and MODEL!');endif nargin<9refpnt=[];if nargin<8tes_flag=1;if nargin<7init_flag=1;if nargin<6thres=1e-5; % threshold to icp iterations if nargin<5fitting=2; % fitting methodif nargin<4min_iter=4; % min number of icp iterationsif nargin<3max_iter=104; % max number of icp iterationsendendendendendendelseif nargin>9warning('Too many input arguments!');endif 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');endif 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.');endif isempty(min_iter)min_iter=4;endif isempty(max_iter)max_iter=100+min_iter;endif max_iter<min_iter;max_iter=min_iter;warning('max_iter<min_iter , max_iter has been changed to be equal min_iter');endif min_iter<0;min_iter=0;warning('min_iter<0 , min_iter has been changed to be equal 0');endif isempty(thres)thres=1e-5;elseif thres<0thres=abs(thres);warning('thres negative , thres have been changed to -thres');endif isempty(fitting)fitting=2;elseif fitting(1)==2[fi1,fi2]=size(fitting);lef=max([fi1,fi2]);if lef>1if fi1<fi2fitting=fitting';endif 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)))<0warning('Illegeal value of the weights! Unweighted minimization will be used.');fitting=2;elseif max(fitting(2:(size(data,2)+1)))==0warning('Illegeal values of the weights! Unweighted minimization will be used.');fitting=2;elsesu=sum(fitting(2:(size(data,2)+1)));fitting(2:(size(data,2)+1))=fitting(2:(size(data,2)+1))/su;thres=thres/su;endendelseif fitting(1)==3if length(fitting)<2fitting=[fitting,round(0.95*size(data,2))];elseif fitting(2)>1if fitting(2)>floor(fitting(2))fitting(2)=floor(fitting(2));warning(['lambda has been changed to',num2str(fitting(2)),'!']);endif fitting(2)>size(data,2)fitting(2)=size(data,2);warning(['lambda has been changed to',num2str(fitting(2)),'!']);endelseif fitting(2)>0if fitting(2)<=0.5warning('lambda small. Troubles might occur!');endfitting(2)=round(fitting(2)*size(data,2));elseif fitting(2)<=0fitting(2)=round(0.95*size(data,2));warning(['lambda has been changed to ',num2str(fitting(2)),'!']);endelsefitting=2;warning('fitting has been changed to 2');endif 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');endif (size(refpnt,2)>size(refpnt,1))refpnt=refpnt';endif (size(refpnt,1)~=size(DATA,1))if not(isempty(refpnt))refpnt=[];warning('Dimension of refpnt dismatch. refpnt is put to [] (empty matrix).');endendif (size(refpnt,2)>1)refpnt=refpnt(:,1);warning('Only the first point in refpnt is used.');end% Start the ICP algorithmN = size(DATA,2);icp_init(init_flag,fitting); % initiate a starting rotation matrix and starting translation vectortes_flag=icp_struct(tes_flag); % construct a Delaunay tesselation and two vectors make it easy to find in TesERROR=icp_closest_start(tes_flag,fitting); % initiate a vector with indices of closest MODEL pointsicp_transformation(fitting,[]); % find transformationDATA = TR*data; % apply transformationDATA=DATA+repmat(TT,1,N); %for iter=1:max_iterolderror = ERROR;ERROR=icp_closest(tes_flag,fitting); % find indices of closest MODEL points and total errorif iter<min_itericp_transformation(fitting,[]); % find transformation elseicp_transformation(fitting,refpnt); % find transformation endDATA = TR*data; % apply transformationDATA=DATA+repmat(TT,1,N); %if iter>=min_iterif abs(olderror-ERROR) < thresbreakendendend %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%function icp_init(init_flag,fitting)%function icp_init(init_flag)%ICP_INIT Initial alignment for ICP.global MODEL DATA TR TTif init_flag==0TR=eye(size(MODEL,1));TT=zeros(size(MODEL,1),1);elseif init_flag==1N = size(DATA,2);if fitting(1)==2if length(fitting)==1mem=mean(MODEL,2); med=mean(DATA,2);elsemem=MODEL*fitting(2:(N+1)); med=DATA*fitting(2:(N+1));endelsemem=mean(MODEL,2); med=mean(DATA,2);endTR=eye(size(MODEL,1));TT=mem-med;DATA=DATA+repmat(TT,1,N); % apply transformationelseerror('Wrong init_flag');end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%function tes_flag=icp_struct(tes_flag)if tes_flag==0global irif isempty(ir)global Tesindif isempty(Tesind)error('No tesselation system exists');elsetes_flag=2;endelsetes_flag=1;endelseif tes_flag==3returnelseglobal 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);elseTes=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(:,:);elseTes=delaunayn(MODELred');endendendTes=sortrows(sort(Tes,2));[mT,nT]=size(Tes);if tes_flag==1global ir jcnum=zeros(1,n);for i=1:mTfor j=1:nTnum(Tes(i,j))=num(Tes(i,j))+1;endendnum=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:mTfor j=1:nTir(ind(Tes(i,j)))=i;ind(Tes(i,j))=ind(Tes(i,j))+1;endendelse% tes_flag==2Tesind=zeros(mT,nT);Tesver=zeros(mT,nT);couvec=zeros(mT,1);for i=1:(mT-1)for j=(i+1):mTif couvec(i)==nTbreak;elseif Tes(i,1)==Tes(j,1)if nT==2Tesind(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;elsefor k=2:nTfor kk=k:nTifall(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;endendif or(couvec(i)==nT,couvec(j)==nT)breakendendendelseif Tes(i,1)==Tes(j,2)if nT==2Tesind(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;elsefor k=2:nTif 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;endif or(couvec(i)==nT,couvec(j)==nT)breakendendendelseif Tes(i,2)==Tes(j,1)if nT==2Tesind(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;elsefor k=2:nTif 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;endif or(couvec(i)==nT,couvec(j)==nT)breakendendendelseif Tes(i,2)==Tes(j,2)if nT==2Tesind(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;endelseif 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;endelseif Tes(j,1)>Tes(i,2)break;endendendendend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%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 pointsif tes_flag==3global MODEL DATA iclosestmd=size(DATA,2);mm=size(MODEL,2);iclosest=zeros(1,md);ERROR=0;for id=1:mddist=Inf;for im=1:mmdista=norm(MODEL(:,im)-DATA(:,id));if dista<disticlosest(id)=im;dist=dista;endendERROR=ERROR+err(dist,fitting,id);endelseif tes_flag==1global MODEL DATA Tes ir jc iclosestmd=size(DATA,2);iclosest=zeros(1,md);mid=round(md/2);iclosest(mid)=round(size(MODEL,2)/2);bol=logical(1);while bolbol=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<distcdistc=distcc;bol=not(bol);iclosest(mid)=ind;break;endendif bolbreak;endendendERROR=err(distc,fitting,mid);for id = (mid+1):mdiclosest(id)=iclosest(id-1);bol=not(bol);while bolbol=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<distcdistc=distcc;bol=not(bol);iclosest(id)=ind;break;endendif bolbreak;endendendERROR=ERROR+err(distc,fitting,id);endfor id=(mid-1):-1:1iclosest(id)=iclosest(id+1);bol=not(bol);while bolbol=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<distcdistc=distcc;bol=not(bol);iclosest(id)=ind;break;endendif bolbreak;endendendERROR=ERROR+err(distc,fitting,id);endelse% tes_flag==2global MODEL DATA Tes Tesind Tesver icTesind iclosestmd=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:2Ti=Tesind(icTesind(mid),in(ii));if Ti>0if not(visited(Ti))break;endendendif Ti==0bol=not(bol);elseif visited(Ti)bol=not(bol);elseTv=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)));endendiclosest(mid)=Tes(icTesind(mid),in(1));ERROR=err(so(1),fitting,mid);for id = (mid+1):mdiclosest(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:2Ti=Tesind(icTesind(id),in(ii));if Ti>0if not(visited(Ti))break;endendendif Ti==0bol=not(bol);elseif visited(Ti)bol=not(bol);elseTv=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)));endendiclosest(id)=Tes(icTesind(id),in(1));ERROR=ERROR+err(so(1),fitting,id);endfor id=(mid-1):-1:1iclosest(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:2Ti=Tesind(icTesind(id),in(ii));if Ti>0if not(visited(Ti))break;endendendif Ti==0bol=not(bol);elseif visited(Ti)bol=not(bol);elseTv=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)));endendiclosest(id)=Tes(icTesind(id),in(1));ERROR=ERROR+err(so(1),fitting,id);endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%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==3global MODEL DATA iclosestmd=size(DATA,2);mm=size(MODEL,2);ERROR=0;for id=1:mddist=Inf;for im=1:mmdista=norm(MODEL(:,im)-DATA(:,id));if dista<disticlosest(id)=im;dist=dista;endendERROR=ERROR+err(dist,fitting,id);endelseif tes_flag==1global 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 bolbol=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<distcdistc=distcc;bol=not(bol);iclosest(id)=ind;break;endendif bolbreak;endendendERROR=ERROR+err(distc,fitting,id);endelse% tes_flag==2global 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:2Ti=Tesind(icTesind(id),in(ii));if Ti>0if not(visited(Ti))break;endendendif Ti==0bol=not(bol);elseif visited(Ti)bol=not(bol);elseTv=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)));endendiclosest(id)=Tes(icTesind(id),in(1));ERROR=ERROR+err(so(1),fitting,id);endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%function icp_transformation(fitting,refpnt)% Finds the rotation and translation of the DATA pointsglobal MODEL DATA iclosest TR TTM=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');endif bolif fitting(1)==2if length(fitting)>1weights=fitting(2:(N+1)).*weights;weights=weights/sum(weights);endmed=DATA*weights; mem=MODEL(:,iclosest)*weights;A=DATA-repmat(med,1,N);B=MODEL(:,iclosest)-repmat(mem,1,N);for i=1:NA(:,i)=weights(i)*A(:,i);endelseif fitting(1)==3V=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));endendelseif fitting(1)==2if length(fitting)==1med=mean(DATA,2); mem=mean(MODEL(:,iclosest),2);A=DATA-repmat(med,1,N);B=MODEL(:,iclosest)-repmat(mem,1,N);elsemed=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:NA(:,i)=fitting(i+1)*A(:,i);endendelseif fitting(1)==3V=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));endend[U,S,V] = svd(B*A');U(:,end)=U(:,end)*det(U*V');R=U*V';% Compute the translationT=(mem-R*med);TR=R*TR; TT=R*TT+T;function ERR=err(dist,fitting,ind)if fitting(1)==2if (ind+1)>length(fitting)ERR=dist^2;elseERR=fitting(ind+1)*dist^2;endelseif fitting(1)==3ERR=dist^2;elseERR=0;warning('Unknown value of fitting!');endfunction weights=weightfcn(distances)maxdistances=max(distances);mindistances=min(distances);if maxdistances>1.1*mindistancesweights=1+mindistances/(maxdistances-mindistances)-distances/(maxdistan ces-mindistances);elseweights=maxdistances+mindistances-distances;endweights=weights/sum(weights);。