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

    一、创建velodyneFileReader

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

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

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

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

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

    ptCloud=readFrame(veloReader);

    三、设置点云显示

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

    1. %设置点云显示的区域
    2. xlimits = [-25,45];
    3. ylimits = [-25,45];
    4. zlimits = [-20,20]; 
    5. %创建pcplayer
    6. lidarViewer = pcplayer(xlimits,ylimits,zlimits);  
    7. %定义坐标轴标签
    8. xlabel(lidarViewer.Axes,'X(m)')   
    9. ylabel(lidarViewer.Axes,'Y(m)')   
    10. zlabel(lidarViewer.Axes,'Z(m)') 
    11. %显示激光雷达点云
    12. view(lidarViewer,ptCloud)

    输出结果如图所示

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

    1. %定义用于分段点的标签
    2. colorLabels=[...
    3.     0      0.4470 0.7410;...%未标记点
    4.     0.4660 0.6740 0.1880;...%地平面点
    5.     0.9290 0.6940 0.1250;...%主车点
    6.     0.635,0.078,0.1840];%障碍点
    7. %为每个标签定义索引
    8. colors.Unlabeled=1;
    9. colors.Ground=2;
    10. colors.Ego=3;
    11. colors.Obstacle=4;
    12. %设置颜色映射
    13. colormap(lidarViewer.Axes, colorLabels)

    输出结果如下图所示

     

     四、分割主车

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

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

    vehicleDims=vehicleDimensions ();

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

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

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

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

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

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

    helperSegmentEgoFromLidarData函数程序如下：

    1. %helperSegmentEgoFromLidarData函数
    2. function egoPoints=helperSegmentEgoFromLidarData(ptCloud, vehicleDims,mountLocation)
    3. bufferZone= [0.1,0.1,0.1];
    4. %在车辆坐标系中定义主车限值
    5. egoXMin=-vehicleDims.RearOverhang-bufferZone (1);
    6. egoXMax=egoXMin+vehicleDims.Length+bufferZone (1);
    7. egoYMin=-vehicleDims.Width/2-bufferZone (2);
    8. egoYMax=egoYMin+vehicleDims.Width+bufferZone (2);
    9. egoZMin=0-bufferZone (3);
    10. egoZMax=egoZMin+vehicleDims. Height+bufferZone (3);
    11. egoXLimits= [egoXMin, egoXMax];
    12. egoYLimits= [egoYMin, egoYMax];
    13. egoZLimits= [egoZMin, egoZMax];
    14. %转换为激光雷达坐标系
    15. egoXLimits=egoXLimits-mountLocation(1);
    16. egoYLimits=egoYLimits-mountLocation(2);
    17. egoZLimits=egoZLimits-mountLocation(3);
    18. %使用逻辑索引选择主车立方体内的点
    19. egoPoints=ptCloud.Location(:,:,1)>egoXLimits(1)...
    20.    & ptCloud. Location(:,:,1) <egoXLimits(2)...
    21.    & ptCloud. Location(:,:,2) >egoYLimits(1)...
    22.    & ptCloud. Location(:,:,2) <egoYLimits(2)...
    23.    & ptCloud. Location(:,:,3) >egoZLimits(1)...
    24.    & ptCloud. Location(:,:,3) <egoZLimits(2);
    25. end

    helperUpdateView函数程序如下

    1. %helperUpdateView函数程序
    2. function isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer)
    3. if closePlayer
    4.      hide (lidarViewer);
    5.      isPlayerOpen=false;
    6.      return;
    7. end
    8. scanSize=size (ptCloud.Location);
    9. scanSize=scanSize (1:2);
    10. %初始化颜色映射
    11. colormapValues=ones (scanSize,'like',ptCloud.Location) * colors.Unlabeled;
    12. if isfield(points,'GroundPoints') 
    13.     colormapValues (points.GroundPoints)=colors.Ground;
    14. end
    15. if isfield(points, 'EgoPoints')
    16.     colormapValues (points.EgoPoints)=colors.Ego;
    17. end
    18. if isfield (points, 'ObstaclePoints')
    19.     colormapValues (points.ObstaclePoints)=colors.Obstacle;   
    20. end
    21. %更新视图
    22. view (lidarViewer,ptCloud. Location, colormapValues)
    23. %检查播放器是否打开
    24. isPlayerOpen=isOpen (lidarViewer);
    25. end

    输出结果如下所示

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

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

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

     输出结果如图所示

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

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

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

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

    输出结果如下

    六、激光雷达数据处理

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

    1. reset(veloReader);
    2. stopTime=veloReader.StartTime+seconds(20);
    3. isPlayerOpen=true;
    4. while hasFrame(veloReader)&&veloReader.CurrentTime<stopTime&&isPlayerOpen
    5. %获取下一次激光雷达扫描
    6.     ptCloud=readFrame(veloReader);
    7. %属于主车的分段点
    8.     points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation);
    9. %属于地平面的分段点
    10.     points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta',elevationDelta);
    11. %删除属于主车和地平面的点
    12.     nonEgoGroundPoints=~points.EgoPoints&~points.GroundPoints;
    13.     ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full');
    14. %分段障碍物
    15.     points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius);
    16.     closePlayer=~hasFrame(veloReader);
    17. %更新激光雷达显示
    18.     isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
    19. end
    20. snapnow

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

     完整代码如下图所示

    1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     一
    2. fileName = 'lidarData_ConstructionRoad.pcap';   
    3. deviceModel = 'HDL32e';   
    4. veloReader = velodyneFileReader(fileName,deviceModel); 
    5. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     二
    6. ptCloud=readFrame(veloReader);
    7. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     三
    8. %设置点云显示的区域
    9. xlimits = [-25,45];
    10. ylimits = [-25,45];
    11. zlimits = [-20,20]; 
    12. %创建pcplayer
    13. lidarViewer = pcplayer(xlimits,ylimits,zlimits);  
    14. %定义坐标轴标签
    15. xlabel(lidarViewer.Axes,'X(m)')   
    16. ylabel(lidarViewer.Axes,'Y(m)')   
    17. zlabel(lidarViewer.Axes,'Z(m)') 
    18. %显示激光雷达点云
    19. view(lidarViewer,ptCloud)  
    20.  
    21. %定义用于分段点的标签
    22. colorLabels=[...
    23.     0      0.4470 0.7410;...%未标记点
    24.     0.4660 0.6740 0.1880;...%地平面点
    25.     0.9290 0.6940 0.1250;...%主车点
    26.     0.635,0.078,0.1840];%障碍点
    27. %为每个标签定义索引
    28. colors.Unlabeled=1;
    29. colors.Ground=2;
    30. colors.Ego=3;
    31. colors.Obstacle=4;
    32. %设置颜色映射
    33. colormap(lidarViewer.Axes, colorLabels)
    34. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    四
    35. %分割主车
    36. vehicleDims=vehicleDimensions ();
    37. mountLocation= [...
    38.     vehicleDims.Length/2-vehicleDims.RearOverhang,...%设置X方向
    39.     0,...                                            %设置Y方向
    40.     vehicleDims.Height];                             %设置Z方向
    41. points=struct();
    42. points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims, mountLocation);
    43. closePlayer=false;
    44. helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
    45. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%      五
    46. %%%%分割地平面和周围障碍物
    47. elevationDelta = 10;   
    48. points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta', elevationDelta);   
    49. helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer); 
    50.  
    51. nonEgoGroundPoints=~points.EgoPoints &~points.GroundPoints;
    52. ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full');
    53.  
    54. sensorLocation=[0,0,0];%传感器位于坐标系的原点
    55. radius=40;
    56. points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius);
    57. %分割障碍物的可视化
    58. helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
    59. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%       六
    60. reset(veloReader);
    61. stopTime=veloReader.StartTime+seconds(20);
    62. isPlayerOpen=true;
    63. while hasFrame(veloReader)&&veloReader.CurrentTime<stopTime&&isPlayerOpen
    64. %获取下一次激光雷达扫描
    65.     ptCloud=readFrame(veloReader);
    66. %属于主车的分段点
    67.     points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation);
    68. %属于地平面的分段点
    69.     points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta',elevationDelta);
    70. %删除属于主车和地平面的点
    71.     nonEgoGroundPoints=~points.EgoPoints&~points.GroundPoints;
    72.     ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full');
    73. %分段障碍物
    74.     points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius);
    75.     closePlayer=~hasFrame(veloReader);
    76. %更新激光雷达显示
    77.     isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
    78. end
    79. snapnow
    80.  
    81. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    82. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    83. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%下列函数属于第四部分
    84.  
    85.  
    86. %helperSegmentEgoFromLidarData函数
    87. function egoPoints=helperSegmentEgoFromLidarData(ptCloud, vehicleDims,mountLocation)
    88. bufferZone= [0.1,0.1,0.1];
    89. %在车辆坐标系中定义主车限值
    90. egoXMin=-vehicleDims.RearOverhang-bufferZone (1);
    91. egoXMax=egoXMin+vehicleDims.Length+bufferZone (1);
    92. egoYMin=-vehicleDims.Width/2-bufferZone (2);
    93. egoYMax=egoYMin+vehicleDims.Width+bufferZone (2);
    94. egoZMin=0-bufferZone (3);
    95. egoZMax=egoZMin+vehicleDims. Height+bufferZone (3);
    96. egoXLimits= [egoXMin, egoXMax];
    97. egoYLimits= [egoYMin, egoYMax];
    98. egoZLimits= [egoZMin, egoZMax];
    99. %转换为激光雷达坐标系
    100. egoXLimits=egoXLimits-mountLocation(1);
    101. egoYLimits=egoYLimits-mountLocation(2);
    102. egoZLimits=egoZLimits-mountLocation(3);
    103. %使用逻辑索引选择主车立方体内的点
    104. egoPoints=ptCloud.Location(:,:,1)>egoXLimits(1)...
    105.    & ptCloud. Location(:,:,1) <egoXLimits(2)...
    106.    & ptCloud. Location(:,:,2) >egoYLimits(1)...
    107.    & ptCloud. Location(:,:,2) <egoYLimits(2)...
    108.    & ptCloud. Location(:,:,3) >egoZLimits(1)...
    109.    & ptCloud. Location(:,:,3) <egoZLimits(2);
    110. end
    111.  
    112. %helperUpdateView函数程序
    113. function isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer)
    114. if closePlayer
    115.      hide (lidarViewer);
    116.      isPlayerOpen=false;
    117.      return;
    118. end
    119. scanSize=size (ptCloud.Location);
    120. scanSize=scanSize (1:2);
    121. %初始化颜色映射
    122. colormapValues=ones (scanSize,'like',ptCloud.Location) * colors.Unlabeled;
    123. if isfield(points,'GroundPoints') 
    124.     colormapValues (points.GroundPoints)=colors.Ground;
    125. end
    126. if isfield(points, 'EgoPoints')
    127.     colormapValues (points.EgoPoints)=colors.Ego;
    128. end
    129. if isfield (points, 'ObstaclePoints')
    130.     colormapValues (points.ObstaclePoints)=colors.Obstacle;   
    131. end
    132. %更新视图
    133. view (lidarViewer,ptCloud. Location, colormapValues)
    134. %检查播放器是否打开
    135. isPlayerOpen=isOpen (lidarViewer);
    136. end
    137.  
    138.  
    139.  
    140.  
    141.  
    142.  
    143.  
    144.  
    145.  
    146.  
    147.  
    148.  
    149.

    码字不易，点个赞再走吧QAQ

    转载请附加转载链接说明！！！！！

  • 相关阅读:
    含文档+PPT+源码等]精品基于SSM的公交线路查询系统[包运行成功]Java毕业设计SSM项目源码论文
    gitlab登录出现的Invalid login or password问题
    【独家专访】“数网”同防筑牢屏障——新型电力系统网络安全保障体系需加快调整
    计算机网络中点到点与端到端协议的区别
    【学生管理系统】权限管理之角色管理
    uniapp登录逻辑
    机器人使用记录
    windows10+cpu+pycharm跑yolov5
    MySQL（十）事务隐性提交
    云端golang开发，无需本地配置，能上网就能开发和运行
  • 原文地址：https://blog.csdn.net/m0_64346597/article/details/126441020