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”；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, baseDownloadURL +“iMuoOrientations.mat”， 选项）;DISP（'下载GPSSequence.mat（3 KB）......') websave(gpsFileName, baseDownloadURL +“gpssequence.mat”， 选项）;结束

从MAT-file加载激光雷达数据data = load（lidarfilename）;lidarpointclouds = data.lidarpointclouds;％显示前几行的LIDAR数据头（Lidarpointclouds）

％加载来自Mat文件的GPS序列data = load（gpsfilename）;gpssequence = data.gpssequence;%显示前几行GPS数据头（GPSSequence）

来自Mat文件的％加载IMU录制数据=加载（imuFileName）；imuOrientations=数据。imuOrientations；％将IMU录制转换为四元数类型imuOrientations = convertvars (imuOrientations,'方向'那'四元碘'）;％显示前几行的IMU数据头(imuOrientations)

lidarFrameDuration=中值（diff（lidarPointClouds.Time））；gpsFrameDuration=中值（diff（gpsSequence.Time））；imuFrameDuration=中值（diff（imuOrientations.Time））；%将显示格式调整为秒lidarframeduration.format ='；gpsframeduration.format ='；imuframeduration.format ='；％计算帧速率Lidarrate = 1 /秒（Lidarframeduration）;gpsrate = 1 /秒（gpsframedration）;模仿= 1 /秒（IMUFRAMERSURION）;％显示框架持续时间和速率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），刺激）;

%创建地质图层以可视化流式地理坐标拉脱中心= gpssequence.latitude（1）;Loncenter = GPSSequence.Longitude（1）;zoomlevel = 17;gpsplayer = geoplayer（拉脱Center，LonCenter，Zoomlevel）;％绘制完整的路线plotRoute (gpsPlayer gpsSequence。纬度,gpsSequence.Longitude);确定玩家的极限xlimits = [-45 45];%仪表ylimits = [-45 45];zlimits = [-10 20];%创建一个pcplayer以可视化来自激光雷达传感器的流点云lidarPlayer=pcplayer（xlimits、ylimits、zlimits）；％自定义播放器轴标签xlabel（lidarplayer.axes，'x（m）') ylabel (lidarPlayer。轴,'y（m）'）zlabel（lidarplayer.axes，‘Z（m）’）标题（LidarPlayer.axes，'LIDAR传感器数据'）%在屏幕上对齐玩家HelperalignPlayers（{GPSPlayer，LidarPlayer}）;通过GPS读数外回路(信号变慢)为了g = 1:高度(gpsSequence％从时间表提取地理坐标纬度=gpsSequence.纬度（g）；经度=gpsSequence.经度（g）；％更新GPS显示中的当前位置绘图（GPSPlayer，纬度，经度）;％计算当前和下一个GPS读取的时间跨度时间跨度=时间范围(gpsSequence.Time(g)， gpsSequence.Time(g+1));％提取在此时间跨度期间记录的LIDAR帧lidarFrames=lidarPointClouds（时间跨度：）；%内环激光雷达读数（更快的信号）为了l = 1：高度（Lidarframes）%提取点云ptcloud = lidarframes.pointCloud（L）;%更新激光雷达显示器视图（lidarPlayer、ptCloud）；％暂停以减慢显示屏暂停（0.01）结束结束

%隐藏玩家隐藏（gpsPlayer）隐藏（lidarPlayer）％选择LIDAR数据帧以演示注册工作流程Framenum = 600;ptcloud = lidarpointclouds.pointcloud（framenum）;％显示和旋转自我视图以显示LIDAR数据Helpervisualizeegoview（Ptcloud）;

skipframes = 10;Framenum = 100;固定= lidarpointclouds.pointCloud（Framenum）;移动= lidarpointclouds.pointcloud（framenum + skipframes）;

固定处理= HelperProcessPointCloud（固定）;移动处理= HelperProcessPointCloud（移动）;

hFigFixed=图；pcshowpair（固定，固定处理）视图（2）；％调整视图以显示顶视图HelpermakeFigurepublishfriendly（HFigfixed）;在配准前对点云进行下采样。将采样提高%配准精度和算法速度。downsamplePercent = 0.1;fixedDownsampled = pcdownsample (fixedProcessed,'随机的'，下扫描）;移动= pcdownsample（移动处理，'随机的'，下扫描）;

regGridStep=5；tform=pcregisterndt（移动下采样、固定下采样、regGridStep）；movingReg=pctransform（movingProcessed，tform）；在注册之前和之后的尾视图中的％可视化对齐hFigAlign=图；子批次（121）pcshowpair（移动处理、固定处理）标题('注册前'）查看（2）子平台（122）PCShowpair（voldeReg，固定处理）标题（'注册后'）查看（2）HelpermakeFigurepublishFriendly（Hfigalign）;

mergeGridStep = 0.5;ptCloudAccum = pcmerge(fixedProcessed, movingReg, mergeGridStep); / /将数据合并hFigAccum =图;pcshow (ptCloudAccum)标题('累积点云')视图（2）helperMakeFigurePublishFriendly（hFigAccum）；

％创建Map Builder对象MapBuilder = HelperlidarmapBuilder（“DownsamplePercent”，下扫描）;％设置随机数种子RNG（0）;cliftisplay = false;numframes = height（lidarpointclouds）;tform = rigid3d;为了n=1:skipFrames:numFrames-skipFrames％得到第n点云ptCloud=lidarPointClouds.PointCloud（n）；使用来自先前迭代的转换作为初始估计点云注册点的百分比迭代。（恒速）inittform = tform;使用点云更新映射tform = updateMap(mapBuilder, ptCloud, initTform);％更新映射显示updateDisplay (mapBuilder closeDisplay);结束

％选择参考点作为第一个GPS读数原点=[gpsSequence.纬度（1）、gpsSequence.经度（1）、gpsSequence.海拔（1）]；%将GPS读数转换为当地的东-北-北坐标系统[xEast, yNorth, zUp] = latlon2local(gpsSequence。纬度,gpsSequence。经度,...GPSSequence.altitud，Origin）;在轨迹开始时估计粗方向以对齐局部ENULIDAR坐标系的％系统Theta =中位数（atan2d（ynorth（1:15），Xeast（1:15）））;r = [cosd（90-theta）sind（90-θ）0;-sind（90-θ）cosd（90-θ）0;0 0 1];％旋转ENU坐标以与LIDAR坐标系对齐Troundtruthtrajectory = xeast，ynorth，zup] * r;

hold（mapbuilder.axes，'在'）分散（MapBuilder.Axes，TountTreTrajectory（：，1），地面Truthtrajectory（:,2），...“绿色”那'填充'）;Helperaddlegend（MapBuilder.axes，...{'地图积分'那'估计轨迹'那“地面真相轨道”});

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

％重置Map Builder以清除以前构建的地图重置(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 = helperComputeInitialEstimateFromIMU(imureads, tform);结束使用点云更新映射tform = updateMap(mapBuilder, ptCloud, initTform);％更新映射显示updateDisplay (mapBuilder closeDisplay);结束%在新地图上叠加地面真实轨迹hold（mapbuilder.axes，'在'）分散（MapBuilder.Axes，TountTreTrajectory（：，1），地面Truthtrajectory（:,2），...“绿色”那'填充'）;Helperaddlegend（MapBuilder.axes，...{'地图积分'那'估计轨迹'那“地面真相轨道”});%捕获用于发布的快照snapnow;％近距离关闭（[Hfigfixed，Hfigalign，Hfigaccum]）;updateDisplay（mapBuilder，true）；

功能helperAlignPlayers(球员)validateattributes(球员,{“细胞”}, {'向量'});Hasaxes = Cellfun（@（p）是isprop（p，“斧头”),球员,'统一输出'， 真的）;如果〜所有（Hasaxes）错误（'预计所有观众都有一个轴属性'）;结束screensize = get（groot，'屏幕尺寸'); 屏幕边距=[50100]；PlayerSize=cellfun（@getPlayerSize，玩家，'统一输出'， 错误的）;Playersize = Cell2mat（Playersizes）;maxheightinset = max（Playersize（1：3：结束））;％垂直安排玩家，以便最高的球员是100像素％ 顶端。位置=圆形（[屏幕边距（1），屏幕尺寸（4）-屏幕边距（2）-最大高度插入]）；为了n=1:numel（players）player=players{n}；hFig=祖先（player.Axes，'数字'）;hFig.OuterPosition(1:2) =位置;右转，％设置下一个位置location = location + [50+hFig.OuterPosition(3)， 0]; / /当前位置结束功能sz=getPlayerSize（查看器）％获取父数据容器h =祖先（查看者.Axes，'数字'）;sz = h.outerposition（3：4）;结束结束

功能Player = HelpervisualizeeGoview（Ptcloud）%创建一个pcplayer对象xlimits = ptCloud.XLimits;ylimits = ptCloud.YLimits;zlimits = ptCloud.ZLimits;Player = pcplayer(xlimits, ylimits, zlimits);％关掉轴线轴（Player.Axes，'离开'）;％设置相机以显示自我视图Camproj（Player.Axes，“视角”）;Camva（Player.axes，90）;Campos（Player.axes，[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）;％显示点云查看（播放器、云端）；暂停（0.05）结束结束

功能ptcloudprocessed = helperprocesspointcloud（ptcloud）％检查点云是否被组织IsOrganized =〜Ismatrix（Ptcloud.Location）;%如果点云已组织，请使用基于范围的泛洪填充算法％（SegentgegroundFromlidardata）。否则，使用平面拟合。groundSegmentationMethods = [“平面图”那“RangeFloodfill”];方法= GroundSegationMethods（IsOrganized + 1）;如果方法==.“平面图”%线段地面作为主导平面，参考法向量％指向正Z方向，使用PCFitplane。组织%点云，考虑使用SexMeStudio Li DARDATA代替。maxdistance = 0.4;%仪表maxangdistance = 5;％程度refVector = [0,0,1];％z方向[〜，groundindices] = pcfitplane（ptcloud，maxdistance，refvector，maxangdistance）;埃尔塞夫方法==.“RangeFloodfill”使用基于范围的洪水填充分割地面。Groundindices = SegmentgroundFromLIDARDATA（PTCLOUD）;其他的错误（“预期的方法是‘planefit’或‘rangefloodfill’”）结束％段EGO车辆作为给定的传感器半径内的点SensorLocation = [0,0,0];半径= 3.5;egoindices = findneighborsinradius（ptcloud，sensorlocation，半径）;%移除属于地面或车辆的点ptstokeep = true（ptcloud.count，1）;ptstokeep（groundindices）= false;ptstokeep（egoindices）= false;%如果点云已组织，请保留组织结构如果isOrganized ptCloudProcessed = select(ptCloud, find(ptsToKeep))，'输出'那'满的'）;其他的ptcloudprocessed = select（ptcloud，find（ptstokeep））;结束结束

功能tform = helpercomputeinitialestimatefromimu（imureadings，prevtform）%使用先前估计的转换初始化转换tform = prevTform;%如果没有IMU读数可用，则返回如果高度（imuReadings）<=1返回；结束％IMU定向读数被报告为代表的四季度到车身框架的旋转偏移%。计算方向变化%间隔期间第一次和最后一次报告的IMU方向之间%的激光雷达扫描。Q1 = Imureadings.Oorientation（1）;Q2 = Imureadings.Oorientation（END）;％计算第一和最后一个IMU之间的旋转偏移％ - 从Q2帧旋转到车身框架%-从车身框架旋转到q1框架Q = Q1 *结合（Q2）;％转换为欧拉角yawpitchroll = euler（q，'Zyx'那“点”）;％丢弃音调和滚角估计。仅使用标题角度估计从IMU方向百分比。yawpectroll（2：3）= 0;转换回旋转矩阵q =四元数(yawPitchRoll,“欧拉”那'Zyx'那“点”）;r = rotmat（q，“点”）;使用计算旋转tform。T(1:3, 1:3) = r ';结束

功能Helperaddlegend（HAX，标签）％向轴添加一个图例hLegend = legend(hAx, labels{:});％设置文本颜色和字体重量hLegend。TextColor = [1 1 1];hLegend。FontWeight ='胆大'；结束

功能HelpermakeFigurepublishFriendly（HFIG）如果~isempty（hFig）和&isvalid（hFig）hFig.Handleviability='打回来'；结束结束