使用激光雷达接地平面和障碍物检测

这个示例使用:

开放脚本

本示例显示了如何通过分割地面并查找附近的障碍物来处理安装在车辆上的传感器的三维激光雷达数据。这有助于车辆导航的驾驶路径规划。本示例还显示了如何可视化流式激光雷达数据。

创建一个Velodyne文件读取器

使用安装在车辆上的Velodyne HDL32E传感器记录在该示例中使用的LIDAR数据。设置A.velodyneFileReader（电脑视觉工具箱）对象读取记录的PCAP文件。

文件名=“lidarData_ConstructionRoad.pcap”；deviceModel =“HDL32E”；veloReader = velodyneFileReader(fileName, deviceModel);

阅读LIDAR扫描

LIDAR数据的每次扫描都存储为3-D点云。使用快速索引和搜索有效地处理此数据，并且搜索是传感器处理管道性能的关键。使用该效率pointCloud（电脑视觉工具箱）对象，内部使用K-D树数据结构组织数据。

这Veloreader.构建一个有组织的pointCloud用于每次激光雷达扫描。这位置财产pointCloud是一个M-x-N-x-3矩阵，包含以米为单位的点的XYZ坐标。点强度存储在强度。

读取激光雷达扫描数据ptCloud = readFrame (veloReader)% #好< NOPTS >

ptCloud = pointCloud with properties:位置:[32×1083×3 single] Color: [] Normal: [] Intensity: [32×1083 single] Count: 34656 XLimits: [-80.0444 87.1780] YLimits: [-85.6287 92.8721] ZLimits: [-21.6060 14.3558]

设置流点云显示

这PCPlayer.可用于可视化流点云数据。通过配置设置车辆周围的区域以显示PCPlayer.（电脑视觉工具箱）。

%指定点云显示的限制Xlimits = [-25 45];％仪表ylimits = [-25 45];zlimits = [-20 20];%创建一个pcplayerlidarViewer=pcplayer（xlimits、ylimits、zlimits）；%自定义播放器轴标签xlabel（lidarViewer.Axes，‘X（m）’) ylabel (lidarViewer。轴,‘Y（m）’）zlabel（lidarviewer.axes，'z（m）'）%显示原始激光雷达扫描视图(lidarViewer ptCloud)

在这个例子中，我们将是属于地面平面的分段点，自我车辆和附近的障碍物。设置ColorMap以标记这些点。

％定义用于分段点的标签colorLabels = [......0 0.4470 0.7410;......％未标记的点，指定为[r，g，b]0.4660 0.6740 0.1880;......%地面点0.9290 0.6940 0.1250;......％自我要点0.6350 0.0780 0.1840];％障碍点%为每个标签定义索引颜色。无标号= 1;颜色。地面= 2;颜色。自我= 3;颜色。障碍= 4;%设置颜色映射colormap (lidarViewer。轴,colorLabels)

分割Ego车辆

激光雷达安装在车辆的顶部，并且点云可以包含属于车辆本身的点，例如在屋顶或罩上。了解车辆的尺寸，我们可以分割最接近车辆的点。

创建一个vehicleDimensions用于存储车辆尺寸的物体。

vehicleDims = vehicleDimensions ();%典型车辆4.7米× 1.8米× 1.4米

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

安装位置=[......vevicledims.length / 2  -  vehicledims.rearoverhang，......% x0,......y %vevicledims.height];% z

使用辅助功能分割ego车辆helperSegmentEgoFromLidarData。该功能段在自助式车辆定义的长方体内的所有点。将分段点存储在结构中点。

points=struct（）；points.EgoPoints=helpersegometfromlidardata（ptCloud、vehicleDims、mountLocation）；

可视化点云与分段自我车辆。使用Helperupdateview.帮手功能。

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

分段地平面和附近障碍物

为了从激光雷达数据中识别障碍物，首先使用segmentgroundfromlidardata.（电脑视觉工具箱）函数来完成这一点。该功能段属于有组织的LIDAR数据的地面。

elevationDelta = 10;点。地表=段段FromLidardata（PTCloud，“ElevationAngleDelta”, elevationDelta);％可视化分段接地平面。helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

通过使用拆下属于自我车辆的点和地面平面选择（电脑视觉工具箱）在点云上的功能。指定'输出'作为“满”保留点云的有组织性质。

nonEgoGroundPoints = ~点。EgoPoints & ~ points.GroundPoints;ptcloudsegments = select(ptCloud, nonEgoGroundPoints，'输出'那“满”)；

接下来，通过寻找不在自我车辆的某些半径内未在某个半径内部的一部分的所有点来段落附近的障碍物。可以基于LIDAR和感兴趣区域的范围来确定该半径以进行进一步处理。

传感器位置= [0,0,0];％传感器位于坐标系的中心半径= 40;％仪表points.obstaclePoints = findneighborsinradius（ptcloudseded，......传感器位置，半径）;％可视化分段障碍物helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

过程激光雷达序列

