www.gusucode.com > IPCV_Eval_Kit_R2019a_0ce6858工具箱matlab程序源码 > IPCV_Eval_Kit_R2019a_0ce6858/code/demo_files/I4_10_2_myPointCloudsExample.m

    %% 3師尦揰孮偺 昞帵丒埵抲崌傢偣丒寢崌
clc;clear;close all;imtool close all;

%% unorganized 3師尦揰孮偺PLY宍幃偺僨乕僞偺撉傒崬傒乮pointCloud 僋儔僗乯%%%%%%%%
ptCloud = pcread('teapot.ply')   % 41472屄偺x,y,z偺慻丗僐儅儞僪 僂傿儞僪僂嶲徠

%% 椺乯1斣栚偺揰偺丄x,y,z嵗昗
ptCloud.Location(1,:)

%% 昞帵 (怓僨乕僞偑側偄偺偱丄Z幉曽岦偵僌儔僨乕僔儑儞怓昞帵)
figure; pcshow(ptCloud);
xlabel('X'); ylabel('Y'); zlabel('Z'); box on

%% 暿偺僨乕僞僙僢僩丗unorganized偺3師尦揰孮偺張棟
% 揰孮僨乕僞偺撉傒崬傒
load('object3d.mat')

%% 揰孮偺昞帵
figure
pcshow(ptCloud)
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')
title('Original Point Cloud')

%% 僥乕僽儖偺専弌(暯柺僼傿僢僥傿儞僌)
% 暯柺僼傿僢僥傿儞僌偺僷儔儊乕僞偺愝掕
maxDistance = 0.02;
referenceVector = [0,0,1];
maxAngularDistance = 5;
[model1,inlierIndices,outlierIndices] = pcfitplane(ptCloud,...
            maxDistance,referenceVector,maxAngularDistance);
plane1 = select(ptCloud,inlierIndices);
remainPtCloud = select(ptCloud,outlierIndices);
figure
pcshow(plane1)
title('嵟弶偺暯柺')

%% 嵍偺暻柺偺専弌(暯柺僼傿僢僥傿儞僌)
roi = [-inf,inf;0.4,inf;-inf,inf];
sampleIndices = findPointsInROI(remainPtCloud,roi);
[model2,inlierIndices,outlierIndices] = pcfitplane(remainPtCloud,...
            maxDistance,'SampleIndices',sampleIndices);
plane2 = select(remainPtCloud,inlierIndices);
remainPtCloud = select(remainPtCloud,outlierIndices);
figure
pcshow(plane2)
title('2斣栚偺暯柺')

%% 墱偺暻柺偺専弌(暯柺僼傿僢僥傿儞僌)
roi = [-inf,inf;-inf,inf;-inf,inf];
sampleIndices = findPointsInROI(remainPtCloud,roi);
[model3,inlierIndices,outlierIndices] = pcfitplane(remainPtCloud,...
            maxDistance,'SampleIndices',sampleIndices);
plane3 = select(remainPtCloud,inlierIndices);
remainPtCloud = select(remainPtCloud,outlierIndices);
figure
pcshow(plane3)
title('3斣栚偺暯柺')

%% 巆傝偺揰孮偺昞帵
figure
axPc = pcshow(remainPtCloud);
axis([ptCloud.XLimits, ptCloud.YLimits, ptCloud.ZLimits]);
hold on;
plot(model1,'Color','yellow');
alpha(0.5);
plot(model2,'Color','magenta');
alpha(0.5);
plot(model3,'Color','cyan');
alpha(0.5);
title('巆傝偺揰孮傪昞帵')

%% 揰孮偺僙僌儊儞僥乕僔儑儞
distThreshold = 0.1;
[labels,numClusters] = pcsegdist(remainPtCloud,distThreshold);
cmap = lines(numClusters);
for k = 1:numClusters
    pc = select(remainPtCloud,find(labels==k));
    I4_10_2_plot3DBBox(pc,cmap(k,:),gca);
end
title('揰孮偺僙僌儊儞僥乕僔儑儞(僋儔僗僞儕儞僌)')
shg;

%% 暿偺僨乕僞僙僢僩丗organized 3師尦揰孮僨乕僞偺撉崬傒 %%%%%%%%%%%%%%%%%%%%%%%%%
%    livingRoomData: 44枃偺PointCloud僨乕僞乮奺揰偑world嵗昗抣偲丄怓傪帩偮乯
load('livingRoom.mat');

%% 1枃栚偺僨乕僞傪拪弌
ptCloudRef = livingRoomData{1}   %1斣栚偺僨乕僞傪儕僼傽儗儞僗PointCloud偲偡傞

