点云配准icp算法及三个实现Matlab

Matlab实现的经典icp点云数据配准算法，内含三个实例，二维平面下、三位平面下点云数据配准matlab实现程序

function [TRTTdata] = icp（modeldatamaxIterminItercritFunthres）
% ICP （iterative closest point） algorithm
%
%        # point-to-point distance minimization
%
%        # robust criterion function using IRLS （optional）
%
%
%   Simple usage （least-squares minimization）:
%
%   [RTdata2] = icp（modeldata）
%
%   ICP fits points in data to the points in model.
%   （default） Fit with respect to minimize the sum of square
%   errors with the closest model points and data points.
%   （optional） Using a robust criterion function
%
%   INPUT:
%
%   model - matrix with model points [ X_1 X_2 ... X_M ]
%   data - matrix with data points   [ P_1 P_2 ... P_N ]
%
%   OUTPUT:
%
%   R - rotation matrix
%   T - translation vector
%   data2 - matrix with transformed data points   [ P_1 P_2 ... P_N ]
%
%           data2 = R*data + T
%
%
%   Usage:
%
%   [RTdata2] = icp（modeldatamaxIterminItercritFunthres）
%
%   INPUT:
%
% 	maxIter - maximum number of iterations. Default = 100
%
% 	minIter - minimum number of iterations. Default = 5
%
%   critFun  -  0 Fit with respect to minimize the sum of square errors. （default）
%               1 Huber criterion function （robust）
%               2 Tukey‘s bi-weight criterion function （robust）
%               3 Cauchy criterion function （robust）
%               4 Welsch criterion function （robust）
%
% 	thres - error differens threshold to stop iterations. Default = 1e-5
%
%   m-file can be downloaded for free at
%   http://www.mathworks.com/matlabcentral/fileexchange/12627-iterative-closest-point-method
%
%   icp version 1.6
%
%   written by Per Bergstr?m 2016-12-11
%
% Reference:
%
% Bergstr?m P. and Edlund O. 2014 ‘Robust registration of point sets using iteratively reweighted least squares‘
% Computational Optimization and Applications vol 58 no. 3 pp. 543-561 10.1007/s10589-014-9643-2
%
% Check input arguments
if nargin<2
    
    error（‘To few input arguments‘）;
    
elseif nargin<6
    
    thres=1e-5;                     % threshold to stop icp iterations
    if nargin<5
        critFun=0;                  % critFun method LS
        if nargin<4
            minIter=5;              % min number of icp iterations
            if nargin<3
                maxIter=100;        % max number of icp iterations
            end
        end
    end
    
end
if or（isempty（model）isempty（data））
    error（‘Something is wrong with the model points and data points‘）;
end
% Use default values
if isempty（maxIter）
    maxIter=100;
end
if isempty（minIter）
    minIter=5;
end
if isempty（critFun）
    critFun=0;
end
if isempty（thres）
    thres=1e-5;
end
% Size of model points and data points
if （size（model2）    mTranspose=true;
    m=size（model2）;
    M=size（model1）;
else
    mTranspose=false;
    m=size（model1）;
    M=size（model2）;
end
if （size（data2）    data=data‘;
end
if m~=size（

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1837  2020-05-14 18:11  icpexam2.m

     文件       2613  2020-05-14 18:12  icpexam3.m

     文件       8321  2020-05-14 18:14  icp.m

     文件        767  2020-05-14 18:12  icpexam.m

----------- ---------  ---------- -----  ----

                13538                    4