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 알고리즘의 초기 설정에 따라 결정됩니다. 전처리 단계를 적용하여 잡음을 필터링하거나 데이터에 적합한 초기 속성값을 설정할 수 있습니다. 여기서는 상자 그리드 필터를 사용하고 그리드 필터의 크기를 0.1m로 설정해서 데이터를 다운샘플링하는 전처리를 수행합니다. 그리드 필터는 포인트 클라우드 공간을 큐브로 나눕니다. 각 큐브 내의 점들은 이들의 X,Y,Z 좌표의 평균을 구하는 방식으로 출력 점 하나로 결합됩니다.

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

다운샘플링 단계는 정합 속도를 높일 뿐만 아니라 정확도도 높일 수 있습니다.

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

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

tform = pcregistericp(moving,fixed,Metric="pointToPlane");
ptCloudAligned = pctransform(ptCloudCurrent,tform);

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

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

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

subplot(2,2,3)
imshow(ptCloudCurrent.Color)
title("Second input image",Color="w")

% Visualize the world scene.
subplot(2,2,[2,4])
pcshow(ptCloudScene1,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, xlabel X (m), ylabel Y (m) 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.

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

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

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

figure
hAxes = pcshow(ptCloudScene1,VerticalAxis="Y",VerticalAxisDir="Down");
title("Updated world scene")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")

% 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");

    % Transform the current point cloud to the reference coordinate system
    % defined by the first point cloud.
    accumTform = rigidtform3d(accumTform.A * tform.A);
    ptCloudAligned = pctransform(ptCloudCurrent,accumTform);
    
    % Update the world scene.
    ptCloudScene1 = pcmerge(ptCloudScene1,ptCloudAligned,mergeSize);

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

Figure contains an axes object. The axes object with title Updated world scene, xlabel X (m), ylabel Y (m) contains an object of type scatter.

Kinect로 기록하는 동안에는 방향이 아래쪽을 가리키고 있었습니다. 지면 평면이 X-Z 평면과 평행하도록 장면을 변환합니다.

angle = -10;
translation = [0 0 0];
tform = rigidtform3d([angle 0 0],translation);
ptCloudScene1 = pctransform(ptCloudScene1,tform);

플롯의 카메라 각도를 변경하여 이어 붙인 결과를 자세히 검사합니다.

hAxes1 = pcshow(ptCloudScene1,AxesVisibility="off");
hAxes1.CameraPosition = [-0.6 0.2 0.5];
hAxes1.CameraTarget = [1.3 0.5 0.3];
hAxes1.CameraUpVector = [0.2 -0.9 -0.1];
hAxes1.CameraViewAngle = 60;

이어 붙인 장면이 정렬된 것처럼 보이더라도 자세히 검사하면 장면에 조금씩 이동된 부분(drift)들이 있는 것을 볼 수 있습니다. 예를 들어 판다 근처 책상 위의 꽃이 제대로 정렬되지 않았습니다. 응용 사례에 따라 장면의 정확도를 더욱 높이고 싶을 수 있습니다. 결과를 개선하려면 ICP 알고리즘의 Metric 이름-값 인수를 "planeToPlane"으로 설정해 보십시오. 또는 포인트 클라우드가 색 정보를 포함하는 경우 이 정보를 사용하여 3차원 장면의 정확도를 높일 수 있습니다.

색 정보를 사용하여 포인트 클라우드 시퀀스 이어 붙이기

pcregistericp 함수는 Metric 이름-값 인수를 "pointToPlaneWithColor" 또는 "planeToPlaneWithColor"로 설정하면 포인트 클라우드의 색 정보를 사용합니다. 함수 helperStitchPointCloudsUsingColor는 Metric 이름-값 인수를 "pointToPlaneWithColor"로 설정해서 이전 섹션의 단계를 반복합니다.

색 정보를 사용하여 포인트 클라우드 시퀀스를 이어 붙입니다.

ptCloudScene2 = helperStitchPointCloudsUsingColor(livingRoomData);

업데이트된 세계 장면을 시각화합니다.

figure
pcshow(ptCloudScene2,VerticalAxis="Y",VerticalAxisDir="Down")
title("Updated world scene with registration using color information")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")

Figure contains an axes object. The axes object with title Updated world scene with registration using color information, xlabel X (m), ylabel Y (m) contains an object of type scatter.

플롯의 카메라 각도를 변경하여 이어 붙인 결과를 자세히 검사합니다.

hAxes2 = pcshow(ptCloudScene2,AxesVisibility="off");
hAxes2.CameraPosition = [-12.6 -2.9 -0.9];
hAxes2.CameraTarget = [27.3 7.4 3.6];
hAxes2.CameraUpVector = [0.27 -0.93 -0.24];
hAxes2.CameraViewAngle = 11;

포인트 클라우드의 색 정보를 활용해서 이어 붙인 장면에는 이동된 부분이 더 줄어들었습니다. 예를 들어 결과로 생성된 3차원 장면에서 판다 옆에 있는 꽃의 정렬이 개선되었습니다.

결론

이 예제는 ICP 포인트 클라우드 정합을 사용해 여러 포인트 클라우드를 이어 붙여 3차원 장면을 복원하는 방법을 보여줍니다. 또한 ICP를 사용해 포인트 클라우드에 있는 색 정보를 활용하여 복원된 장면의 정확도를 높이는 방법도 보여줍니다.

지원 함수

helperStitchPointCloudsUsingColor 함수는 pcregistericp 함수를 "pointToPlaneWithColor" 메트릭과 함께 사용하여 여러 포인트 클라우드를 이어 붙여서 복원된 세계 장면을 반환합니다.

function ptCloudScene = helperStitchPointCloudsUsingColor(livingRoomData)

% Extract the first point cloud as reference.
ptCloudRef = livingRoomData{1};

% Downsample the point cloud.
gridSize = 0.1;
moving = pcdownsample(ptCloudRef,gridAverage=gridSize);

% Set the merge size to merge each point cloud to the scene.
mergeSize = 0.015;
ptCloudScene = ptCloudRef;

% Store the transformation object that accumulates the transformation.
accumTform = rigidtform3d();

for i = 2: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="pointToPlaneWithColor",InlierDistance=0.1);

    % Transform the current point cloud to the reference coordinate system
    % defined by the first point cloud.
    accumTform = rigidtform3d(accumTform.A * tform.A);
    ptCloudAligned = pctransform(ptCloudCurrent,accumTform);
    
    % Update the world scene.
    ptCloudScene = pcmerge(ptCloudScene,ptCloudAligned,mergeSize);
end

% During the recording, the Kinect was pointing downward.
% Transform the scene so that the ground plane is parallel
% to the X-Z plane.
angle = -10;
translation = [0 0 0];
tform = rigidtform3d([angle 0 0],translation);
ptCloudScene = pctransform(ptCloudScene,tform);
end