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

% Read a scan of lidar dataptCloud = readFrame(veloReader)%#ok

% Specify limits of point cloud displayxlimits = [-25 45];% metersylimits = [-25 45]; zlimits = [-20 20];% Create a pcplayerlidarViewer = pcplayer(xlimits, ylimits, zlimits);% Customize player axes labelsxlabel(lidarViewer.Axes,'X (m)'）ylabel(lidarViewer.Axes,'Y (m)'）zlabel(lidarViewer.Axes,'Z (m)'）% Display the raw lidar scanview(lidarViewer, ptCloud)

% Define labels to use for segmented pointscolorLabels = [...0 0.4470 0.7410;...% Unlabeled points, specified as [R,G,B]0.4660 0.6740 0.1880;...% Ground points0.9290 0.6940 0.1250;...% Ego points0.6350 0.0780 0.1840];% Obstacle points% Define indices for each labelcolors.Unlabeled = 1; colors.Ground = 2; colors.Ego = 3; colors.Obstacle = 4;% Set the colormapcolormap(lidarViewer.Axes, colorLabels)

vehicleDims = vehicleDimensions();% Typical vehicle 4.7m by 1.8m by 1.4m

mountLocation = [...vehicleDims.Length/2 - vehicleDims.RearOverhang,...% x0,...% yvehicleDims.Height];% z

points = struct(); points.EgoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);

closePlayer = false; helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

elevationDelta = 10; points.GroundPoints = segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta', elevationDelta);% Visualize the segmented ground plane.helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

nonEgoGroundPoints = ~points.EgoPoints & ~points.GroundPoints; ptCloudSegmented = select(ptCloud, nonEgoGroundPoints,'OutputSize','full'）;

sensorLocation = [0, 0, 0];% Sensor is at the center of the coordinate systemradius = 40;% meterspoints.ObstaclePoints = findNeighborsInRadius(ptCloudSegmented,...sensorLocation, radius);% Visualize the segmented obstacleshelperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

% Rewind the |veloReader| to start from the beginning of the sequencereset(veloReader);% Stop processing after 30 secondsstopTime = veloReader.StartTime + seconds(30); isPlayerOpen = true;whilehasFrame(veloReader) && veloReader.CurrentTime < stopTime && isPlayerOpen% Grab the next lidar scanptCloud = readFrame(veloReader);% Segment points belonging to the ego vehiclepoints.EgoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);% Segment points belonging to the ground planepoints.GroundPoints = segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta', elevationDelta);% Remove points belonging to the ego vehicle and ground planenonEgoGroundPoints = ~points.EgoPoints & ~points.GroundPoints; ptCloudSegmented = select(ptCloud, nonEgoGroundPoints,'OutputSize','full'）;% Segment obstaclespoints.ObstaclePoints = findNeighborsInRadius(ptCloudSegmented, sensorLocation, radius); closePlayer = ~hasFrame(veloReader);% Update lidar displayisPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);endsnapnow

functionegoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation)%helperSegmentEgoFromLidarData segment ego vehicle points from lidar data% egoPoints = helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation)% segments points belonging to the ego vehicle of dimensions vehicleDims% from the lidar scan ptCloud. The lidar is mounted at location specified% by mountLocation in the vehicle coordinate system. ptCloud is a% pointCloud object. vehicleDimensions is a vehicleDimensions object.% mountLocation is a 3-element vector specifying XYZ location of the% lidar in the vehicle coordinate system.%% This function assumes that the lidar is mounted parallel to the ground% plane, with positive X direction pointing ahead of the vehicle,% positive Y direction pointing to the left of the vehicle in a% right-handed system.% Buffer around ego vehiclebufferZone = [0.1, 0.1, 0.1];% in meters% Define ego vehicle limits in vehicle coordinatesegoXMin = -vehicleDims.RearOverhang - bufferZone(1); egoXMax = egoXMin + vehicleDims.Length + bufferZone(1); egoYMin = -vehicleDims.Width/2 - bufferZone(2); egoYMax = egoYMin + vehicleDims.Width + bufferZone(2); egoZMin = 0 - bufferZone(3); egoZMax = egoZMin + vehicleDims.Height + bufferZone(3); egoXLimits = [egoXMin, egoXMax]; egoYLimits = [egoYMin, egoYMax]; egoZLimits = [egoZMin, egoZMax];% Transform to lidar coordinatesegoXLimits = egoXLimits - mountLocation(1); egoYLimits = egoYLimits - mountLocation(2); egoZLimits = egoZLimits - mountLocation(3);% Use logical indexing to select points inside ego vehicle cubeegoPoints = ptCloud.Location(:,:,1) > egoXLimits(1)...& ptCloud.Location(:,:,1) < egoXLimits(2)...& ptCloud.Location(:,:,2) > egoYLimits(1)...& ptCloud.Location(:,:,2) < egoYLimits(2)...& ptCloud.Location(:,:,3) > egoZLimits(1)...& ptCloud.Location(:,:,3) < egoZLimits(2);end

functionisPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer)%helperUpdateView update streaming point cloud display% isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayers)% updates the pcplayer object specified in lidarViewer with a new point% cloud ptCloud. Points specified in the struct points are colored% according to the colormap of lidarViewer using the labels specified by% the struct colors. closePlayer is a flag indicating whether to close% the lidarViewer.ifclosePlayer hide(lidarViewer); isPlayerOpen = false;return;endscanSize = size(ptCloud.Location); scanSize = scanSize(1:2);% Initialize colormapcolormapValues = ones(scanSize,'like', ptCloud.Location) * colors.Unlabeled;ifisfield(points,'GroundPoints'）colormapValues(points.GroundPoints) = colors.Ground;endifisfield(points,'EgoPoints'）colormapValues(points.EgoPoints) = colors.Ego;endifisfield(points,'ObstaclePoints'）colormapValues(points.ObstaclePoints) = colors.Obstacle;end% Update viewview(lidarViewer, ptCloud.Location, colormapValues)% Check if player is openisPlayerOpen = isOpen(lidarViewer);end