baseDownloadURL='https://github.com/mathworks/udacity-self-driving-data-subset/raw/master/drive_segment_11_18_16/'; dataFolder=fullfile（tempdir，“驾驶室段11、18、16”, filesep);选择= weboptions (“超时”，Inf）；lidarFileName=dataFolder+“lidarPointClouds.mat”；imuFileName=dataFolder+“iMuoOrientations.mat”；gpsFileName=dataFolder+“gpsSequence.mat”；folderExists =存在(dataFolder,“dir”）;matfilesExist =存在(lidarFileName,“文件”)&&exist（imuFileName，“文件”）...&&存在（gpsFileName，“文件”）;如果~ folderExists mkdir (dataFolder);终止如果~ matfilesExist disp ('正在下载lidarPointClouds.mat（613 MB）…') websave(lidarFileName, baseDownloadURL +“lidarPointClouds.mat”、选择);disp (“下载imuOrientations。垫(1.2 MB)……”)websave（imuFileName，baseDownloadURL+“iMuoOrientations.mat”、选择);disp (“下载gpsSequence。垫(3 KB)……”)websave（gpsFileName，baseDownloadURL+“gpsSequence.mat”、选择);终止

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

%从MAT文件加载GPS序列data =负载(gpsFileName);gpsSequence = data.gpsSequence;%显示前几行GPS数据头(gpsSequence)

%从MAT文件加载IMU录制数据=加载（imuFileName）；imuOrientations=数据。imuOrientations；%将IMU录制转换为四元数类型imuOrientations=转换器变量（imuOrientations，“方向”，“四元数”）;%显示IMU数据的前几行头部（定向）

lidarFrameDuration=中值（diff（lidarPointClouds.Time））；gpsFrameDuration=中值（diff（gpsSequence.Time））；imuFrameDuration=中值（diff（imuOrientations.Time））；%将显示格式调整为秒lidarFrameDuration.Format=“年代”；gpsFrameDuration。格式=“年代”；imuFrameDuration。格式=“年代”；%计算帧速率lidarRate=1/秒（lidarFrameDuration）；gpsRate=1/秒（gpsFrameDuration）；imuRate=1/秒（imuFrameDuration）；%显示帧持续时间和速率流('激光雷达:%s， %3.1f Hz\n'，char（lidarFrameDuration），lidarate；fprintf('全球定位系统：%s，%3.1f赫兹\n'，char（gpsFrameDuration），gpsRate；fprintf('IMU:%s，%3.1f Hz\n'char (imuFrameDuration), imuRate);

%创建地质图层以可视化流式地理坐标latCenter=gpsSequence.纬度（1）；lonCenter=gpsSequence.经度（1）；zoomLevel=17；gpsPlayer=地质层（latCenter、lonCenter、zoomLevel）；绘制完整的路线绘图路线（gpsPlayer、gpsSequence.纬度、gpsSequence.经度）；%确定玩家的限制Xlimits = [-45 45];%仪表Ylimits = [-45 45];Zlimits = [-10 20];%创建一个pcplayer以可视化来自激光雷达传感器的流点云lidarPlayer=pcplayer（xlimits、ylimits、zlimits）；%自定义播放器轴标签包含(lidarPlayer。轴,‘X（m）’)ylabel（lidarPlayer.Axes，‘Y（m）’) zlabel (lidarPlayer。轴,‘Z（m）’)标题（lidarPlayer.Axes，“激光雷达传感器数据”）%在屏幕上对齐玩家HelperAlignPlayer（{gpsPlayer，lidarPlayer}）；%外环超过GPS读数（较慢信号）为g=1：高度（GPS顺序）-1%从时间表中提取地理坐标纬度=gpsSequence.纬度（g）；经度=gpsSequence.经度（g）；%更新当前位置在GPS显示plotPosition (gpsPlayer,经度和纬度);%计算当前和下一次GPS读取之间的时间跨度timeSpan=时间范围（gpsSequence.Time（g），gpsSequence.Time（g+1））；%提取在此时间跨度内记录的激光雷达帧lidarFrames=lidarPointClouds（时间跨度：）；%内环激光雷达读数（更快的信号）为l = 1:高度(lidarFrames)%提取点云ptCloud=lidarFrames.PointCloud（l）；%更新激光雷达显示器视图（lidarPlayer、ptCloud）；%暂停以减慢显示速度暂停（0.01）终止终止

%隐藏玩家隐藏（gpsPlayer）隐藏（lidarPlayer）%选择一帧激光雷达数据来演示注册流程frameNum = 600;ptCloud = lidarPointClouds.PointCloud (frameNum);%显示并旋转ego视图以显示激光雷达数据HelpServiceSualizegView（ptCloud）；

skipFrames = 10;frameNum = 100;固定= lidarPointClouds.PointCloud (frameNum);= lidarPointClouds移动。PointCloud (frameNum + skipFrames);

fixedProcessed = helperProcessPointCloud(固定);movingProcessed = helperProcessPointCloud(移动);

hFigFixed=图；pcshowpair（固定，固定处理）视图（2）；%调整视图以显示顶部视图helperMakeFigurePublishFriendly（hFigFixed）；%在注册之前对点云进行下采样。下采样可提高%配准精度和算法速度。下降采样率=0.1；固定下降采样率=pcdownsample（固定已处理，“随机”，下采样率）；移动下采样=pcdownsample（移动已处理，“随机”，降低采样率）；

regGridStep=5；tform=pcregisterndt（移动下采样、固定下采样、regGridStep）；movingReg=pctransform（movingProcessed，tform）；%在注册前后在俯视图中可视化对齐hFigAlign=图；子批次（121）pcshowpair（移动处理、固定处理）标题(“注册前”) view(2) subplot(122) pcshowpair(movingReg, fixedProcessed) title(注册后的)视图（2）helperMakeFigurePublishFriendly（hFigAlign）；

mergeGridStep=0.5；ptCloudAccum=pcmerge（fixedProcessed，movingReg，mergeGridStep）；hFigAccum=figure；pcshow（ptCloudAccum）标题(“累积点云”)视图（2）helperMakeFigurePublishFriendly（hFigAccum）；

创建一个地图构建器对象mapBuilder=helperLidarMapBuilder(“降采样率”，降低采样率）；%设置随机数种子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）；%更新地图显示更新显示（mapBuilder、closeDisplay）；终止

%选择参考点作为第一个GPS读数原点=[gpsSequence.纬度（1）、gpsSequence.经度（1）、gpsSequence.海拔（1）]；%将GPS读数转换为本地东北向上坐标系[xEast，yNorth，zUp]=latlon2local（gpsSequence.纬度，gpsSequence.经度，...GPS序列、高度、原点）；%估计轨迹开始时的大致方向，以对齐局部轨迹%激光雷达坐标系θ=中位数（atan2d（yNorth（1:15），xEast（1:15））；R=[cosd（90θ）sind（90θ）0；-sind（90θ）cosd（90θ）0；0.1]；%旋转ENU坐标以与激光雷达坐标系对齐地面轨迹=[xEast，yNorth，zUp]*R；

按住（mapBuilder.Axes，“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),...“绿色”，“填充”)；helperAddLegend（mapBuilder.Axes，...｛“地图点”，“估计轨迹”，“地面真相轨道”});

