matlab 三维点云的配准和融合

%% 点云的配准 clc; clear; close all; %% 导入数据 dataFile = fullfile(toolboxdir('vision'), 'visiondata', 'livingRoom.mat'); load(dataFile); %% 依次将点云数据存出来 for i=1:length(livingRoomData) pcwrite(livingRoomData{i},['house_init',num2str(i)],'PLYFormat','binary'); end %% 参考点云数据 % Extract two consecutive point clouds and use the first point cloud as % reference. ptCloudRef = livingRoomData{1}; ptCloudCurrent = livingRoomData{2}; %% 显示点云 figure; pcshow(ptCloudRef); figure; pcshow(ptCloudCurrent); %% 点云的预处理 % Note that the downsampling step does not only speed up the registration, % but can also improve the accuracy. gridSize = 0.1; fixed = pcdownsample(ptCloudRef, 'gridAverage', gridSize); moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize); %% 确定点云之间的变换关系 % finding the rigid transformation for aligning the second point cloud with the first point cloud. tform = pcregrigid(moving, fixed, 'Metric','pointToPlane','Extrapolate', true); ptCloudAligned = pctransform(ptCloudCurrent,tform); %% 点云的融合 %The overlapped region is filtered using a 1.5cm box grid filter. %Increase the merge size to reduce the storage requirement of the %resulting scene point cloud, and decrease the merge size to increase %the scene resolution. mergeSize = 0.015; ptCloudScene = pcmerge(ptCloudRef, ptCloudAligned, mergeSize); %% 融合后点云的显示 figure; pcshow(ptCloudScene); %% 多点云的融合 %To compose a larger 3-D scene, repeat the same procedure as above to process %a sequence of point clouds. Use the first point cloud to establish the reference %coordinate system. Transform each point cloud to the reference coordinate system. %This transformation is a multiplication of pairwise transformations. % Store the transformation object that accumulates the transformation. accumTform = tform; for i = 3:length(livingRoomData) ptCloudCurrent = livingRoomData{i}; % 更新参考平面. fixed = moving; moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize); % ICP算法 tform = pcregrigid(moving, fixed, 'Metric','pointToPlane','Extrapolate', true); % 变换到参考坐标系下面 accumTform = affine3d(tform.T * accumTform.T); ptCloudAligned = pctransform(ptCloudCurrent, accumTform); % 融合，并更新新的点云 ptCloudScene = pcmerge(ptCloudScene, ptCloudAligned, mergeSize); end %% 融合后点云的显示 figure; pcshow(ptCloudScene); pcwrite(ptCloudScene,'house','PLYFormat','binary');