基于特征的激光雷达地图生成

打开实时脚本

此示例演示如何处理安装在车辆上的传感器的三维激光雷达数据，以逐步构建地图。此类地图适用于自动驾驶工作流，如定位和导航。这些地图可用于在几厘米内定位车辆。

概述

有不同的方法来注册点云。典型的方法是使用完整的点云进行注册。从激光雷达数据构建地图（自动驾驶工具箱）示例使用此方法构建地图。此示例使用从点云提取的独特特征构建地图。

在本例中，您将学习如何：

加载并可视化记录的驾驶数据。
使用激光雷达扫描构建地图。

负载记录的驾驶数据

这个数据本例中使用的数据表示大约100秒的激光雷达、GPS和IMU数据。数据保存在单独的MAT文件中，如下所示：时间表物体。从下载激光雷达数据MAT文件存储库并将其加载到MATLAB®工作空间中。

注:此下载可能需要几分钟时间。

baseDownloadURL=['https://github.com/mathworks/udacity-self-driving-data'...'-子集/原始/主/驱动器段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段段]; dataFolder=fullfile（tempdir，“驾驶室段11、18、16”，filesep）；options=weboptions(“超时”，Inf）；lidarFileName=dataFolder+“lidarPointClouds.mat”;%检查文件夹和数据文件是否已存在folderExists=存在（dataFolder，“dir”)；matfilesExist=exist（lidarFileName，“文件”);%如果不存在，请创建新文件夹如果~folderExists mkdir（dataFolder）；终止%下载激光雷达数据（如果不存在）如果~matfiles disp('正在下载lidarPointClouds.mat（613 MB）…'); websave（lidarFileName，baseDownloadURL）+“lidarPointClouds.mat”，选项）；终止

加载从Velodyne®HDL32E激光雷达传感器保存的点云数据。使用点云对象。此对象使用Kd树数据结构在内部组织数据，以加快搜索速度。与每个激光雷达扫描相关的时间戳记录在时间变量时间表对象

%从MAT文件加载激光雷达数据数据=加载（lidarFileName）；lidarPointClouds=data.lidarPointClouds；%显示前几行激光雷达数据头部（激光点云）

ans=8×1时刻表时间点云

可视化驾驶数据

要了解场景包含的内容，请使用pcplayer对象

%为玩家指定限制xlimits=[-45]；%仪表ylimits=[-45]；zlimits=[-1020]；%创建一个pcplayer以可视化来自激光雷达传感器的流点云lidarPlayer=pcplayer（xlimits、ylimits、zlimits）；%自定义播放器轴标签xlabel（lidarPlayer.Axes，‘X（m）’); ylabel（lidarPlayer.Axes，‘Y（m）’)；zlabel（lidarPlayer.Axes，‘Z（m）’)；标题（lidarPlayer.Axes），“激光雷达传感器数据”);%循环并可视化数据对于l=1：高度（Lidarpoint云）%提取点云ptCloud=lidarPointClouds.PointCloud（l）；%更新激光雷达显示器视图（lidarPlayer、ptCloud）；终止

使用记录的激光雷达数据建立地图

激光雷达可以用来制作厘米级的精确地图，以后可以用于车内定位。构建这样一张地图的典型方法是对齐从移动车辆获得的连续激光雷达扫描，并将它们组合成单个大型点云。本示例的其余部分将探讨这种方法。

预处理

取两个点云，对应于附近的激光雷达扫描。每十次扫描用于加快处理速度，并在扫描之间积累足够的运动。

rng(“默认”); skipFrames=10；frameNum=100；固定=lidarPointClouds.PointCloud（frameNum）；moving=lidarPointClouds.PointCloud（frameNum+skipFrames）；

处理点云以保留点云中与众不同的结构。这些步骤使用helperProcessPointCloud功能：

检测并移除接地层。
检测并移除车辆。

这些步骤在中有更详细的描述用激光雷达探测地面和障碍物（自动驾驶工具箱）实例

fixedProcessed=helperProcessPointCloud（固定）；movingProcessed=helperProcessPointCloud（移动）；

在俯视图中显示初始点云和处理过的点云。洋红色点对应于地平面和车辆。

hFigFixed=图；轴固定=轴(“家长”，hFigFixed，“颜色”，[0,0,0]）；pcshowpair（固定，固定处理，“家长”，axFixed）；标题（axFixed，“分段地面和Ego车辆”); 视图（axFixed，2）；

对点云进行下采样以提高配准精度和算法速度。

gridStep=0.5；fixedDownsampled=pcdownsample（fixedProcessed，“网格平均”，gridStep）；movingDownsampled=pcdownsample（movingProcessed，“网格平均”，gridStep）；

基于特征的配准

使用基于特征的配准对齐和组合连续激光雷达扫描，如下所示：

