ld = load('drivingLidarPoints.mat');

groundPtsIdx = segmentGroundFromLidarData(ld.ptCloud);

groundPtCloud = select(ld.ptCloud,groundPtsIdx);
figure
pcshow(groundPtCloud)

nonGroundPtCloud = select(ld.ptCloud,~groundPtsIdx,'OutputSize','full');
distThreshold = 0.5;   
[labels,numClusters] = segmentLidarData(nonGroundPtCloud,distThreshold);  

figure
colormap(hsv(numClusters))
pcshow(nonGroundPtCloud.Location,labels)
title('Point Cloud Clusters')

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

xlimits = [-40 40];
ylimits = [-15 15];
zlimits = [-3 3];
player = pcplayer(xlimits,ylimits,zlimits);

xlabel(player.Axes,'X (m)')
ylabel(player.Axes,'Y (m)')
zlabel(player.Axes,'Z (m)')

colors = [0 1 0; 1 0 0]; 
greenIdx = 1;
redIdx = 2;

colormap(player.Axes,colors)
title(player.Axes,'Segmented Ground Plane of Lidar Point Cloud');
     for i = 1 : 200
         % Read current frame.
         ptCloud = velodyneFileReaderObj.readFrame(i);
         
         % Create label array.
         colorLabels = zeros(size(ptCloud.Location,1),size(ptCloud.Location,2)); 
 
         % Find the ground points.
         groundPtsIdx = segmentGroundFromLidarData(ptCloud);
 
         % Map color ground points to green.
         colorLabels(groundPtsIdx (:)) = greenIdx;
         
         % Map color nonground points to red.
         colorLabels(~groundPtsIdx (:)) = redIdx;
 
         % Plot the results.
         view(player,ptCloud.Location,colorLabels)
    end