基于ICP算法的图像配准的MATLAB实现

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

% points will affect the translation and rotation.

% If 1

% 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?objectI

d=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)