matlab利用已有激光雷达数据寻找地平面和车辆周围的障碍物仿真实验

第一步：显示激光三维点云

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)   

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
 

第二步：激光点云颜色映射。

为了分割属于地平面、主车辆和附近障碍物的点，需要设置颜色标签，并进行颜色映射。

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)

  

 

  
1
  
2
  
3
  
4
  
5
  
6
 

第三步：分割主车辆

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);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
 

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

  

 

  
 
 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
 

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

  

 

  
 
 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
 

第四步：分割地平面。

为了从激光雷达数据中检测障碍物，首先对地平面进行分段，从有组织的激光雷达数据中分割出属于地平面的点。

elevationDelta = 10;   
points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta', elevationDelta);   
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer); 

  

 

  
1
  
2
  
3
 

第五步：分割障碍物。

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);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
 

第六步：显示激光雷达数据处理结果。

从激光雷达记录的数据序列中处理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

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
 

文章来源: luckystar.blog.csdn.net，作者：爱打瞌睡的CV君，版权归原作者所有，如需转载，请联系作者。

原文链接：luckystar.blog.csdn.net/article/details/120120310