%关闭地图显示updateDisplay（mapBuilder，true）；

%重置地图生成器以清除以前生成的地图重置（mapBuilder）；%设置随机数种子rng（0）；initTform=rigid3d；为n=1:skipFrames:numFrames-skipFrames%获取第n个点云ptCloud=lidarPointClouds.PointCloud（n）；如果n > 1由于IMU传感器以更快的速度报告读数，请收集%自上次激光雷达扫描以来报告的IMU读数。prevTime = lidarPointClouds。时间(n - skipFrames);currTime = lidarPointClouds.Time (n);timeSinceScan = timerange(prevTime, currTime);imuReadings = imuOrientations (timeSinceScan,“方向”）;%使用IMU读数形成初始估计值initTform=helpercomputeinitialestimatefromu（imuReadings，tform）；终止%使用点云更新地图tform=updateMap（mapBuilder、ptCloud、initTform）；%更新地图显示更新显示（mapBuilder、closeDisplay）；终止%在新地图上叠加地面真实轨迹按住（mapBuilder.Axes，“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),...“绿色”，“填充”)；helperAddLegend（mapBuilder.Axes，...｛“地图点”，“估计轨迹”，“地面真相轨道”});%捕获用于发布的快照斯内普诺；%关闭打开数字关闭（[hFigFixed，hFigAlign，hFigAccum]）；更新显示（mapBuilder，true）；

