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

Скрипт Open Live Script

В этом примере показано, как объединить несколько облаков точек для восстановления трехмерной сцены с помощью итерационного алгоритма ближайшей точки (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.

Документация Computer Vision Toolbox

Поддержка

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