ICP实现代码MATLAB

function [TR TT] = icp（modeldatamax_itermin_iterfittingthresinit_flagtes_flagrefpnt）
% ICP Iterative Closest Point Algorithm. Takes use of
% Delaunay tesselation of points in model.
%
%   Ordinary usage:
%
%   [R T] = icp（modeldata）
%
%   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（modeltes_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（modeldatamax_itermin_iter...
%                         fittingthresinit_flagtes_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. =[2w] 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. =[3lambda] 0.0%                  In each iteration only the amount lambda of the closest
%                  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 c