第一步：显示激光三维点云
fileName = 'lidarData_ConstructionRoad.pcap'; deviceModel = 'HDL32e'; veloReader = velodyneFileReader(fileName,deviceModel); ptCloud = readFrame(veloReader); xlimits = [-25,45];ylimits = [-25,45];zlimits = [-20,20]; lidarViewer = pcplayer(xlimits,ylimits,zlimits); xlabel(lidarViewer.Axes,'X(m)') ylabel(lidarViewer.Axes,'Y(m)') zlabel(lidarViewer.Axes,'Z(m)') view(lidarViewer,ptCloud)
第二步：激光点云颜色映射。
为了分割属于地平面、主车辆和附近障碍物的点，需要设置颜色标签，并进行颜色映射。
colorLabels=[0,0.4470,0.7410;0.4660 0.6740 0.1880;0.929,0.694,0.125;0.635,0.078,0.1840]; colors.Unlabeled=1; colors.Ground=2; colors.Ego=3; colors.Obstacle=4; colormap(lidarViewer.Axes, colorLabels)
第三步：分割主车辆
vehicleDims=vehicleDimensions (); mountLocation= [vehicleDims.Length/2-vehicleDims.RearOverhang,... 0,vehicleDims.Height]; points=struct(); points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims, mountLocation); closePlayer=false; helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
helperSegmentEgoFromLidarData函数程序如下：
function egoPoints=helperSegmentEgoFromLidarData(ptCloud, vehicleDims,mountLocation) bufferZone= [0.1,0.1,0.1]; egoXMin=-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]; egoXLimits=egoXLimits-mountLocation(1); egoYLimits=egoYLimits-mountLocation(2); egoZLimits=egoZLimits-mountLocation(3); egoPoints=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
helperUpdateView函数程序如下：
function isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer) if closePlayer hide (lidarViewer); isPlayerOpen=false; return; end scanSize=size (ptCloud.Location); scanSize=scanSize (1:2); colormapValues=ones (scanSize,'like',ptCloud.Location) * colors.Unlabeled; if isfield(points,'GroundPoints') colormapValues (points.GroundPoints)=colors.Ground; end if isfield(points, 'EgoPoints') colormapValues (points.EgoPoints)=colors.Ego; end if isfield (points, 'ObstaclePoints') colormapValues (points.ObstaclePoints)=colors.Obstacle; end view (lidarViewer,ptCloud. Location, colormapValues) isPlayerOpen=isOpen (lidarViewer); End
第四步：分割地平面。
为了从激光雷达数据中检测障碍物，首先对地平面进行分段，从有组织的激光雷达数据中分割出属于地平面的点。
elevationDelta = 10; points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta', elevationDelta); helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
第五步：分割障碍物。
nonEgoGroundPoints=~points.EgoPoints &~points.GroundPoints; ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full'); sensorLocation=[0,0,0]; radius=40; points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius); helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
第六步：显示激光雷达数据处理结果。
从激光雷达记录的数据序列中处理20s。
reset(veloReader); stopTime=veloReader.StartTime+seconds(20); isPlayerOpen=true; while hasFrame(veloReader)&&veloReader.CurrentTime<stopTime&&isPlayerOpen ptCloud=readFrame(veloReader); points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation); points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta',elevationDelta); nonEgoGroundPoints=~points.EgoPoints&~points.GroundPoints; ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full'); points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius); closePlayer=~hasFrame(veloReader); isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer); end
