ICP.zip_ICP matlab_ICP算法_icp_icp 算法实现

function [TR, TT] = ICP(model,data,max_iter,min_iter,fitting,thres,init_flag,tes_flag) % 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. % % 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�m 2007-03-07 if nargin<1 error('To few input arguments!'); elseif or(nargin==1,nargin==2) bol=1; 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<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 elseif nargin>8 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)); wa