data = load('drivingLidarPoints.mat');

roi = [-40 40 -6 9 -2 1];
in = findPointsInROI(data.ptCloud,roi);
ptCloudIn = select(data.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