模拟实车多线激光雷达
clc
clear
close all
%% 一
fileName = 'lidarData_ConstructionRoad.pcap';
deviceModel = 'HDL32e';
veloReader = velodyneFileReader(fileName,deviceModel);
%% 二
ptCloud=readFrame(veloReader);
%% 三
%设置点云显示的区域
xlimits = [-25,45];
ylimits = [-25,45];
zlimits = [-20,20];
%创建pcplayer
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.9290 0.6940 0.1250;...%主车点
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,...%设置X方向
0,... %设置Y方向
vehicleDims.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);
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);
%% 六
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
snapnow
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 下列函数属于第四部分
%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