ldr = zeros(5,100);
ldr(:,1:50) = 10;
ldr(:,51:end) = 20;
pitch = linspace(-18,18,5);
pitch = repmat(pitch',1,100); 
yaw = linspace(-90,90,100);
yaw = repmat(yaw,5,1); 

X = ldr .* cosd(pitch) .* sind(yaw);
Y = ldr .* cosd(pitch) .* cosd(yaw);
Z = ldr .* sind(pitch);
pc = pointCloud(cat(3,X,Y,Z));

figure
pcshow(pc.Location,'r')
title('Unclustered Point Cloud')

distThreshold = 5;

labels = segmentLidarData(pc,distThreshold);

figure
hold on
title('Segmented Clusters')

pc1 = select(pc,find(labels == 1));
pcshow(pc1.Location,'g')

pc2 = select(pc,find(labels == 2));
pcshow(pc2.Location,'y')

veloReader = velodyneFileReader('lidarData_ConstructionRoad.pcap','HDL32E');

veloReader.CurrentTime = veloReader.StartTime + seconds(0.3);
ptCloud = readFrame(veloReader);

groundPtsIdx = segmentGroundFromLidarData(ptCloud);
ptCloudWithoutGround = select(ptCloud,~groundPtsIdx,'OutputSize','full');

distThreshold = 0.5;
[labels,numClusters] = segmentLidarData(ptCloudWithoutGround,distThreshold);

numClusters = numClusters+1;
labels(groundPtsIdx) = numClusters;

labelColorIndex = labels+1;
pcshow(ptCloud.Location,labelColorIndex)
colormap([hsv(numClusters);[0 0 0]])
title('Point Cloud Clusters')