Main Content

3차원 포인트 클라우드 정합 및 이어 붙이기

이 예제에서는 ICP(Iterative Closest Point) 알고리즘을 사용해 여러 포인트 클라우드를 결합하여 3차원 장면을 복원하는 방법을 보여줍니다.

개요

이 예제에서는 Kinect로 캡처된 포인트 클라우드 모음을 함께 이어 붙여 장면에 대한 더 큰 3차원 보기를 생성합니다. 이 예제에서는 ICP를 두 개의 연속된 포인트 클라우드에 적용합니다. 이러한 유형의 복원 작업은 객체의 3차원 모델을 개발하거나 동시 위치추정 및 지도작성(SLAM)을 위한 3차원 세계 맵을 작성하는 데 사용할 수 있습니다.

두 개의 포인트 클라우드 정합시키기

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

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

정합 품질은 데이터 잡음 및 ICP 알고리즘의 초기 설정에 따라 결정됩니다. 전처리 단계를 적용하여 잡음을 필터링하거나 데이터에 적합한 초기 속성값을 설정할 수 있습니다. 여기서 상자 그리드 필터로 다운샘플링하여 데이터를 전처리하고 그리드 필터의 크기를 10cm로 설정하십시오. 그리드 필터는 포인트 클라우드 공간을 큐브로 나눕니다. 각 큐브 내의 점들은 이들의 X,Y,Z 좌표의 평균을 구하는 방식으로 출력 점 하나로 결합됩니다.

gridSize = 0.1;
fixed = pcdownsample(ptCloudRef, '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.

두 개의 포인트 클라우드를 정렬하려면 ICP 알고리즘을 사용하여 다운샘플링된 데이터에 대한 3차원 강체 변환을 추정해야 합니다. 첫 번째 포인트 클라우드를 기준으로 사용하여, 추정한 변환을 원래의 두 번째 포인트 클라우드에 적용합니다. 장면의 포인트 클라우드를 정렬된 포인트 클라우드와 병합하여 중첩된 점을 처리해야 합니다.

먼저 두 번째 포인트 클라우드를 첫 번째 포인트 클라우드와 정렬하기 위해 강체 변환을 구합니다. 이를 사용하여 두 번째 포인트 클라우드를 첫 번째 포인트 클라우드가 정의한 기준 좌표계로 변환합니다.

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

이제 정합된 데이터로 세계 장면을 만들 수 있습니다. 중첩된 영역은 1.5cm 상자 그리드 필터를 사용하여 필터링됩니다. 결과로 생성되는 장면 포인트 클라우드에 필요한 저장공간을 줄이려면 병합 크기를 늘리고, 장면의 해상도를 높이려면 병합 크기를 줄이십시오.

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

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

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

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

Figure contains 3 axes objects. Axes object 1 with title Initial world scene contains an object of type scatter. Axes object 2 with title First input image contains an object of type image. Axes object 3 with title Second input image contains an object of type image.

drawnow

포인트 클라우드 시퀀스 이어 붙이기

더 큰 3차원 장면을 구성하려면 위와 동일한 절차를 반복하여 포인트 클라우드 시퀀스를 처리하십시오. 첫 번째 포인트 클라우드를 사용하여 기준 좌표계를 설정합니다. 각 포인트 클라우드를 기준 좌표계로 변환합니다. 이 변환은 쌍별 변환의 곱셈입니다.

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

figure
hAxes = pcshow(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 = pcregistericp(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('limitrate')
end

Figure contains an axes object. The axes object with title Updated world scene contains an object of type scatter.

% 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));
pcshow(ptCloudScene, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', ...
        'Parent', hAxes)
title('Updated world scene')
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')

Figure contains an axes object. The axes object with title Updated world scene contains an object of type scatter.