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.

首先，讀取參考點雲（reference,target）和目標點雲（current,object）資料，在上述代碼中分別對應ptCloudRef與ptCloudCurrent； 去除體外孤點（可選，速度較慢） 降採樣，上述代碼中採用的是網格採樣的方法（除此之外，matlab point cloud processing還內建random方法，並且更加推薦使用random）。網格採樣大體思路是件點雲資料劃分為正方體網格，每個網格內輸出一個3D資料點，該點的位置則由網格中所有點平均得來。

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

經過上述處理之後，就可以通過ICP演算法來計算參考點雲和當前點雲之間的變換關係； 並將當前點雲轉換到參考點雲所在座標系當中；

mergeSize = 0.015;ptCloudScene = pcmerge(pcCloudRef, ptCloudAligned, mergeSize);% Visualize the input images.figuresubplot(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;

兩片點雲拼合到一起後，重疊地區同樣可以使用網格對之進行過濾
上圖即為兩幅即時室內3D點雲資料拼合到一起後的顯示結果。
3D點雲拼合
要組成更大範圍的3D情境，需要重複上述過程來處理一系列點雲資料。使用第一個點雲來建立參考座標系，將其他每個點雲資料變化到該座標系中。

% Store the transformation object that accumulates the transformation.accumTform = tform;figurehAxes = showPointCloud(ptCloudScene, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down');title('Updated world scene');% Set the axes property for faster renderinghAxes.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)')

可將本次變換矩陣用於下一次的變換矩陣初值，以加速變換過程。