%% 椺乯(100,100)偺 x,y,z嵗昗
ptCloudRef.Location(100, 100, :)

%% 怓僨乕僞(僇儔乕夋憸)傪昞帵
figure; imshow(ptCloudRef.Color); title('1st');

%% 3師尦揰孮僨乕僞傪昞帵
figure; pcshow(ptCloudRef, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('1st'); view(0,-90); box on;

%% 2斣栚偺僨乕僞傪拪弌丒暲傋偰昞帵
ptCloudCurrent = livingRoomData{2};
% 昞帵
subplot(1,2,1);pcshow(ptCloudRef, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('1st'); view(0,-90);box on
subplot(1,2,2);pcshow(ptCloudCurrent, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('2nd'); view(0,-90);box on; shg;

%% 2偮偺揰孮僨乕僞傪廳偹偰昞帵
figure; pcshowpair(ptCloudRef, ptCloudCurrent, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down');
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); box on

%% [慜張棟] PointCloud偺僨乕僞揰傪娫堷偒乮僆儕僕僫儖偼307,200僨乕僞揰 => 栺1500僨乕僞揰傊乯
%    棫曽懱(box grid)撪偺慡揰偺嵗昗傪暯嬒偟偰1偮偺揰偵廤栺
%    埵抲偁傢偣偺懍搙側傜傃偵惛搙偺夵慞乮pcdenoise偺巊梡偼僆僾僔儑僫儖乯
gridSize = 0.1;   % 10cm妏丂乮揰孮僨乕僞偺X,Y,Z抣偺扨埵偑m乯
fixed = pcdownsample(ptCloudRef, 'gridAverage', gridSize);
moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize);

% 昞帵
figure;
subplot(1,2,1);pcshow(fixed, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('1st'); view(0,-90);box on
subplot(1,2,2);pcshow(moving, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('2nd'); view(0,-90);box on; shg;

%% ICP(iterative closest point) algorithm偵傛傞儗僕僗僩儗乕僔儑儞
% 儗僕僗僩儗乕僔儑儞乮2偮偺揰孮娫偺曄姺峴楍傪悇掕乯
tformICP = pcregistericp(moving, fixed, 'Metric','pointToPlane','Extrapolate', true)
tformICP.T     % 曄姺(幩塭)峴楍傪妋擣

% (娫堷偒側偟偺僨乕僞偵懳偟) 婔壗妛揑曄姺偟丄2斣栚偺揰孮傪丄1斣栚偺揰孮偺嵗昗傊曄姺\帵
ptCloudAlignedICP = pctransform(ptCloudCurrent, tformICP);

%昞帵
subplot(2,2,1);pcshow(ptCloudRef, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('1st'); view(0,-90); box on;
subplot(2,2,2);pcshow(ptCloudCurrent, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('2nd'); view(0,-90); box on;
subplot(2,2,3);pcshow(ptCloudAlignedICP, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('2nd (Aligned with ICP)'); view(0,-90); box on; shg;

%% NDT(normal-distributions transform) algorithm偵傛傞儗僕僗僩儗乕僔儑儞
% 儗僕僗僩儗乕僔儑儞乮2偮偺揰孮娫偺曄姺峴楍傪悇掕乯
gridStep = 0.5;
tformICP = pcregisterndt(moving,fixed,gridStep)
tformICP.T     % 曄姺(幩塭)峴楍傪妋擣

% (娫堷偒側偟偺僨乕僞偵懳偟) 婔壗妛揑曄姺偟丄2斣栚偺揰孮傪丄1斣栚偺揰孮偺嵗昗傊曄姺\帵
ptCloudAlignedNDT = pctransform(ptCloudCurrent, tformICP);

%昞帵
subplot(2,2,4);pcshow(ptCloudAlignedNDT, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('2nd (Aligned with NDT)'); view(0,-90); box on; shg;

%% CPD(coherent point drift)傾儖僑儕僘儉偵傛傞旕崉懱揰孮儗僕僗僩儗乕僔儑儞
% 2偮偺揰孮娫偺懳墳娭學偐傜揔梡愭偺奺揰孮偺displacement傪悇掕
tformCPD = pcregistercpd(moving,fixed);

% 2斣栚偺揰孮傪丄1斣栚偺揰孮偺嵗昗傊曄姺
% (婔壗妛曄姺峴楍偺悇掕偱偼側偔丄
% 揰孮娫偺displacement傪寁嶼偟偰偄傞偨傔僟僂儞僒儞僾儖偟偨傕偺偵揔梡)
ptCloudAlignedCPD = pctransform(moving, tformCPD);

%昞帵
figure; pcshow(ptCloudAlignedCPD, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('2nd (Aligned with CPD)'); view(0,-90); box on; shg;

%% 2偮偺3師尦揰孮傪摑崌\帵
mergeSize = 0.015;   % 1.5cm偺box grid filter偱丄廳暋椞堟傪僼傿儖僞儕儞僌丅
                     % box撪偵暋悢揰偑偁傞応崌丄埵抲丒怓偺暯嬒傪寁嶼
ptCloudSceneICP = pcmerge(ptCloudRef, ptCloudAlignedICP, mergeSize);

% 昞帵
subplot(1,2,1);pcshow(ptCloudRef, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('1st'); view(0,-90); box on
subplot(1,2,2);pcshow(ptCloudSceneICP, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); title('Stitched (1st+2nd)'); view(0,-90);box on; shg;

%% 3斣栚埲崀偺揰孮傕寢崌偟偰偄偔
accumTformICP = tformICP; 

figure;
hAxes = pcshow(ptCloudSceneICP, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); box on
% Axes偺僾儘僷僥傿乕傪僙僢僩乮昤夋偺崅懍壔乯
hAxes.CameraViewAngleMode = 'auto';
hScatter = hAxes.Children;

for i = 3:44      %length(livingRoomData)
    % 師偺億僀儞僩僋儔僂僪僨乕僞傪撉崬傒
    ptCloudCurrent = livingRoomData{i};
    % 堦偮慜偺億僀儞僩僋儔僂僪僨乕僞傪儕僼傽儗儞僗偲偟偰愝掕
    fixed = moving;
    % 億僀儞僩僋儔僂僪偺僨乕僞傪娫堷偒
    moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize);
    
    % ICP 埵抲偁傢偣傪揔梡
    tformICP = pcregistericp(moving, fixed, 'Metric','pointToPlane','Extrapolate', true);

    % 嵟弶偺夋憸偺嵗昗宯傊曄姺
    accumTformICP = affine3d(tformICP.T * accumTformICP.T);
    ptCloudAlignedICP = pctransform(ptCloudCurrent, accumTformICP);
    
    % 揰孮偺寢崌
    ptCloudSceneICP = pcmerge(ptCloudSceneICP, ptCloudAlignedICP, mergeSize);

    % Visualize the world scene.
    hScatter.XData = ptCloudSceneICP.Location(:,1);
    hScatter.YData = ptCloudSceneICP.Location(:,2);
    hScatter.ZData = ptCloudSceneICP.Location(:,3);
    hScatter.CData = ptCloudSceneICP.Color;
    drawnow limitrate;
end  

%% 彴偺柺傪悇掕偡傞偨傔偺僷儔儊乕僞傪掕媊
maxDistance = 0.01;                 % 悇掕偟偨柺偲inlier揰偺嵟戝嫍棧 (1cm)
referenceVector = [0, 5, 1.5];      % 嶲徠梡 柺偺朄慄儀僋僩儖
% 嶲徠梡偺朄慄儀僋僩儖偲丄悇掕偡傞柺偺朄慄儀僋僩儖偺嫋梕嵟戝岆嵎乮愨懳抣乯
maxAngularDistance = 5;
rng('default');

%% 嶲徠梡偺朄慄儀僋僩儖偵嬤偄柺傪悇掕
[model1, inlierIndices, outlierIndices] = ...
      pcfitplane(ptCloudSceneICP,maxDistance,referenceVector,maxAngularDistance);
model1        % planeModel 僆僽僕僃僋僩: ax+by+cz+d=0 偺學悢丄朄慄儀僋僩儖

%% 慡3師尦揰孮僨乕僞偵丄悇掕偟偨柺傪廳偹彂偒
figure; pcshow(ptCloudSceneICP, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18); hold on;
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); box on

h1 = plot(model1); hold off
h1.FaceAlpha = 0.5;

%% 悇掕偟偨彴柺傪丄XZ暯柺偵暯峴偵偡傞\帵
angle = -1*atan(model1.Normal(3)/model1.Normal(2))
A = [1,           0,          0, 0;...
     0,  cos(angle), sin(angle), 0; ...
     0, -sin(angle), cos(angle), 0; ...
     0,           0,          0, 1];
ptCloudScene1 = pctransform(ptCloudSceneICP, affine3d(A));

figure; pcshow(ptCloudScene1, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18)
xlabel('X (m)');ylabel('Y (m)');zlabel('Z (m)'); box on;

%%  廔椆













%% 夞揮妏傪峴楍傊偺曄姺
rotationVectorToMatrix([0, 0,  -0.2691])

%% 悇掕偟偨彴柺忋偺揰偺傒傪拪弌\帵
plane1 = select(ptCloudSceneICP,inlierIndices);
figure;pcshow(plane1, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); box on


%% 僗僠僢僥傿儞僌傪ICP偲NDT偱斾妑
% organized 3師尦揰孮僨乕僞偺撉崬傒 %%%%%%%%%%%%%%%%%%%%%%%%%
% livingRoomData: 88枃偺PointCloud僨乕僞乮奺揰偑world嵗昗抣偲丄怓傪帩偮乯
load('livingRoom.mat');
sz = get(groot,'ScreenSize');
% Stop 儃僞儞昞帵
a=true;
figure('MenuBar','none','Toolbar','none','Position',[20 sz(4)-100 100 70]);
uicontrol('Style', 'pushbutton', 'String', 'Stop',...
        'Position', [20 20 80 40], 'Callback', 'a=false;');

figure;
title('Updated world scene');
gridSize = 0.1;   % 10cm妏
mergeSize = 0.015;   % 1.5cm偺box grid filter偱丄廳暋椞堟傪僼傿儖僞儕儞僌

while (a)
ptCloudRef = livingRoomData{1};
ptCloudCurrent = livingRoomData{2};
fixed  = pcdownsample(ptCloudRef, 'gridAverage', gridSize);
moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize);
tformICP = pcregistericp(moving, fixed, 'Metric','pointToPlane','Extrapolate', true);
tformNDT = pcregisterndt(moving, fixed, 0.5);
ptCloudAlignedICP = pctransform(ptCloudCurrent, tformICP);
ptCloudAlignedNDT = pctransform(ptCloudCurrent, tformNDT);
ptCloudSceneICP = pcmerge(ptCloudRef, ptCloudAlignedICP, mergeSize);
ptCloudSceneNDT = pcmerge(ptCloudRef, ptCloudAlignedNDT, mergeSize);

accumTformICP = tformICP; 
accumTformNDT = tformNDT; 

ax = subplot(1,2,1);
hAxes = pcshow(ptCloudSceneICP, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18, 'Parent',ax);
title('ICP');
ax = subplot(1,2,2);
hAxes2 = pcshow(ptCloudSceneNDT, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', 'MarkerSize',18, 'Parent',ax);
title('NDT');
% Axes偺僾儘僷僥傿乕傪僙僢僩乮昤夋偺崅懍壔乯
hAxes.CameraViewAngleMode = 'auto';
hAxes2.CameraViewAngleMode = 'auto';
hScatter = hAxes.Children;
hScatter2 = hAxes2.Children;

for i = 3:44      %length(livingRoomData)
    if (~a)
        break;
    end
    % 師偺億僀儞僩僋儔僂僪僨乕僞傪撉崬傒
    ptCloudCurrent = livingRoomData{i};
    % 堦偮慜偺億僀儞僩僋儔僂僪僨乕僞傪儕僼傽儗儞僗偲偟偰愝掕
    fixed = moving;
    % 億僀儞僩僋儔僂僪偺僨乕僞傪娫堷偒
    moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize);
    
    % ICP 埵抲偁傢偣傪揔梡
    tformICP = pcregistericp(moving, fixed, 'Metric','pointToPlane','Extrapolate', true);
    tformNDT = pcregisterndt(moving, fixed, 0.5);

    % 嵟弶偺夋憸偺嵗昗宯傊曄姺
    accumTformICP = affine3d(tformICP.T * accumTformICP.T);
    ptCloudAlignedICP = pctransform(ptCloudCurrent, accumTformICP);
    accumTformNDT = affine3d(tformICP.T * accumTformNDT.T);
    ptCloudAlignedNDT = pctransform(ptCloudCurrent, accumTformNDT);
    
    % 揰孮偺寢崌
    ptCloudSceneICP = pcmerge(ptCloudSceneICP, ptCloudAlignedICP, mergeSize);
    ptCloudSceneNDT = pcmerge(ptCloudSceneNDT, ptCloudAlignedNDT, mergeSize);

    % Visualize the world scene.
    hScatter.XData = ptCloudSceneICP.Location(:,1);
    hScatter.YData = ptCloudSceneICP.Location(:,2);
    hScatter.ZData = ptCloudSceneICP.Location(:,3);
    hScatter.CData = ptCloudSceneICP.Color;
    hScatter2.XData = ptCloudSceneNDT.Location(:,1);
    hScatter2.YData = ptCloudSceneNDT.Location(:,2);
    hScatter2.ZData = ptCloudSceneNDT.Location(:,3);
    hScatter2.CData = ptCloudSceneNDT.Color;
    drawnow limitrate;
end  

end   %while




%% Copyright 2015 The MathWorks, Inc.