作用HelperAlignPlayer（玩家）验证属性（玩家、{“细胞”}, {“向量”})hasAxes=cellfun（p）isprop（p，“斧头”)，玩家，“UniformOutput”，对）；如果~all（hasAxes）错误('预期所有查看器都具有Axis属性'）;终止屏幕大小=获取（groot，“拉”); 屏幕边距=[50100]；PlayerSize=cellfun（@getPlayerSize，玩家，“UniformOutput”，false）；playerSizes=cell2mat（playerSizes）；maxHeightInSet=max（playerSizes（1:3:end））；%垂直排列播放器，使最高的播放器距离屏幕100像素%。位置=圆形（[屏幕边距（1），屏幕尺寸（4）-屏幕边距（2）-最大高度插入]）；为n=1:numel（players）player=players{n}；hFig=祖先（player.Axes，“数字”)hFig.外部位置（1:2）=位置；向右设置下一个位置位置=位置+[50+hFig.外部位置（3），0]；终止作用sz=getPlayerSize（查看器）%获取父图形容器h=祖先（viewer.Axes，“数字”)sz=h.外置（3:4）；终止终止

作用player=helpervicesualizegoview（ptCloud）%创建pcplayer对象xlimits=ptCloud.xlimits；ylimits=ptCloud.ylimits；zlimits=ptCloud.zlimits；player=pcplayer（xlimits，ylimits，zlimits）；%关闭轴线轴（player.Axes，“关”）;%设置摄像头以显示自我视图camproj(球员。轴,“透视图”); camva（player.Axes，90）；坎波斯（player.Axes，[0]）；camtarget（player.Axes，[-1 0]）；%将变换设置为旋转5度θ= 5;R = [cosd(theta) sind(theta) 0 0 -sind(theta) cosd(theta) 0 0 0 1 0 0 0 1];rotateByTheta = rigid3d (R);为n=0:θ：359%按θ旋转点云ptCloud=pctransform（ptCloud，rotateByTheta）；%显示点云查看（播放器、云端）；暂停（0.05）终止终止

作用ptCloudProcessed = helperProcessPointCloud (ptCloud)%检查点云是否有组织isOrganized=~ismatrix（ptCloud.Location）；%如果点云已组织，请使用基于范围的泛洪填充算法%（segmentGroundFromLidarData）。否则，请使用平面配件。地面分段方法=[“planefit”，“rangefloodfill”]；方法=地基分段方法（同构+1）；如果方法==“planefit”%以参考法向量分段地面作为主平面%使用pcfitplane指向正z方向。有组织的%点云，考虑使用SexMeStudio Li DARDATA代替。最大距离=0.4；%仪表最大距离=5；%度refVector=[0,0,1]；%z方向[~，地面指数]=pcfitplane（ptCloud，maxDistance，refVector，maxAngDistance）；埃尔塞夫方法==“rangefloodfill”%使用基于范围的泛洪填充分割地面。地面指数=分段地面数据（ptCloud）；其他的错误(“预期方法为‘planefit’或‘rangefloodfill’”）终止在给定的传感器半径内，以点的形式分割自我车辆传感器位置=[0,0,0]；半径=3.5；EgoIndex=findNeighborsInRadius（ptCloud，传感器位置，半径）；%移除属于地面或车辆的点ptsToKeep=true（ptCloud.Count，1）；ptsToKeep（groundindex）=false；ptsToKeep（egoindex）=false；%如果点云已组织，请保留组织结构如果isOrganized ptCloudProcessed=选择（ptCloud，查找（ptstokep），“OutputSize”，“满”）;其他的ptCloudProcessed=select（ptCloud，find（ptstokep））；终止终止

作用tform=HelperComputeInitiatialestimateFromIMU（imuReadings，prevTform）%使用先前估计的转换初始化转换tform=prevTform；%如果没有IMU读数可用，则返回如果高度（imuReadings）<=1回来；终止%IMU方向读数报告为四元数，表示%到主体框架的旋转偏移。计算方向变化%间隔期间第一次和最后一次报告的IMU方向之间%激光雷达扫描的结果。q1=模拟方向（1）；q2=模拟方向（结束）；%通过以下公式计算第一个和最后一个IMU读数之间的旋转偏移：%-从q2机架旋转到车身机架%-从车身框架旋转到q1框架q=q1*conj（q2）；%转换为欧拉角yawPitchRoll =欧拉(q,“ZYX股票”，“点”）;放弃俯仰和滚转角度估计。只使用方向角估计%从IMU方向。yawPitchRoll（2:3）=0；%转换回旋转矩阵q=四元数（yawPitchRoll，“欧拉”，“ZYX股票”，“点”)；R=rotmat（q，“点”）;%使用计算旋转T形式T（1:3，1:3）=R'；终止

作用helperAddLegend（hAx、标签）给坐标轴添加一个图例hLegend=图例（hAx，标签{：}）；%设置文本颜色和字体大小hLegend.TextColor=[1]；hLegend.FontWeight=“大胆的”；终止

作用helperMakeFigurePublishFriendly（hFig）如果~isempty（hFig）和&isvalid（hFig）hFig.Handleviability=“回调”；终止终止