Multi.zip

clear all close all clc load('livingRoom.mat'); % Extract two consecutive point clouds and use the first point cloud as % reference. %提取两个连续的点云，并使用第一个点云作为参考。 ptCloudRef = livingRoomData{1}; ptCloudCurrent = livingRoomData{2}; ptCloud2 = livingRoomData{3}; ptCloud3 = livingRoomData{4}; ptCloud4 = livingRoomData{5}; ptCloud5 = livingRoomData{6}; ptCloud6 = livingRoomData{7}; ptCloud7 = livingRoomData{8}; 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. %注意，下采样步骤不仅加快了配准速度，但也可以提高准确度。 tform = pcregistericp(moving, fixed, 'Metric','pointToPlane','Extrapolate', true); ptCloudAligned = pctransform(ptCloudCurrent,tform); mergeSize = 0.015; ptCloudScene = pcmerge(ptCloudRef, ptCloudAligned, mergeSize); % Visualize the input images. %可视化输入图像。 figure subplot(2,2,1) imshow(ptCloudCurrent.Color) title('First input image','Color','w') drawnow subplot(2,2,3) imshow(ptCloud2.Color) title('Second input image','Color','w') drawnow fixed1 = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize); moving1 = pcdownsample(ptCloud2, 'gridAverage', gridSize); tform1 = pcregistericp(moving1, fixed1, 'Metric','pointToPlane','Extrapolate', true); ptCloudAligned1 = pctransform(ptCloud2,tform1); ptCloudScene1 = pcmerge(ptCloudCurrent, ptCloudAligned1, mergeSize); subplot(2,2,[2,4]) pcshow(ptCloudScene1, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down') title('Initial world scene') figure subplot(2,2,1) imshow(ptCloud4.Color) title('First input image','Color','w') drawnow subplot(2,2,3) imshow(ptCloud5.Color) title('Second input image','Color','w') drawnow fixed2 = pcdownsample(ptCloud4, 'gridAverage', gridSize); moving2 = pcdownsample(ptCloud5, 'gridAverage', gridSize); tform2 = pcregistericp(moving2, fixed2, 'Metric','pointToPlane','Extrapolate', true); ptCloudAligned2 = pctransform(ptCloud5,tform2); ptCloudScene2 = pcmerge(ptCloud4, ptCloudAligned2, mergeSize); subplot(2,2,[2,4]) pcshow(ptCloudScene2, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down') title('Initial world scene') figure subplot(2,2,1) imshow(ptCloud6.Color) title('First input image','Color','w') drawnow subplot(2,2,3) imshow(ptCloud7.Color) title('Second input image','Color','w') drawnow fixed3 = pcdownsample(ptCloud6, 'gridAverage', gridSize); moving3 = pcdownsample(ptCloud7, 'gridAverage', gridSize); tform3 = pcregistericp(moving3, fixed3, 'Metric','pointToPlane','Extrapolate', true); ptCloudAligned3 = pctransform(ptCloud7,tform3); ptCloudScene3 = pcmerge(ptCloud6, ptCloudAligned3, mergeSize); subplot(2,2,[2,4]) pcshow(ptCloudScene3, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down') title('Initial world scene') 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)') % 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. %申请ICP注册 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. %在录制过程中，指向下方为了更容易地可视化结果，让我们转换数据，使地平面与X-Z平面平行。 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)')