使用特征提取作用
通过使用pcmatchfeatures作用
使用估计几何变换3D作用
使用估计的变换将点云与参考点云对齐并合并。这是使用pcalign作用

%为每个点云提取FPFH特征邻居=40；[fixedFeature，fixedValidInds]=提取的FfHfeatures（fixedDownsampled，...“纽曼尼斯堡”，邻居）；[movingFeature，movingValidInds]=提取FPFHfeatures（下采样移动，...“纽曼尼斯堡”，邻居）；fixedValidPts=select（fixedDownsampled，fixedValidInds）；movingValidPts=select（movingDownsampled，movingValidInds）；%确定点对应关系方法=“详尽的”; 阈值=1；比率=0.96；indexPairs=pcmatchfeatures（移动特征、固定特征、移动有效数据、，...固定包，“方法”方法“匹配阈值”门槛...“不合格率”，比率）；matchedFixedPts=select（fixedValidPts，indexPairs（：，2））；matchedMovingPts=选择（移动有效数据，索引（：，1））；%运动点云相对于参考点的刚性变换估计%点云maxDist=2；maxNumTrails=3000；tform=estimateGeometricTransform3D（匹配移动点位置，...匹配的固定点位置，“刚性”,“最大距离”，maxDist，...“最大值”，maxNumTrails）；%将移动点云变换为参考点云，以%在注册前后可视化对齐movingReg=pctransform（movingProcessed，tform）；%移动和固定点云由洋红色和绿色表示hFigAlign=图；axAlign1=子批次（1,2,1,，“颜色”, [0, 0, 0],“家长”，hFigAlign）；pcshowpair（移动处理，固定处理，“家长”，axAlign1）；标题（axAlign1，“注册前”); 视图（axAlign1，2）；AXALIGIN2=子批次（1,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,，“颜色”, [0, 0, 0],“家长”，hFigAlign）；pcshowpair（移动注册、固定处理、，“家长”，axAlign2）；标题（axAlign2，“注册后”); 视图（axAlign2，2）；

%对齐并合并点云alignGridStep=1；ptCloudAccum=pcalign（[fixedProcessed，movingProcessed]，...[rigid3d，tform]，alignGridStep）；%可视化累积的点云hFigAccum=图形；axAccum=轴(“家长”，hFigAccum，“颜色”，[0,0,0]）；pcshow（ptCloudAccum，“家长”，axacum）；头衔（axacum，“累积点云”)；视图（axAccum，2）；

地图生成

在整个记录数据序列上循环应用预处理和基于特征的注册步骤。结果是车辆所经过环境的地图。

rng(“默认”); numFrames=高度（lidarPointClouds）；accumTform=rigid3d；pointCloudMap=点云（零（0,0,3））；%为玩家指定限制xlimits=[-200 250]；%仪表ylimits=[-150500]；zlimits=[-100100]；%创建一个pcplayer来可视化地图mapPlayer=pcplayer（xlimits、ylimits、zlimits）；title（mapPlayer.Axes、，“累积地图”); mapPlayer.Axes.View=[0,90]；%在整个数据上循环以生成地图对于n=1:skipFrames:numFrames-skipFrames%获取第n个点云ptCloud=lidarPointClouds.PointCloud（n）；%分段接地并拆下车辆ptProcessed=helperProcessPointCloud（ptCloud）；%对点云进行向下采样，以获得运行速度ptDownsampled=pcdownsample（ptProcessed，“网格平均”，gridStep）；%从点云中提取特征[ptFeature，ptValidInds]=提取FPFHfeatures（ptDownsampled，...“纽曼尼斯堡”，邻居）；ptValidPts=选择（ptDownsampled，ptValidInds）；如果n==1移动=ptValidPts；movingFeature=ptFeature；pointCloudMap=ptValidPts；其他的固定=移动；固定特征=移动特征；移动=ptValidPts；movingFeature=ptFeature；%匹配特征以查找对应项indexPairs=pcmatchfeatures（移动特征、固定特征、移动特征、，...固定的“方法”方法“匹配阈值”门槛...“不合格率”，比率）；matchedFixedPts=选择（固定，索引（：，2））；matchedMovingPts=选择（移动，索引（：，1））；%注册移动点云w.r.t参考点云tform=估计几何变换器3D（匹配移动点位置，...匹配的固定点位置，“刚性”,“最大距离”，maxDist，...“最大值”，maxNumTrails）；%计算到原始参考系的累积变换accumTform=rigid3d（tform.T*accumTform.T）；%将移动点云与累积地图对齐并合并pointCloudMap=pcalign（[pointCloudMap，移动]，...[rigid3d，accumTform]，alignGridStep）；终止%更新地图显示视图（mapPlayer、pointCloudMap）；终止