从激光雷达数据构建地图
这个例子展示了如何在惯性测量单元(IMU)读数的帮助下，处理安装在车辆上的传感器的3d激光雷达数据，逐步构建地图。这样的地图可以方便车辆导航的路径规划，也可以用于定位。为了评估生成的地图，这个示例还展示了如何将车辆的轨迹与全球定位系统(GPS)记录进行比较。
概述
高清晰度(HD)地图是一种地图服务，可以提供精确到几厘米的道路几何形状。这种精度水平使得高清地图适用于自动驾驶工作流程，如定位和导航。这种高清地图是通过3d激光雷达扫描生成的地图，结合高精度GPS和/或IMU传感器，可用于在几厘米内定位车辆。这个例子实现了构建这样一个系统所需的功能子集。
在这个例子中，你将学习如何:
加载，探索和可视化记录的驾驶数据
使用激光雷达扫描建立一个地图
使用IMU读数改善地图
加载和探索记录的驾驶数据
本例中使用的数据来自这个GitHub®存储库，代表了大约100秒的激光雷达、GPS和IMU数据。数据以mat文件的形式保存，每个mat文件包含一个时间表．从存储库中下载mat文件并将其加载到MATLAB®工作区中。
注意:这个下载可能需要几分钟。
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”;folderExists = exists(数据文件夹，“dir”）;matfilesExist = exist(lidarFileName，“文件”) &&存在(imuFileName，“文件”）…& &存在(gpsFileName“文件”）;如果~ folderExists mkdir (dataFolder);结束如果~ matfilesExist disp (“下载lidarPointClouds。mat (613 MB)……”) websave(lidarFileName, baseDownloadURL +“lidarPointClouds.mat”、选择);disp (“下载imuOrientations。mat (1.2 MB)……”) websave(imuFileName, baseDownloadURL +“imuOrientations.mat”、选择);disp (“下载gpsSequence。mat (3kb)……”) websave(gpsFileName, baseDownloadURL +“gpsSequence.mat”、选择);结束
首先，加载从Velodyne®HDL32E激光雷达中保存的点云数据。激光雷达数据的每次扫描都被存储为三维点云pointCloud对象。该对象使用K-d树数据结构在内部组织数据，以便更快地进行搜索。与每次激光雷达扫描相关联的时间戳记录在时间时间表的变数。
从mat文件加载激光雷达数据data = load(lidarFileName);lidarPointClouds = data.lidarPointClouds;显示前几行激光雷达数据头(lidarPointClouds)
时间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文件加载GPS数据。的纬度，经度,高度的变量时间表用于存储由车辆上的GPS设备记录的地理坐标。
从mat文件加载GPS序列data = load(gpsFileName);gpsSequence = data.gpsSequence;显示前几行GPS数据头(gpsSequence)
的时间经度纬度海拔 _____________ ________ _________ ________ 23:46:11.4563 23:46:12.4563 37.4 -122.11 -42.5 37.4 -122.11 -42.5 37.4 -122.11 -42.5 23:46:13.4565 23:46:14.4455 23:46:15.4455 37.4 -122.11 -42.5 37.4 -122.11 -42.5 37.4 -122.11 -42.5 23:46:16.4567 23:46:17.4573 23:46:18.4656 37.4 -122.11 -42.5 37.4 -122.11 -42.5
从mat文件加载IMU数据。IMU通常由报告车辆运动信息的单个传感器组成。它们结合了多个传感器，包括加速度计、陀螺仪和磁力计。的取向变量存储IMU传感器报告的方向。这些读数被报告为四元数。每次读取被指定为包含四个四元数部分的1 × 4向量。将1 × 4向量转换成a四元数对象。
%从mat文件加载IMU录音data = load(imuFileName);imuOrientations = data.imuOrientations;%将IMU记录转换为四元数类型imuOrientations = convertars (imuOrientations，“定位”，“四元数”）;%显示IMU数据的前几行头(imuOrientations)
时间取向 _____________ ______________ 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 = median(diff(lidarpointcloud . time));gpsframe = median(diff(gpsSequence.Time));imuFrameDuration = median(diff(imuorientation . time));%调整显示格式为秒lidarFrameDuration。格式=“年代”;gpsFrameDuration。格式=“年代”;imuFrameDuration。格式=“年代”;%计算帧速率lidarRate = 1/秒(lidarFrameDuration);gpsRate = 1/秒(gpsFrameDuration);imuRate = 1/秒(imuFrameDuration);%显示帧持续时间和速率流(“激光雷达:%s， %3.1 Hz\n”， char(lidarFrameDuration)， lidarRate);流(“GPS: %s， %3.1f Hz\n”， char(gpsFrameDuration)， gpsRate);流(IMU: %s， %3.1f Hz\n， char(imuFrameDuration)， imuRate);
激光雷达:0.10008秒，10.0 Hz GPS: 1.0001秒，1.0 Hz IMU: 0.002493秒，401.1 Hz
GPS传感器是最慢的，运行速度接近1hz。其次是激光雷达，运行速度接近10赫兹，其次是IMU，运行速度接近400赫兹。
可视化驾驶数据
为了理解场景所包含的内容，使用流媒体播放器将记录的数据可视化。要可视化GPS读数，使用geoplayer．可视化激光雷达读数使用pcplayer．
创建一个geoplayer来可视化流的地理坐标latCenter = gpsSequence.Latitude(1);lonCenter = gpsSequence.Longitude(1);zoomLevel = 17;gpsPlayer = geoplayer(latCenter, lonCenter, zoomLevel);%绘制完整路由plotRoute (gpsPlayer gpsSequence。纬度,gpsSequence.Longitude);确定玩家的限制Xlimits = [-45 -45];%米Ylimits = [-45 -45];Zlimits = [-10 20];创建一个pc播放器来可视化从激光雷达传感器流点云lidarPlayer = pcplayer(xlimits, ylimits, zlimits);%自定义播放器轴标签包含(lidarPlayer。轴,“X (m)”) ylabel (lidarPlayer。轴,“Y (m)”) zlabel (lidarPlayer。轴,“Z (m)”)标题(lidarPlayer。轴,“激光雷达传感器数据”）%在屏幕上对齐球员helperAlignPlayers ({gpsPlayer, lidarPlayer});% GPS读数外环(较慢的信号)为g = 1:高度(gpsSequence)-1%从时间表中提取地理坐标latitude = gpsSequence.Latitude(g);longitude = gpsSequence.Longitude(g);在GPS显示中更新当前位置plotPosition(gpsPlayer，纬度，经度);%计算当前和下一次GPS读数之间的时间跨度timeSpan = timerange(gpsSequence.Time(g)， gpsSequence.Time(g+1));%提取在此时间范围内记录的激光雷达帧lidarFrames = lidarPointClouds(timeSpan，:);%激光雷达读数的内循环(更快的信号)为l = 1: height(lidarFrames)%提取点云ptCloud = lidarFrames.PointCloud(l);%更新激光雷达显示视图(lidarPlayer ptCloud);%暂停以减慢显示速度暂停(0.01)结束结束
使用记录的激光雷达数据来构建地图
激光雷达是一种功能强大的传感器，可用于在其他传感器无法发挥作用的挑战性环境中进行感知。它们提供了车辆环境的详细、完整的360度视图。
%隐藏玩家隐藏(gpsPlayer)隐藏(lidarPlayer)%选择一帧激光雷达数据来演示注册工作流程frameNum = 600;ptCloud = lidarpointcloud . pointcloud (frameNum);显示并旋转自我视图以显示激光雷达数据helperVisualizeEgoView (ptCloud);
激光雷达可以用来制作厘米级精度的高清地图，包括整个城市的高清地图。这些地图以后可以用于车载定位。建立这种地图的一个典型方法是将从移动车辆获得的连续激光雷达扫描结果对齐，并将它们合并成一个大的点云。本示例的其余部分将探索这种构建地图的方法。
对齐激光雷达扫描:使用点云配准技术(如迭代最近点(ICP)算法或正态分布变换(NDT)算法)对齐连续激光雷达扫描。看到
pcregistericp和pcregisterndt有关每种算法的更多详细信息。这个例子使用无损检测，因为它通常更准确，特别是在考虑旋转时。的pcregisterndt函数返回使移动点云相对于参考点云进行对齐的刚性变换。通过连续组合这些转换，每个点云被转换回第一个点云的参考框架。合并对齐扫描:一旦一个新的点云扫描被配准并转换回第一个点云的参考帧，点云就可以与第一个点云合并
pcmerge．
首先取两个与附近激光雷达扫描相对应的点云。为了加快处理速度，并在扫描之间积累足够的运动，每10次扫描使用一次。
skipFrames = 10;frameNum = 100;fixed = lidarpointcloud . pointcloud (frameNum);移动= lidarPointClouds。PointCloud(frameNum + skipFrames);
在配准之前，对点云进行处理，以保留点云中具有特征的结构。这些预处理步骤包括:
检测并拆除接地面
发现并移除自我飞行器
中更详细地描述了这些步骤利用激光雷达进行地平面和障碍物检测的例子。在本例中，helperProcessPointCloud辅助函数完成这些步骤。
fixedProcessed = helperProcessPointCloud(fixed);movingProcessed = helperProcessPointCloud(moving);
在顶视图中显示原始和处理过的点云。洋红色点在加工过程中被去除。这些点对应于地平面和自我飞行器。
hFigFixed =图;pcshowpair(fixed, fixedProcessed)视图(2);调整视图以显示俯视图helperMakeFigurePublishFriendly (hFigFixed);在注册之前下载点云样本。将采样提高%的配准精度和算法速度。downsamplePercent = 0.1;fixeddownsampling = pcdownsample(fixedProcessed，“随机”, downsamplePercent);movingdownsampling = pcdownsample(movingProcessed，“随机”, downsamplePercent);
对点云进行预处理后，使用无损检测进行配准。在注册前后可视化对齐。
regGridStep = 5;tform = pcregisterndt(movingdownsampling, fixeddownsampling, regGridStep);movingReg = pctransform(movingProcessed, tform);在注册前后的俯视图中可视化对齐hFigAlign =图;subplot(121) pcshowpair(movingProcessed, fixedProcessed)之前注册的(2) subplot(122) pcshowpair(movingReg, fixedProcessed) title(注册后的)视图(2)helperMakeFigurePublishFriendly(hFigAlign);
注意点云在配准后对齐良好。尽管点云排列得很近，但排列仍然不完美。
接下来，合并点云使用pcmerge．
mergeGridStep = 0.5;ptCloudAccum = pcmerge(fixedProcessed, movingReg, mergeGridStep);hFigAccum =图;pcshow (ptCloudAccum)标题(“累积点云”)视图(2)helperMakeFigurePublishFriendly(hFigAccum);
现在已经很好地理解了一对点云的处理管道，将它们放在一个循环中，覆盖整个记录数据序列。的helperLidarMapBuilder类把所有这些放在一起。的updateMap类的方法采用一个新的点云，并经过前面详细的步骤:
处理点云通过删除地平面和自我车辆，使用
processPointCloud方法。对点云进行下采样。
估计合并前点云与当前点云所需的刚性变换。
将点云转换回第一帧。
合并点云和累积点云图。
此外,updateMap方法还接受用于初始化注册的初始转换估计。良好的初始化可以显著提高配准结果。相反，糟糕的初始化会对注册产生不利影响。提供良好的初始化也可以改善算法的执行时间。
为配准提供初始估计的常用方法是使用恒定速度假设。使用来自前一个迭代的转换作为初始估计。
的updateDisplay方法另外创建并更新二维顶视图流点云显示。
创建一个地图生成器对象mapBuilder = helperLidarMapBuilder(“DownsamplePercent”, downsamplePercent);%设置随机数种子rng (0);closeDisplay = false;numFrames = height(lidarPointClouds);Tform = rigidtform3d;为n = 1: skipFrames: numFrames - skipFrames得到第n个点云ptCloud = lidarpointcloud . pointcloud (n);使用来自先前迭代的转换作为初始估计点云配准的当前迭代。(恒定速度)initTform = tform;使用点云更新地图tform = updateMap(mapBuilder, ptCloud, initTform);%更新地图显示updateDisplay (mapBuilder closeDisplay);结束
点云注册单独构建车辆所穿越的环境地图。虽然地图可能在局部看起来是一致的，但它可能在整个序列中产生了显著的漂移。
使用记录的GPS读数作为地面真实轨迹，以直观地评估所建地图的质量。首先将GPS读数(纬度、经度、高度)转换为本地坐标系统。选择一个与序列中第一个点云的原点一致的局部坐标系。这个转换使用两个转换来计算:
将GPS坐标转换为本地笛卡尔东-北-上坐标
latlon2local函数。以轨迹起始处的GPS位置作为参考点，定义本地x、y、z坐标系的原点。旋转笛卡尔坐标，使本地坐标系与第一个激光雷达传感器坐标对齐。由于车辆上的激光雷达和GPS的确切安装配置是未知的，它们是估计的。
%选择参考点作为第一个GPS读数origin = [gpsSequence.Latitude(1)， gpsSequence.Longitude(1)， gpsSequence.Altitude(1)];将GPS读数转换为本地的东北向坐标系统[xEast, yNorth, zUp] = latlon2local(gpsSequence.)纬度,gpsSequence。经度,…gpsSequence。高度，origin);估计轨迹开始时的粗略方向，以对准本地ENU%系统与激光雷达坐标系统theta = median(atan2d(yNorth(1:15)， xEast(1:15)));R = [cosd(90- θ) sind(90- θ) 0;-sind(90) cosd(90) 0;[0 0 1];旋转ENU坐标以与激光雷达坐标系统对齐groundTruthTrajectory = [xEast, yNorth, zUp] * R;
将地面真实轨迹叠加在已建成的地图上。
(mapBuilder。轴,“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),…“绿色”，“填充”）;helperAddLegend (mapBuilder。轴,…｛“地图点”，“估计轨迹”，“地面真相轨迹”}）;
初始转弯后，估计轨迹明显偏离地面真实轨迹。单独使用点云配准估计的轨迹可能会漂移，原因如下:
来自传感器的噪声扫描没有足够的重叠
缺乏足够强大的功能，例如，在漫长的道路附近
不准确的初始转换，特别是当旋转是重要的。
%关闭地图显示updateDisplay (mapBuilder,真实);
使用IMU方向来改进构建地图
IMU是一种安装在平台上的电子设备。imu包含多个传感器，可以报告有关车辆运动的各种信息。典型的imu包括加速度计、陀螺仪和磁力计。IMU可以提供可靠的方向测量。
使用IMU读数为注册提供更好的初步估计。本例中使用的imu报告的传感器读数已经在设备上进行了过滤。
重置地图生成器以清除以前构建的地图重置(mapBuilder);%设置随机数种子rng (0);initTform = rigidtform3d;为n = 1: skipFrames: numFrames - skipFrames得到第n个点云ptCloud = lidarpointcloud . pointcloud (n);如果N > 1由于IMU传感器报告读数的速度快得多，收集自上次激光雷达扫描以来报告的% IMU读数。prevTime = lidarPointClouds。时间(n - skipFrames);currTime = lidarpointcloud . time (n);timeSinceScan = timerange(prevTime, currTime);imureads = imuOrientations(timeSinceScan，“定位”）;使用IMU读数形成初步估计initTform = helperComputeInitialEstimateFromIMU(imureads, tform);结束使用点云更新地图tform = updateMap(mapBuilder, ptCloud, initTform);%更新地图显示updateDisplay (mapBuilder closeDisplay);结束在新地图上叠加地面真实轨迹(mapBuilder。轴,“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),…“绿色”，“填充”）;helperAddLegend (mapBuilder。轴,…｛“地图点”，“估计轨迹”，“地面真相轨迹”}）;%捕获用于发布的快照snapnow;%关闭打开数字close([hFigFixed, hFigAlign, hFigAccum]);updateDisplay (mapBuilder,真实);
使用IMU的方向估计可以显著改善配准，从而获得更接近的轨迹和更小的漂移。
万博1manbetx支持功能
helperAlignPlayers对齐流媒体播放器的单元阵列，使它们在屏幕上从左到右排列。
函数helperAlignPlayers(players) validateattributes(players， {“细胞”}, {“向量”}）;hasAxes = cellfun(@(p)isprop(p)，“轴”),球员,“UniformOutput”,真正的);如果~ (hasAxes)错误("期望所有查看器都有一个Axes属性"）;结束screenSize = get(root;“拉”）;screenMargin = [50,100];playersize = cellfun(@getPlayerSize, players;“UniformOutput”、假);playersize = cell2mat(playersize);maxhightinset = max(playerSizes(1:3:end));垂直排列球员，使最高的球员距离100像素%顶部。location = round([screenMargin(1)， screenSize(4)-screenMargin(2)- maxhightinset]);为N = 1: numel(players) player = players{N};hFig =祖先(玩家)轴,“图”）;hFig.OuterPosition(1:2) = location;向右走，建立下一个位置location = location + [50+hFig.OuterPosition(3)， 0];结束函数sz = getPlayerSize(查看器)%获取父图形容器H =祖先(查看者)。轴,“图”）;sz = h.OuterPosition(3:4);结束结束
helperVisualizeEgoView通过围绕中心旋转在自我视角中可视化点云数据。
函数玩家= helperVisualizeEgoView(ptCloud)创建一个pcplayer对象xlimits = ptCloud.XLimits;ylimits = ptCloud.YLimits;zlimits = ptCloud.ZLimits;Player = pcplayer(xlimits, ylimits, zlimits);%关闭轴线轴(球员。轴,“关闭”）;设置相机显示自我视图camproj(球员。轴,“视角”）;camva(球员。轴,90);坎波斯(球员。轴,[0 0 0]); camtarget(player.Axes, [-1 0 0]);设置一个变换，旋转5度θ = 5;eulerAngles = [0 0 θ];翻译= [0 0 0];rotateByTheta = rigidtform3d(eulerAngles, translation);为N = 0: θ = 359旋转点云ptCloud = pctransform(ptCloud, rotateByTheta);%显示点云视图(球员,ptCloud);暂停(0.05)结束结束
helperProcessPointCloud通过移除属于地平面或自我车辆的点来处理点云。
函数ptCloudProcessed = helperProcessPointCloud(ptCloud)%检查点云是否被组织isOrganized = ~ismatrix(ptCloud.Location);%如果点云是有组织的，则使用基于距离的洪水填充算法% (segmentGroundFromLidarData)。否则，请使用平面配件。groundSegmentationMethods = [“planefit”，“rangefloodfill”];method = groundSegmentationMethods(isOrganized+1);如果方法= =“planefit”线段以地面为主导面，以参考法向量%指向正z方向，使用pcfitplane。为组织%点云，考虑使用segmentGroundFromLidarData代替。maxDistance = 0.4;%米maxAngDistance = 5;%度refVector = [0,0,1];% z方向[~， groundindiices] = pcfitplane(ptCloud, maxDistance, refVector, maxAngDistance);elseif方法= =“rangefloodfill”%分段地面使用范围为基础的洪水填充。groundIndices = segmentGroundFromLidarData(ptCloud);其他的错误("预期方法为'planefit'或'rangefloodfill'"）结束在给定的传感器半径范围内，将自我车辆分割为点sensorLocation = [0,0,0];半径= 3.5;egoIndices = findNeighborsInRadius(ptCloud, sensorLocation, radius);移除属于地面或自我车辆的点数ptsToKeep = true(ptCloud;统计,1);ptsToKeep(groundIndices) = false;ptsToKeep(egoIndices) = false;%如果点云是有组织的，则保留有组织的结构如果isOrganized ptCloudProcessed = select(ptCloud, find(ptsToKeep)，“OutputSize”，“全部”）;其他的ptCloudProcessed = select(ptCloud, find(ptsToKeep));结束结束
helperComputeInitialEstimateFromIMU使用IMU方向读数和先前估计的转换来估计无损检测的初始转换。
函数tform = helperComputeInitialEstimateFromIMU(imureads, prevTform)使用先前估计的转换初始化转换tform = prevTform;%如果没有IMU读数，返回如果高度(imureads) <= 1返回;结束% IMU方向读数报告为四元数，表示%到主体框架的旋转偏移量。计算方向变化在间隔时间内，第一次和最后一次报告的IMU方向之间的百分比%的激光雷达扫描。q1 = imureads . orientation (1);q2 = imureads . orientation (end);%计算第一次和最后一次IMU读取之间的旋转偏移% -从q2帧旋转到body帧% -从身体帧旋转到第1帧Q = q1 * conj(q2);%转换成欧拉角yawPitchRoll = euler(q，“ZYX股票”，“点”）;%放弃俯仰角和横摇角估计。只使用航向角估计%来自IMU方向。yawPitchRoll(2:3) = 0;%转换回旋转矩阵四元数(yawPitchRoll，“欧拉”，“ZYX股票”，“点”）;R = rotmat(q，“点”）;%使用计算旋转tform。T(1:3, 1:3) = r ';结束
helperAddLegend在坐标轴上添加图例。
函数helperAddLegend (hAx、标签)在坐标轴上添加一个图例hLegend = legend(hAx, labels{:});%设置文本颜色和字体粗细hLegend。TextColor = [1 1 1];hLegend。FontWeight =“大胆”;结束
helperMakeFigurePublishFriendly调整数字，以便发布捕获的屏幕截图是正确的。
函数helperMakeFigurePublishFriendly (hFig)如果~isempty(hFig) && isvalid(hFig) hFig。HandleVisibility =“回调”;结束结束
