从LIDAR数据构建地图
该示例示出了如何从安装在车辆上的传感器处理3-D LIDAR数据，以惯性测量单元（IMU）读数的帮助逐步构建地图。这样的地图可以促进车辆导航的路径规划或可用于本地化。为了评估所生成的地图，该示例还示出了如何将车辆的轨迹与全球定位系统（GPS）记录进行比较。
概述
高清晰度(HD)地图是一种提供精确道路几何图形的地图服务，精确度可达几厘米。这种精度水平使高清地图适用于自动驾驶流程，如定位和导航。这种高清地图是由3-D激光雷达扫描生成的，结合高精度GPS和或IMU传感器，可以用于定位车辆在几厘米内。这个示例实现了构建这样一个系统所需的特性子集。
在此示例中，您将学习如何：
加载，探索和可视化录制的驾驶数据
使用LIDAR扫描构建地图
使用IMU读数改进地图
加载和探索记录的驾驶数据
在此示例中使用的数据来自这GitHub®库，并且代表大约100秒的LIDAR，GPS和IMU数据。数据以MAT文件的形式保存，每个形式包含一个时间表
注意:这个下载可能需要几分钟。
baseDownloadURL =“https://github.com/mathworks/udacity-self-driving-data-subset/raw/master/drive_segment_11_18_16/”；dataFolder = fullfile (tempdir,“drive_segment_11_18_16”，filesep）;选项= weboptions（“超时”、正);lidarFileName = dataFolder +“lidarpointclouds.mat”；imufilename = datafolder +“imuOrientations.mat”；gpsfilename = datafolder +“gpssequence.mat”；folderxists =存在（datafolder，'dir'）;matfilesexist =存在（lidarfilename，“文件”）&&存在（imufilename，“文件”）．..&&存在（gpsfilename，“文件”）;如果〜Folderexists Mkdir（DataFolder）;结尾如果〜Matfilesexist Disp（'下载lidarpointclouds.mat（613 MB）......'）Websave（Lidarfilename，BribyOrdLoadURL +“lidarpointclouds.mat”， 选项）;DISP（'下载imuorientation.mat（1.2 MB）......'）Websave（imufilename，brienialloadlowull +“imuOrientations.mat”， 选项）;DISP（'下载GPSSequence.mat（3 KB）......'）Websave（GPSFileName，BriencedlighLoadURL +“gpssequence.mat”， 选项）;结尾
首先，加载从Velodyne®HDL32ELIDAR保存的点云数据。使用的LIDAR数据的每次扫描都被存储为3-D点云使用pointcloud.时间时间表的变量。
来自Mat文件的％加载LIDAR数据data =负载(lidarFileName);lidarPointClouds = data.lidarPointClouds;％显示前几行的LIDAR数据头(lidarPointClouds)
ANS = 8×1时间表时间POINTCLOUD _____________ ________________ 23：46：10.5115 [1×1 POINTCLOUD] 23：46：10.6115 [1×1 POINTCLOUD] 23：46：10.7116 [1×1 POINTCLOUD] 23：46：10.8117 [1×1 pointcloud] 23：46：10.9118 [1×1 pointcloud] 23：46：11.0119 [1×1 pointcloud] 23：46：11.1120 [1×1 pointcloud] 23：46：11.2120 [1×1 pointcloud]
从MAT-file加载GPS数据。这纬度那经度,高度变量的时间表用于存储由车载GPS设备记录的地理坐标。
载入GPS序列从MAT-filedata = load（gpsfilename）;gpssequence = data.gpssequence;％显示前几行GPS数据头（GPSSequence）
ans = 8×3时间表时间纬度经度海拔_____________ ________ _________ ________ 23:46:11.4563 37.4 -122.11 -42.5 23:46:12.4563 37.4 -122.11 -42.5 23:46:14.4455 37.4 -122.11 -42.5 23:46:15.4455 37.4 -122.11 -42.5 23:46:16.4567 37.4 -122.11 -42.5 23:46:16.4567 37.4 -122.11 -42.5 23:46:17.4573 37.4 -122.11 -42.5 23:46:17.4573-122.11 - -42.5
从MAT文件加载IMU数据。IMU通常由个人传感器组成，该传感器报告有关车辆运动的信息。它们组合多个传感器，包括加速度计，陀螺仪和磁力计。这方向变量存储IMU传感器的报告方向。这些读数报告为四元数。每次读数被指定为包含四个四元数零件的1×4向量。将1-×4向量转换为a四元素（自动驾驶工具箱）
从mat文件加载IMU录音data =负载(imuFileName);imuOrientations = data.imuOrientations;％将IMU录制转换为四元数类型imuorientations = conventVars（Imuorientation，“定位”那'四元碘'）;％显示前几行的IMU数据头（Imuorientation）
ans = 8×1时间表时间定向_____________ ________________ 23：46：11.4570 [1×1四元音] 23：46：11.4605 [1×1四元数] 23：46：11.4620 [1×1四元数] 23：46：11.4655 [1×1季度] 23：46：11.4670 [1×1四元数] 23：46：11.4705 [1×1四元数] 23：46：11.4720 [1×1四元数] 23：46：11.4755 [1×1四元数]
要了解传感器读数如何进入每个传感器，计算近似帧期间
lidarFrameDuration =值(diff (lidarPointClouds.Time));gpsFrameDuration =值(diff (gpsSequence.Time));imuFrameDuration =值(diff (imuOrientations.Time));%调整显示格式为秒lidarframeduration.format ='；gpsframeduration.format ='；imuframeduration.format ='；％计算帧速率lidarRate = 1 /秒(lidarFrameDuration);gpsRate = 1 /秒(gpsFrameDuration);imuRate = 1 /秒(imuFrameDuration);％显示框架持续时间和速率fprintf（'LIDAR：％s，％3.1f hz \ n'char (lidarFrameDuration), lidarRate);fprintf（'GPS：％s，％3.1f hz \ n'，char（gpsframeduration），gpsrate）;fprintf（'IMU：％s，％3.1f hz \ n'，char（imuframeduration），刺激）;
LIDAR：0.10008秒，10.0 Hz GPS：1.0001秒，1.0 Hz IMU：0.002493秒，401.1 Hz
GPS传感器是最慢的，以靠近1 Hz的速率运行。LIDAR是下一个最慢的，以靠近10 Hz的速度运行，其次是近400 Hz的速率。
可视化驾驶数据
为了理解场景包含什么，使用流媒体播放器可视化记录的数据。要使GPS读数形象化，请使用Geoplayer.（自动驾驶工具箱）pcplayer
%创建一个地理播放器来可视化流地理坐标latCenter = gpsSequence.Latitude (1);lonCenter = gpsSequence.Longitude (1);zoomLevel = 17;gpsPlayer = geoplayer(latCenter, lonCenter, zoomLevel);％绘制完整的路线PlotRoute（GPSPlayer，GPSSequence.latitude，GPSSequence.Longitude）;％确定玩家的限制xlimits = [-45 45];%米ylimits = [-45 45];zlimits = [-10 20];%创建一个pc播放器从激光雷达传感器可视化流点云lidarPlayer = pcplayer(xlimits, ylimits, zlimits);%自定义播放器轴标签xlabel（lidarplayer.axes，“X (m)”）ylabel（Lidarplayer.axes，'y（m）'）zlabel（lidarplayer.axes，“Z (m)”)标题(lidarPlayer。轴,'LIDAR传感器数据'）％对齐屏幕上的玩家HelperalignPlayers（{GPSPlayer，LidarPlayer}）;GPS读数的％外循环（较慢的信号）为了G = 1：高度（GPSSequence）-1％从时间表提取地理坐标纬度= gpsSequence.Latitude (g);经度= gpsSequence.Longitude (g);％更新GPS显示中的当前位置绘图（GPSPlayer，纬度，经度）;％计算当前和下一个GPS读取的时间跨度timespan = timerange（gpssequence.time（g），gpssequence.time（g + 1））;%提取这段时间内记录的激光雷达帧lidarFrames = lidarPointClouds(timeSpan，:);激光雷达读数(更快的信号)为了l = 1：高度（Lidarframes）提取点云ptCloud = lidarFrames.PointCloud (l);更新激光雷达显示视图(lidarPlayer ptCloud);％暂停以减慢显示屏暂停(0.01)结尾结尾
使用录制的LIDAR数据来构建地图
Lidars是强大的传感器，可用于在挑战环境中的感知，其中其他传感器没有用。它们提供了一种详细的，完整的360度的车辆环境视图。
%隐藏球员隐藏(gpsPlayer)隐藏(lidarPlayer)％选择LIDAR数据帧以演示注册工作流程Framenum = 600;ptcloud = lidarpointclouds.pointcloud（framenum）;％显示和旋转自我视图以显示LIDAR数据helperVisualizeEgoView (ptCloud);
Lidars可用于构建厘米准确的高清地图，包括整个城市的高清地图。这些地图稍后可以用于车载本地化。构建这种地图的典型方法是将从移动车辆获得的连续激光乐队扫描对准，并将它们组合成单个大点云。此示例的其余部分探讨了构建地图的这种方法。
对齐LIDAR扫描：使用像迭代最近点（ICP）算法（ICP）算法等点云登记技术对齐连续的LIDAR扫描，或常规分布变换（NDT）算法。看
pcregistericppcregisterndtpcregisterndt函数返回将移动点云相对于参考点云对齐的刚性转换。通过依次组合这些变换，每个点云被转换回第一个点云的参考系。结合对齐的扫描：一旦注册了一个新点云扫描并转换回第一个点云的参考帧，可以使用第一点云合并点云
pcmerge
首先取两个点云对应附近的激光雷达扫描。为了加快处理速度，并在两次扫描之间积累足够的移动，请每10次扫描一次。
skipframes = 10;Framenum = 100;固定= lidarpointclouds.pointCloud（Framenum）;移动= lidarpointclouds.pointcloud（framenum + skipframes）;
在配准之前，对点云进行处理，以保留点云中独特的结构。这些预处理步骤包括:*检测和移除地平面*检测和移除自我车辆
这些步骤将更详细地描述用激光雷达探测地面和障碍物（自动驾驶工具箱）例子。在这个例子中，HelperProcessPointCloud.Helper函数完成这些步骤。
固定处理= HelperProcessPointCloud（固定）;移动处理= HelperProcessPointCloud（移动）;
在俯视图中显示原始和处理过的点云。洋红色点在加工过程中被去除。这些点对应于地平面和自我飞行器。
hFigFixed =图;pcshowpair(固定,fixedProcessed)视图(2);％调整视图以显示顶视图helperMakeFigurePublishFriendly (hFigFixed);在注册之前，％拆下点云。下采样改善%配准精度和算法速度。DownSamplePercent = 0.1;FiledownSampled = PCDownSample（固定处理，'随机的', downsamplePercent);movingDownsampled = pcdownsample (movingProcessed,'随机的', downsamplePercent);
对点云进行预处理后，使用无损检测进行注册。在注册之前和之后可视化对齐。
regGridStep = 5;tform = pcregisterndt(movingdownsampling, fixeddownsampling, regGridStep); / /指定采样时间movingReg = pctransform(movingProcessed, tform);%在注册前和注册后的顶部视图中可视化对齐hFigAlign =图;subplot(121) pcshowpair(movingProcessed, fixedProcessed) title('注册前'）查看（2）子平台（122）PCShowpair（voldeReg，固定处理）标题（'注册后')视图(2)helperMakeFigurePublishFriendly (hFigAlign);
请注意，点云在注册后齐全。即使点云密切对齐，对齐仍然不完美。
接下来，合并点云使用pcmerge。
mergegridstep = 0.5;ptcloudaccum = pcmerge（固定处理，vishreg，mergegridstep）;hfigaccum =图;pcshow（ptcloudaccum）标题（'累积点云')视图(2)helperMakeFigurePublishFriendly (hFigAccum);
现在，对于单个点云的处理管道得到很好的理解，在整个记录数据序列上将其放入循环中。这helperLidarMapBuilder班级将所有这些放在一起。这UpdateMap.该类的方法采用新的点云，并通过前面详述的步骤：
通过使用删除地面平面和自我车辆来处理点云
ProcessPointCloud.方法。对点云下采样。
估计与当前点云合并前一点云所需的刚性变换。
将点云转换回第一帧。
将点云与累积的点云图合并。
此外，这是UpdateMap.方法还接受一个初始转换估计，该估计用于初始化注册。良好的初始化可以显著改善注册的结果。相反，糟糕的初始化会对注册产生不利影响。提供良好的初始化还可以提高算法的执行时间。
提供用于注册初始估计的常用方法是使用恒定的速度假设。使用从前一个迭代的转换为初始估计。
这updateDisplay方法另外创建和更新2-D顶视图流点云显示。
％创建Map Builder对象mapBuilder = helperLidarMapBuilder ('downsamplepercent', downsamplePercent);%设置随机数种子rng (0);closeDisplay = false;numFrames =身高(lidarPointClouds);tform = rigid3d;为了n = 1: skipFrames: numFrames - skipFrames％得到第n点云ptCloud = lidarPointClouds.PointCloud (n);％使用从之前的迭代转换为初始估计点云注册点的百分比迭代。（恒速）inittform = tform;使用点云更新映射tform = updatemap（MapBuilder，PtCloud，Inittform）;％更新映射显示updatedisplay（mapbuilder，cliftisplay）;结尾
仅点云注册就可以构建车辆所经过环境的地图。虽然地图可能在局部上看起来是一致的，但它可能在整个序列上产生了显著的漂移。
使用录制的GPS读数作为地面真理轨迹，在视觉上评估内置地图的质量。首先将GPS读数（纬度，经度，高度）转换为本地坐标系。选择一个本地坐标系，该系统与序列中的第一点云的原点一致。使用两个转换计算此转换：
将GPS坐标转换为当地笛卡尔东北方坐标使用
latlon2local（自动驾驶工具箱）旋转笛卡尔坐标，使局部坐标系统与第一个激光雷达传感器坐标对齐。由于激光雷达和GPS在车辆上的确切安装配置还不知道，它们是估计的。
％选择参考点作为第一个GPS读数origin = [gpsSequence.Latitude(1)， gpsSequence.Longitude(1)， gpsSequence.Altitude(1)];％将GPS读数转换为当地东北坐标系[Xeast，Ynorth，zup] = Latlon2local（GPSSequence.litditude，GPSSequence.Longitude，．..gpsSequence。高度那origin);在轨迹开始时估计粗略方向以对准本地enuLIDAR坐标系的％系统theta =中值(atan2d(yNorth(1:15)， xEast(1:15)));R = [cosd(90-theta) sind(90-theta) 0;信德(90 -θ)cosd(90 -θ)0;0 0 1);旋转ENU坐标与激光雷达坐标系统对齐Troundtruthtrajectory = xeast，ynorth，zup] * r;
在内置地图上叠加地面真相轨迹。
(mapBuilder。轴,'在'）分散（MapBuilder.Axes，TountTreTrajectory（：，1），地面Truthtrajectory（:,2），．..'绿色'那'填充'）;helperAddLegend (mapBuilder。轴,．..{'地图积分'那'估计轨迹'那“地面实况轨迹”}）;
在初始转弯之后，估计的轨迹在地面真相显着轨迹。单独使用点云注册估计的轨迹可以漂移多种原因：
传感器的噪声扫描没有足够的重叠
缺乏足够强大的特征，例如，在较长的道路附近
不准确的初始转换，尤其是当旋转显着时。
％关闭地图显示updateDisplay (mapBuilder,真实);
使用IMU方向改进已建地图
IMU是安装在平台上的电子设备。IMU包含多个传感器，该传感器报告有关车辆运动的各种信息。典型的IMU包括加速度计，陀螺仪和磁力计。IMU可以提供可靠的方向度量。
使用IMU读数为注册提供一个更好的初始估计。本例中使用的imu报告的传感器读数已经在设备上进行了过滤。
重置地图生成器以清除先前生成的地图重置（MapBuilder）;%设置随机数种子rng (0);inittform = rigid3d;为了n = 1: skipFrames: numFrames - skipFrames％得到第n点云ptCloud = lidarPointClouds.PointCloud (n);如果n> 1％以来，IMU传感器以更快的速度报告读数，收集上次激光雷达扫描后的IMU读数。previme = lidarpointclouds.time（n - skipframes）;curtime = lidarpointclouds.time（n）;TimeIncescan = Timerange（previmity，curtime）;Imureadings = Imuorientations（TimeIncescan，“定位”）;%使用IMU读数形成初步估计inittform = helpercomputeinitialestimate fromimu（Imureadings，tform）;结尾使用点云更新映射tform = updatemap（MapBuilder，PtCloud，Inittform）;％更新映射显示updatedisplay（mapbuilder，cliftisplay）;结尾在新地图上叠加地面真实轨迹(mapBuilder。轴,'在'）分散（MapBuilder.Axes，TountTreTrajectory（：，1），地面Truthtrajectory（:,2），．..'绿色'那'填充'）;helperAddLegend (mapBuilder。轴,．..{'地图积分'那'估计轨迹'那“地面实况轨迹”}）;%捕获用于发布的快照snapnow;％近距离关闭（[Hfigfixed，Hfigalign，Hfigaccum]）;updateDisplay (mapBuilder,真实);
使用来自IMU的方向估计显著改善了配准，导致更近的轨迹和更小的漂移。
金宝app支持功能
HelperalignPlayers.对齐电池阵列的流媒体播放器，以便在屏幕上从左到右排列。
函数HelperalignPlayers（玩家）validateattributes（玩家，{“细胞”}， {“向量”}）;Hasaxes = Cellfun（@（p）是isprop（p，“轴”），玩家，“UniformOutput”,真正的);如果~ (hasAxes)错误(“希望所有观众都能拥有Axes属性”）;结尾拉=得到(大的,'屏幕尺寸'）;screenMargin = [50,100];cellfun(@getPlayerSize, players，“UniformOutput”， 错误的）;Playersize = Cell2mat（Playersizes）;maxheightinset = max（Playersize（1：3：结束））;垂直排列球员，使最高的球员是100像素％ 顶端。location = round([screenMargin(1)， screenSize(4)-screenMargin(2)- maxheight set]);为了N = 1: numel(players) player = players{N};hFig =祖先(球员。轴,'数字'）;hfig.uterposition（1：2）=位置;右转，％设置下一个位置位置=位置+ [50 + hfig.outerposition（3），0];结尾函数深圳= getPlayerSize(观众)％获取父数据容器h =祖先（查看者.Axes，'数字'）;深圳= h.OuterPosition (3:4);结尾结尾
helperVisualizeEgoView通过绕中心旋转，在自我视角中可视化点云数据。
函数球员= helperVisualizeEgoView (ptCloud)％创建PCPlayer对象xlimits = ptcloud.xlimits;ylimits = ptcloud.ylimits;zlimits = ptcloud.zlimits;Player = PCPlayer（xlimits，ylimits，zlimits）;％关掉轴线轴（Player.axes，'离开'）;％设置相机以显示自我视图Camproj（Player.Axes，'看法'）;camva(球员。轴,90);坎波斯(球员。轴,[0 0 0]); camtarget(player.Axes, [-1 0 0]);％设置转换以旋转5度θ= 5;r = [cosd（θ）sind（θ）0 0 0 -sind（θ）cosd（θ）0 0 0 0 1 0 0 0 0 1];rotatebytheta = rigid3d（r）;为了N = 0: theta: 359%将点云旋转为ptcloud = pctransform（ptcloud，rotatebytheta）;显示点云视图(球员,ptCloud);暂停(0.05)结尾结尾
HelperProcessPointCloud.通过去除属于地平面或自我载体的点来处理点云。
函数ptcloudprocessed = helperprocesspointcloud（ptcloud）％检查点云是否被组织IsOrganized =〜Ismatrix（Ptcloud.Location）;%如果点云是有组织的，使用基于范围的洪水填充算法% (segmentGroundFromLidarData)。否则，使用平面配件。GroundSegationMethods = [“平面图”那“RangeFloodfill”];方法= GroundSegationMethods（IsOrganized + 1）;如果方法= =“平面图”％段作为主导平面，参考正常矢量%指向正z方向，使用pcfitplane。为组织%点云，考虑使用segmentGroundFromLidarData代替。maxDistance = 0.4;%米maxAngDistance = 5;％程度refvector = [0,0,1];% z方向[~， grounddinices] = pcfitplane(ptCloud, maxDistance, refVector, maxAngDistance);elseif方法= =“RangeFloodfill”使用基于范围的洪水填充的％段地面。groundIndices = segmentGroundFromLidarData (ptCloud);别的错误(“预期方法是”平面“或”RangeFloodFill“的”）结尾％段EGO车辆作为给定的传感器半径内的点SensorLocation = [0,0,0];半径= 3.5;egoindices = findneighborsinradius（ptcloud，sensorlocation，半径）;移除属于地面或自我车辆的点数ptstokeep = true（ptcloud.count，1）;ptstokeep（groundindices）= false;ptstokeep（egoindices）= false;%如果点云是有组织的，保持有组织的结构如果ISORANGANIZED PTCLOUDPROCESSED = SELECT（PTCLOUD，查找（PTSTOKEEP），'输出'那'满的'）;别的ptcloudprocessed = select（ptcloud，find（ptstokeep））;结尾结尾
helperComputeInitialEstimateFromIMU使用IMU方向读数和先前估计的转换来估算NDT的初始转换。
函数tform = helperComputeInitialEstimateFromIMU(imureads, prevTform)使用先前估计的变换初始化转换tform = prevtform;%如果没有IMU读数，返回如果高度(imuReadings) < = 1返回；结尾% IMU方向读数报告为四元数表示％旋转偏移到车身框架。计算方向变化%在间隔期间第一个和最后一个报告的IMU方向之间LIDAR扫描的百分比。q1 = imuReadings.Orientation (1);q2 = imuReadings.Orientation(结束);％计算第一和最后一个IMU之间的旋转偏移% -从q2帧旋转到体帧% -从身体框架旋转到q1框架Q = Q1 *结合（Q2）;％转换为欧拉角yawpitchroll = euler（q，'Zyx'那“点”）;％丢弃音调和滚角估计。仅使用标题角度估计%从IMU方向。yawPitchRoll (2:3) = 0;％转换回旋转矩阵q =四元数（yawpectroll，“欧拉”那'Zyx'那“点”）;r = rotmat（q，“点”）;％使用计算的旋转tform.t（1：3,1：3）= r';结尾
杠杆制服向轴添加一个图例。
函数Helperaddlegend（HAX，标签）％向轴添加一个图例hlegend = legend（hax，标签{：}）;设置文本颜色和字体权重hlegend.textcolor = [1 1 1];hlegend.fontweight =.“大胆”；结尾
HelpermakeFigurepublish友谊调整数据，使截图发布是正确的。
函数HelpermakeFigurepublishFriendly（HFIG）如果~是空的(hFig) &&是有效的(hFig)HandleVisibility ='打回来'；结尾结尾
也可以看看
功能
对象
pcplayer|pointcloud.|Geoplayer.（自动驾驶工具箱）
相关的话题
- 使用SLAM构建来自LIDAR数据的地图
- 用激光雷达探测地面和障碍物（自动驾驶工具箱）
外部网站
你也可以从以下列表中选择一个网站:
