fileName = 'test.pcap';
deviceModel = 'VLP16';
veloReader = velodyneFileReader(fileName, deviceModel);
ptCloud = readFrame(veloReader)
roi = [0 40 -20 30 -7 7];
in = findPointsInROI(ptCloud,roi);
ptCloudIn = select(ptCloud,in);
hcluster = figure;
panel = uipanel('Parent',hcluster,'BackgroundColor',[0 0 0]);
ax = axes('Parent',panel,'Color',[0 0 0]);
pcshow(ptCloudIn,'MarkerSize',30,'Parent',ax)
title('Input Point Cloud')
maxDistance = 0.3;
referenceVector = [0 0 1];
[~,inliers,outliers] = pcfitplane(ptCloudIn,maxDistance,referenceVector);
ptCloudWithoutGround = select(ptCloudIn,outliers,'OutputSize','full');
hSegment = figure;
panel = uipanel('Parent',hSegment,'BackgroundColor',[0 0 0]);
ax = axes('Parent',panel,'Color',[0 0 0]);
pcshowpair(ptCloudIn,ptCloudWithoutGround,'Parent',ax)
legend('Ground Region','Non-Ground Region','TextColor', [1 1 1])
title('Segmented Ground Plane')
distThreshold = 1;
[labels,numClusters] = pcsegdist(ptCloudWithoutGround,distThreshold);
labelColorIndex = labels;
hCuboid = figure;
panel = uipanel('Parent',hCuboid,'BackgroundColor',[0 0 0]);
ax = axes('Parent',panel,'Color',[0 0 0]);
pcshow(ptCloudIn.Location,labelColorIndex,'Parent',ax)
title('Fitting Bounding Boxes')
hold on
for i = 1:numClusters
idx = find(labels == i);
model = pcfitcuboid(ptCloudWithoutGround,idx);
plot(model)
end