3D点云配准与拼合

本文来源于：http://cn.mathworks.com/help/vision/examples/3-d-point-cloud-registration-and-stitching.html

3D点云配准与拼合在点云处理中具有非常重要的作用，例如可以用于将kinect获取得到的一系列点云数据拼合到一起从而构建更大范围的3D场景。

本文将ICP算法应用于点云之间的配准，进一步可以用于建立物体的3D模型、建立真实世界的3D地图，学术上称这种方法为SLAM——Simultaneous localization and mapping.

3D点云配准

上图是ICP进行点云配准的具体过程，具体代码如下：

dataDir = fullfile(toolboxdir('vision'), 'visiondata', 'livingRoom.mat');
load(dataDir);

% Extract two consecutive point clouds and use the first point cloud as
% reference.
pcCloudRef = livingRoomData{1};
ptCloudCurrent = livingRoomData{2};

gridSize = 0.1;
fixed = pcdownsample(pcCloudRef, 'gridAverage', gridSize);
moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize);

% Note that the downsampling step does not only speed up the registration,
% but can also improve the accuracy.

1. 首先，读取参考点云（reference,target）和目标点云（current,object）数据，在上述代码中分别对应ptCloudRef与ptCloudCurrent；
2. 去除体外孤点（可选，速度较慢）
3. 降采样，上述代码中采用的是网格采样的方法（除此之外，matlab point cloud processing还自带random方法，并且更加推荐使用random）。网格采样大体思路是件点云数据划分为正方体网格，每个网格内输出一个3D数据点，该点的位置则由网格中所有点平均得来。

tform = pcregrigid(moving, fixed, 'Metric','pointToPlane','Extrapolate', true);
ptCloudAligned = pctransform(ptCloudCurrent,tform);

1. 经过上述处理之后，就可以通过ICP算法来计算参考点云和当前点云之间的变换关系；
2. 并将当前点云转换到参考点云所在坐标系当中；

mergeSize = 0.015;
ptCloudScene = pcmerge(pcCloudRef, ptCloudAligned, mergeSize);

% Visualize the input images.
figure
subplot(2,2,1);
imshow(pcCloudRef.Color);
title('First input image');
drawnow;

subplot(2,2,3);
imshow(ptCloudCurrent.Color);
title('Second input image');
drawnow;

% Visualize the world scene.
subplot(2,2,[2,4]);
showPointCloud(ptCloudScene, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down');
title('Initial world scene');
xlabel('X (m)');
ylabel('Y (m)')
zlabel('Z (m)')
drawnow;

1. 两片点云拼合到一起后，重叠区域同样可以使用网格对之进行过滤

上图即为两幅实时室内3D点云数据拼合到一起后的显示结果。

3D点云拼合

要组成更大范围的3D场景，需要重复上述过程来处理一系列点云数据。使用第一个点云来建立参考坐标系，将其他每个点云数据变化到该坐标系中。

% Store the transformation object that accumulates the transformation.
accumTform = tform;

figure
hAxes = showPointCloud(ptCloudScene, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down');
title('Updated world scene');
% Set the axes property for faster rendering
hAxes.CameraViewAngleMode = 'auto';
hScatter = hAxes.Children;

for i = 3:length(livingRoomData)
    ptCloudCurrent = livingRoomData{i};

    % Use previous moving point cloud as reference.
    fixed = moving;
    moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize);

    % Apply ICP registration.
    tform = pcregrigid(moving, fixed, 'Metric','pointToPlane','Extrapolate', true);

    % Transform the current point cloud to the reference coordinate system
    % defined by the first point cloud.
    accumTform = affine3d(tform.T * accumTform.T);
    ptCloudAligned = pctransform(ptCloudCurrent, accumTform);

    % Update the world scene.
    ptCloudScene = pcmerge(ptCloudScene, ptCloudAligned, mergeSize);

    % Visualize the world scene.
    hScatter.XData = ptCloudScene.Location(:,1);
    hScatter.YData = ptCloudScene.Location(:,2);
    hScatter.ZData = ptCloudScene.Location(:,3);
    hScatter.CData = ptCloudScene.Color;
    drawnow('update');
end

% During the recording, the Kinect was pointing downward. To visualize the
% result more easily, let's transform the data so that the ground plane is
% parallel to the X-Z plane.
angle = -pi/10;
A = [1,0,0,0;...
     0, cos(angle), sin(angle), 0; ...
     0, -sin(angle), cos(angle), 0; ...
     0 0 0 1];
ptCloudScene = pctransform(ptCloudScene, affine3d(A));
showPointCloud(ptCloudScene, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', ...
        'Parent', hAxes);
title('Updated world scene');
xlabel('X (m)');
ylabel('Y (m)')
zlabel('Z (m)')

1. 可将本次变换矩阵用于下一次的变换矩阵初值，以加速变换过程。