激光雷达的检测仿真代码详解（附Matlab源码详解）

100 阅读 0 评论 66 点赞

我是靠谱客的博主忧虑中心，这篇文章主要介绍激光雷达的检测仿真代码详解（附Matlab源码详解），现在分享给大家，希望可以做个参考。

一、创建velodyneFileReader

本示例中的激光雷达数据来自安装在车辆上的激光雷达Velodyne HDL32E。创建velodyneFileReader对象以读取录制的PCAP文件。

复制代码

1
2
3
fileName = 'lidarData_ConstructionRoad.pcap';   
deviceModel = 'HDL32e';   
veloReader = velodyneFileReader(fileName,deviceModel);

二、读取激光雷达扫描数据

激光雷达每次扫描的数据都存储为三维点云。为了能够高效地处理这些数据，需要使用快速索引和搜索，通过K-d tree数据结构处理数据。

veloReader为激光雷达每个扫描构建一个有组织的点云。点云的位置属性是M×N×3的矩阵，包含点的X，Y，Z坐标（m）。

复制代码

1
2
ptCloud=readFrame(veloReader);

三、设置点云显示

pcplayer用于点云数据的可视化，通过配置pcplayer设置车辆周围区域。

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
%设置点云显示的区域
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)

输出结果如图所示

分割属于地平面、主车和附近障碍物的点；设置用于标记这些点的颜色映射。

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
%定义用于分段点的标签
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)

输出结果如下图所示

四、分割主车

激光雷达安装在车辆顶部，点云可能包含属于车辆本身的点，例如车顶或发动机罩上的点。了解车辆的尺寸，可以分割出离车辆最近的点。

创建车辆尺寸对象以存储车辆尺寸，典型车辆尺寸为4.7m×1.8m×1.4m。

复制代码

1
vehicleDims=vehicleDimensions ();

在车辆坐标系中指定激光雷达的安装位置。在该示例中，车辆坐标系原点位于后轴中心，正X方向指向前方，正Y方向指向左侧，正Z方向指向上方，激光雷达安装在车辆的顶部中心，与地面平行。

复制代码

1
2
3
4
5
mountLocation= [...
    vehicleDims.Length/2-vehicleDims.RearOverhang,...%设置X方向
    0,...                                            %设置Y方向
    vehicleDims.Height];                             %设置Z方向

使用helper函数helperSegmentEgoFromLidarData分割主车，该函数分割主车定义的长方体内的所有点；将分段点存储在结构points中。

复制代码

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

使用helperUpdateView函数对主车分段点云可视化

复制代码

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

helperSegmentEgoFromLidarData函数程序如下：

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
%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函数程序如下

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
%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

输出结果如下所示

五、分割地平面和周围的障碍物

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

复制代码

1
2
3
4
5
elevationDelta = 10;   
points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta', elevationDelta); 
%分割地平面的可视化
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);

输出结果如图所示

使用点云上的选择功能删除属于主车和地平面的点；将“Outputsize”指定为“full”，以保留点云的组织性质。

复制代码

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

通过寻找距离主车一定半径内所有不属于地平面和主车的点来分割附近的障碍物。这个半径可以根据激光雷达的范围和感兴趣的区域来确定。

复制代码

1
2
3
4
5
6
sensorLocation=[0,0,0];%传感器位于坐标系的原点
radius=40;
points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius);
%分割障碍物的可视化
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);

输出结果如下

六、激光雷达数据处理

从激光雷达记录的数据序列中处理30s。

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
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

点击“Run”按钮，可以看到激光雷达点云30s点云的变化，如下图所示

完整代码如下图所示

复制代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     一
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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     一
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