现在，单次激光雷达扫描的点云处理管道已经布置好，将所有这些放在一起，以处理记录数据序列中的30秒。以下代码缩短，因为关键参数已在前面的步骤中定义。此处使用参数，无需进一步解释。

%倒带|veloReader|，从序列的开头开始重置(veloReader);30秒后停止处理Stoptime = Veloreader.startTime +秒（30）;Isplayeropen = true;而hasFrame（veloReader）和&veloReader.CurrentTime<停止时间和显示时间％抓住下一个LIDAR扫描ptCloud = readFrame (veloReader);%属于自我车辆的部分点数points.EgoPoints=helperSegmentEgoFromLidarData（云端、车辆IMS、安装位置）；属于地面平面的％分段点点。地表=段段FromLidardata（PTCloud，“ElevationAngleDelta”, elevationDelta);%移除属于ego车辆和地平面的点nonEgoGroundPoints = ~点。EgoPoints & ~ points.GroundPoints;ptcloudsegments = select(ptCloud, nonEgoGroundPoints，'输出'那“满”)；％段障碍物points.obstaclePoints = findneighborsinradius（ptcloudseded，sensorLocation，半径）;特写镜头=〜hasfame（Veloreader）;％更新激光雷达显示isPlayerOpen=HelperUpdate视图（lidarViewer、ptCloud、点、颜色、closePlayer）；结束Snapnow.

金宝app支持功能

helperSegmentEgoFromLidarData给定车辆的尺寸和安装位置，属于自我车辆的部分点。

作用Egopoints = alpersegmentegofromlidardata（ptcloud，车辆lexims，mountlocation）%helperSegmentEgoFromLidarData段自我车辆点从激光雷达数据% egpoints = helperSegmentEgoFromLidarData(ptCloud, vehicledimms,mountLocation)%分段分属于自我车辆的尺寸车辆的dimms从LIDAR扫描Ptcloud的％。LIDAR安装在指定的位置通过MountLocation在车辆坐标系中的％。Ptcloud是A.% pointCloud对象。vehicleDimensions是一个vehicleDimensions对象。%mountLocation是一个3元素向量，指定车辆坐标系中的％LIDAR。％这个函数假设激光雷达与地面平行安装％平面，带有正x方向指向车辆前方，指向车辆左侧的％正y方向%右撇子体系。%缓冲自我车辆周围bufferZone = [0.1, 0.1, 0.1];％在米在车辆坐标中定义自我车辆限制egoXMin = -vehicleDims。RearOverhang bufferZone (1);egoXMax = egoXMin + vehicledimms。长度+ bufferZone (1);egoYMin = -vehicleDims。宽/ 2 - bufferZone (2);egoYMax = egoYMin + vehicles。宽度+ bufferZone (2);egoZMin = 0 - bufferZone(3);egoZMax = egoZMin + vehicles。身高+ bufferZone (3);egoXLimits = [egoXMin, egoXMax];egoYLimits = [egoYMin, egoYMax]; egoZLimits = [egoZMin, egoZMax];转换到激光雷达坐标EGOXLIMITS = EGOXLIMITS  -  MountLocation（1）;egoylimits = egoylimits  -  mountlocation（2）;egozlimits = egozlimits  -  MountLocation（3）;%使用逻辑索引选择ego vehicle cube内的点egoPoints=ptCloud.Location（：，：，1）>egoXLimits（1）......& ptCloud.Location(:，:，1) < egoXLimits(2)......& ptCloud.Location(:，: 2) > egoYLimits(1)......＆ptcloud.location（：，:,2）......& ptCloud.Location(:，:，3) > egoZLimits(1)......& ptCloud.Location(:，:，3) < egoZLimits(2);结束

Helperupdateview.使用最新的点云和关联的颜色标签更新流式点云显示。

作用Isplayeropen = HelperupdateView（LidarViewer，Ptcloud，点，颜色，特写玩家）%helperUpdateView更新流点云显示％Isplayeropen = HelperupdateView（LidarViewer，Ptcloud，点，颜色，特写玩家）％更新LidarViewer中指定的PCPlayer对象，其中包含一个新点％云ptcloud。结构点中指定的点是有色的%根据lidarViewer的颜色图使用指定的标签%结构的颜色。closePlayer是一个指示是否关闭的标志％lidarviewer。如果特写玩家隐藏（Lidarviewer）;isplayeropen = false;返回；结束scanSize =大小(ptCloud.Location);scanSize = scanSize (1:2);%初始化颜色映射colormapValues = 1 (scanSize,“喜欢”* colors.Unlabeled;如果isfield（积分，'地面')colormapValues（points.GroundPoints）=colors.Ground；结束如果isfield（积分，'Egopoints'）ColorMapValues（points.egopoints）= colors.ego;结束如果isfield（积分，“障碍点”) colormapValues(points.ObstaclePoints) = colors.Obstacle;结束%更新视图视图(lidarViewer ptCloud。位置,colormapValues)%检查玩家是否打开Isplayeropen = Isopen（Lidarviewer）;结束