3-D регистрации и сшивания облака точек

В этом примере показано, как объединить несколько облаков точек для восстановления 3-D сцены с помощью итерационного алгоритма ближайшей точки (ICP).

Обзор

В этом примере сшивается набор облаков точек, которые были захвачены Kinect, чтобы создать больший 3-D вид сцены. Пример применяет ICP к двум последующим облакам точек. Этот тип реконструкции может использоваться для разработки 3-D моделей объектов или создания 3-D карт мира для одновременной локализации и картографии (SLAM).

Регистрация двух облаков точек

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. Можно применить шаги предварительной обработки для фильтрации шума или задать начальные значения свойств, соответствующие вашим данным. Здесь предварительно обработайте данные методом понижающей дискретизации с помощью коробчатого сеточного фильтра и установите размер сеточного фильтра равным 10 см. Сеточный фильтр разделяет пространство облака точек на кубы. Точки внутри каждого куба объединяются в одну выходную точку путем усреднения их координат 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-D твердое преобразование с пониженной дискретизацией данных. Мы используем первое облако точек в качестве ссылки, а затем применяем предполагаемое преобразование к исходному второму облаку точек. Нам нужно объединить облако точек сцены с выровненным облаком точек, чтобы обработать перекрывающиеся точки.

Начните с нахождения твердого преобразования для выравнивания второго облака точек с первым облаком точек. Используйте его, чтобы преобразовать второе облако точек в систему координат ссылки, заданную первым облаком точек.

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

Теперь мы можем создать мировую сцену с зарегистрированными данными. Перекрывающаяся область фильтруется с помощью коробчатого сеточного фильтра 1,5 см. Увеличьте размер слияния, чтобы уменьшить потребность в памяти полученного облака точек сцены, и уменьшите размер слияния, чтобы увеличить разрешение сцены.

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. Axes 1 with title Initial world scene contains an object of type scatter. Axes 2 with title First input image contains an object of type image. Axes 3 with title Second input image contains an object of type image.

drawnow

Сшить последовательность облаков точек

Чтобы создать большую 3-D сцену, повторите ту же процедуру, что и выше, чтобы обработать последовательность облаков точек. Используйте первое облако точек, чтобы установить систему координат ссылки. Преобразуйте каждое облако точек в ссылку систему координат. Это преобразование является умножением попарных преобразований.

% 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. The axes 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. The axes with title Updated world scene contains an object of type scatter.

Для просмотра документации необходимо авторизоваться на сайте