| icp(model,data,max_iter,min_iter,fitting,thres,init_flag,tes_flag,refpnt)
|
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 Bergstrm 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
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);
|
|
