三维激光扫描点云配准

很好的学习点云配准的资料，这里是经典ICP算法的代码

%% 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 plus noise into model-matrix 
M = R * D + repmat（T 1 n）;

% Add noise to model and data
rng（2912673）;
M = M + 0.01*randn（3n）;
D = D + 0.01*randn（3n）;

%% Run ICP （standard settings）
[Ricp Ticp ER t] = icp（M D 15）;

% Transform data-matrix using ICP result
Dicp = Ricp * D + repmat（Ticp 1 n）;

% Plot model points blue and transformed points red
figure;
subplot（221）;
plot3（M（1:）M（2:）M（3:）‘bo‘D（1:）D（2:）D（3:）‘r.‘）;
axis equal;
xlabel（‘x‘）; ylabel（‘y‘）; zlabel（‘z‘）;
title（‘Red: z=sin（x）*cos（y） blue: transformed point cloud‘）;

% Plot the results
subplot（222）;
plot3（M（1:）M（2:）M（3:）‘bo‘Dicp（1:）Dicp（2:）Dicp（3:）‘r.‘）;
axis equal;
xlabel（‘x‘）; ylabel（‘y‘）; zlabel（‘z‘）;
title（‘ICP result‘）;

% Plot RMS curve
subplot（22[3 4]）;
plot（0:15ER‘--x‘）;
xlabel（‘iteration#‘）;
ylabel（‘d_{RMS}‘）;
legend（‘bruteForce matching‘）;
title（[‘Total elapsed time: ‘ num2str（t（end）2） ‘ s‘]）;

%% Run ICP （fast kDtree matching and extrapolation）
[Ricp Ticp ER t] = icp（M D 15 ‘Matching‘ ‘kDtree‘ ‘Extrapolation‘ true）;

% Transform data-matrix using ICP result
Dicp = Ricp * D + repmat（Ticp 1 n）;

% Plot model points blue and transformed points red
figure;
subplot（221）;
plot3（M（1:）M（2:）M（3:）‘bo‘D（1:）D（2:）D（3:）‘r.‘）;
axis equal;
xlabel（‘x‘）; ylabel（‘y‘）; zlabel（‘z‘）;
title（‘Red: z=sin（x）*cos（y） blue: transformed point cloud‘）;

% Plot the results
subplot（222）;
plot3（M（1:）M（2:）M（3:）‘bo‘Dicp（1:）Dicp（2:）Dicp（3:）‘r.‘）;
axis equal;
xlabel（‘x‘）; ylabel（‘y‘）; zlabel（‘z‘）;
title（‘ICP result‘）;

% Plot RMS curve
subplot（22[3 4]）;
plot（0:15ER‘--x‘）;
xlabel（‘iteration#‘）;
ylabel（‘d_{RMS}‘）;
legend（‘kDtree matching and extrapolation‘）;
title（[‘Total elapsed time: ‘ num2str（t（end）2） ‘ s‘]）;

%% Run ICP （partial data）

% Partial model point cloud
Mp = M（:Y>=0）;

% Boundary of partial model point cloud
b = （abs（X（Y>=0）） == 2） | （Y（Y>=0） == min（Y（Y>=0））） | （Y（Y>=0） == max（Y（Y>=0）））;
bound = find（b）;

% Partial data point cloud
Dp = D（:X>=0）;

[Ricp Ticp ER t] = icp（Mp Dp 50 ‘EdgeRejection‘ true ‘Boundary‘ bound ‘Matching‘ ‘kDtree‘）;

% Transform data-matrix using ICP result
Dicp = Ricp * Dp + repmat（Ticp 1 size（Dp2））;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2012-09-21 13:38  __MACOSX\
     文件         216  2012-07-16 23:28  __MACOSX\._demo.m
     文件         284  2012-09-21 13:38  __MACOSX\._icp.m
     文件        3746  2012-07-16 23:28  demo.m
     文件       18342  2012-09-21 13:38  icp.m
     文件        1350  2012-09-21 13:42  license.txt