ICP及改进方法点云配准

采用MATLAB实现点云的ICP及KDtree 方法点云配准，实测可用。

%% demo.m
%
% Shows a couple of sample registrations using 
% ICP - Iterative Closest Point
%
% Jakob Wilm and Martin Kjer Technical University of Denmark 2012

m = 80; % width of grid
n = m^2; % number of points

[XY] = meshgrid（linspace（-22m） linspace（-22m））;

X = reshape（X1[]）;
Y = reshape（Y1[]）;

Z = sin（X）.*cos（Y）;

% Create the data point-matrix
D = [X; Y; Z];

% Translation values （a.u.）:
Tx = 0.5;
Ty = -0.3;
Tz = 0.2;

% Translation vector
T = [Tx; Ty; Tz];

% Rotation values （rad.）:
rx = 0.3;
ry = -0.2;
rz = 0.05;

Rx = [1 0 0;
      0 cos（rx） -sin（rx）;
      0 sin（rx） cos（rx）];
  
Ry = [cos（ry） 0 sin（ry）;
      0 1 0;
      -sin（ry） 0 cos（ry）];
  
Rz = [cos（rz） -sin（rz） 0;
      sin（rz） cos（rz） 0;
      0 0 1];

% Rotation matrix
R = Rx*Ry*Rz;

% Transform data-matrix

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2012-08-09 22:24  __MACOSX\
     文件         216  2012-07-17 05:28  __MACOSX\._demo.m
     文件         216  2012-08-09 22:23  __MACOSX\._icp.m
     文件        3746  2012-07-17 05:28  demo.m
     文件       18320  2012-08-09 22:23  icp.m
     文件        1350  2012-08-09 22:26  license.txt