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')
drawnow

subplot(2,2,3)
imshow(ptCloudCurrent.Color)
title('Second input image')
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)')
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

% 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)')

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