В этом примере показано, как объединить несколько облаков точек для восстановления 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)')
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)')
