BriencedLoadDurl =.“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）；options=weboptions('暂停'，INF）;lidarfilename = datafolder +“lidarPointClouds.mat”；imuFileName = dataFolder +“imuorientation.mat”；gpsFileName = dataFolder +“gpsSequence.mat”；folderExists=exist（dataFolder，“dir”)；matfilesExist=exist（lidarFileName，'文件') & &存在(imuFileName'文件')......& &存在(gpsFileName'文件'）;如果~folderExists mkdir（dataFolder）；结束如果~matfiles disp(“下载lidarPointClouds。垫(613 MB)……”)websave（lidarFileName，baseDownloadURL）+“lidarPointClouds.mat”，选项）；显示('正在下载imuoritations.mat（1.2MB）…') websave(imuFileName, baseDownloadURL +“imuorientation.mat”，选项）；显示('正在下载gpsSequence.mat（3 KB）…') websave(gpsFileName, baseDownloadURL +“gpsSequence.mat”、选择);结束

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

来自Mat文件的％加载GPS序列数据=加载（gpsFileName）；gpsSequence=data.gpsSequence；%显示前几行GPS数据头部（GPS序列）

来自MAT文件的％加载IMU录制data =负载(imuFileName);imuOrientations = data.imuOrientations;%将IMU录音转换为四元数类型imuOrientations = convertvars (imuOrientations,'方向',“四元数”）;%显示IMU数据的前几行头(imuOrientations)

lidarFrameDuration =值(diff (lidarPointClouds.Time));gpsFrameDuration =值(diff (gpsSequence.Time));imuFrameDuration =值(diff (imuOrientations.Time));％调整显示格式到秒lidarFrameDuration。格式='s'；gpsFrameDuration.Format='s'；imuFrameDuration。格式='s'；计算帧速率Lidarrate = 1 /秒（Lidarframeduration）;GPSRate = 1 /秒（GPSFRAMERTION）;模仿= 1 /秒（IMUFREMARDURION）;%显示帧持续时间和速率fprintf('激光雷达：%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），imuRate；

％创建一个Geoplayer以可视化流地理坐标拉脱中心= gpssequence.latitude（1）;Loncenter = GPSSequence.Longitude（1）;zoomlevel = 17;gpsplayer = geoplayer（拉脱Center，LonCenter，Zoomlevel）;%画出完整的路线plotRoute (gpsPlayer gpsSequence。纬度,gpsSequence.Longitude);确定玩家的极限xlimits=[-45]；％仪表ylimits=[-45]；zlimits=[-1020]；％创建一个PCPlayer，以从LIDAR传感器可视化流点云lidarplayer = pcplayer（xlimits，ylimits，zlimits）;％自定义播放器轴标签xlabel（lidarPlayer.Axes，'x（m）') ylabel (lidarPlayer。轴,“Y (m)”)zlabel（lidarPlayer.Axes，'z（m）'）标题（Lidarplayer.axes，“激光雷达传感器数据”)%在屏幕上对齐玩家helperAlignPlayers ({gpsPlayer, lidarPlayer});通过GPS读数外回路(信号变慢)对于g = 1:高度(gpsSequence从时间表中提取地理坐标纬度= gpssequence.latitude（g）;经度= GPSSequence.Longitude（g）;%更新GPS显示中的当前位置绘图位置（gpsPlayer、纬度、经度）；%计算当前和下一个GPS读数之间的时间跨度时间跨度=时间范围(gpsSequence.Time(g)， gpsSequence.Time(g+1));％提取在此时间跨度期间记录的LIDAR帧lidarframes = lidarpointclouds（timespan，:);LIDAR读数％内循环（更快的信号）对于l=1：高度（激光雷达框架）％提取点云ptcloud = lidarframes.pointCloud（L）;％更新LIDAR显示查看（Lidarplayer，Ptcloud）;%暂停以降低显示速度暂停(0.01)结束结束

％隐藏玩家隐藏（gpsplayer）hide（lidarplayer）%选择一帧激光雷达数据以演示注册工作流frameNum=600；ptCloud=lidarPointClouds.PointCloud（frameNum）；%显示和旋转自我视图以显示激光雷达数据Helpervisualizeegoview（Ptcloud）;

skipFrames=10；frameNum=100；fixed=lidarPointClouds.PointClouds（frameNum）；moving=lidarPointClouds.PointClouds（frameNum+skipFrames）；

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

hfigfixed =图;PCShowpair（固定，固定处理）视图（2）;%调整视图以显示俯视图Helpermakefigurepublishfriency（HFigfixed）;在配准前对点云进行下采样。将采样提高％登记准确性和算法速度。downsamplePercent = 0.1;fixedDownsampled = pcdownsample (fixedProcessed,“随机”，沮丧地区）;移动= pcdownsample（移动处理，“随机”，沮丧地区）;

ReggridStep = 5;tform = pcregisterndt（移动的，固定下拉采样，reggridstep）;vishtreg = pctransform（移动处理，tform）;在注册之前和之后，％可视化对齐hFigAlign =图;subplot(121) pcshowpair(movingProcessed, fixedProcessed) title(之前注册的)视图（2）子地块（122）pcshowpair（movingReg，fixedProcessed）标题(“注册后”）查看（2）HelperMakeFigurepublishFriendly（Hfigalign）;

mergeGridStep = 0.5;ptCloudAccum = pcmerge(fixedProcessed, movingReg, mergeGridStep); / /将数据合并hFigAccum =图;pcshow (ptCloudAccum)标题(“累积点云”）查看（2）HelpermakeFigurepublishFriendly（HFigAccum）;

%创建地图生成器对象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读数origin = [gpsSequence.Latitude(1)， gpsSequence.Longitude(1)， gpsSequence.Altitude(1)];%将GPS读数转换为当地的东-北-北坐标系统[xEast, yNorth, zUp] = latlon2local(gpsSequence。纬度,gpsSequence。经度,......GPSSequence.altitude，Origin）;在轨迹开始时估计粗方向以对齐局部ENU%系统与激光雷达坐标系统Theta =中位数（atan2d（ynorth（1:15），Xeast（1:15）））;r = [cosd（90-θ）sind（90-θ）0;-sind（90-θ）cosd（90-θ）0;0 0 1];％旋转ENU坐标与LIDAR坐标系对齐groundTruthTrajectory = [xEast, yNorth, zUp] * R;

hold（mapbuilder.axes，“开”)散布（mapBuilder.Axes、地面轨迹（：，1）、地面轨迹（：，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读数。prevTime=lidarPointClouds.Time（n-skipFrames）；currTime=lidarPointClouds.Time（n）；TimesRunScan=timerange（prevTime，currTime）；imuReadings=imuOrientations（TimesRunScan，'方向'）;％使用IMU读数形成初始估计initTform = helperComputeInitialEstimateFromIMU(imureads, tform);结束%使用点云更新地图tform = updateMap(mapBuilder, ptCloud, initTform);%更新地图显示updateDisplay (mapBuilder closeDisplay);结束在新地图上叠加地面真实轨迹hold（mapbuilder.axes，“开”)散布（mapBuilder.Axes、地面轨迹（：，1）、地面轨迹（：，2），......“绿色”,“填充”）;Helperaddlegend（MapBuilder.axes，......{“地图点”,“预计轨迹”,'地面真理轨迹'}）;%捕获用于发布的快照snapnow;%未结数关闭([hFigFixed、hFigAlign hFigAccum]);updatedisplay（mapbuilder，true）;

功能helperAlignPlayers(球员)validateattributes(球员,{“细胞”}， {'向量'}）;hasAxes = cellfun (@ (p) isprop (p,“轴”),球员,'统一输出'， 真的）;如果〜所有（Lasaxes）错误（'预计所有观众都有一个轴属性'）;结束screensize = get（groot，“屏幕大小”）;screenmargin = [50,100];Playersize = Cellfun（@getPlayerSize，玩家，'统一输出'、假);playerSizes = cell2mat (playerSizes);maxHeightInSet = max (playerSizes(1:3:结束));％垂直安排玩家，以便最高的球员是100像素%顶部。location = round([screenMargin(1)， screenSize(4)-screenMargin(2)- maxheight set]);对于n = 1：numel（玩家）播放器=玩家{n};hfig =祖先（player.axes，“图”）;hFig.OuterPosition(1:2) =位置;%右转设置下一个位置location = location + [50+hFig.OuterPosition(3)， 0]; / /当前位置结束功能深圳= getPlayerSize(观众)%获取父图形容器h =祖先(查看器。轴,“图”）;sz = h.outerposition（3：4）;结束结束

功能Player = HelpervisualizeeGoview（PtCloud）%创建一个pcplayer对象xlimits = ptCloud.XLimits;ylimits = ptCloud.YLimits;zlimits = ptCloud.ZLimits;Player = pcplayer(xlimits, ylimits, zlimits);关闭轴线轴(球员。轴,“关闭”）;设置摄像机显示自我视图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-sind（θ）cosd（θ）0 0 0 0 0 1]；rotateByTheta=rigid3d（R）；对于n = 0：THETA：359％旋转点云θptCloud = pctransform(ptCloud, rotateByTheta);％显示点云查看（播放器，Ptcloud）;暂停（0.05）结束结束

功能ptCloudProcessed=helperProcessPointCloud（ptCloud）%检查点云是否已组织isOrganized = ~ ismatrix (ptCloud.Location);%如果点云是有组织的，使用基于范围的洪水填充算法％（SegmentGroundFromlidardata）。否则，使用平面配件。groundSegmentationMethods = [“planefit”,“洪水填充”];方法= groundSegmentationMethods (isOrganized + 1);如果方法==“planefit”%线段地面作为主导平面，参考法向量％指向正Z方向，使用PCFitplane。组织%点云，考虑使用segmentGroundFromLidarData代替。maxdistance = 0.4;％仪表maxangdistance = 5;%学位refVector = [0,0,1];％z方向[〜，groundindices] = pcfitplane（ptcloud，maxdistance，refvector，maxangdistance）;elsef.方法==“洪水填充”使用基于范围的洪水填充分割地面。groundindices = segmentgroundfromlidardata（ptcloud）;其他的错误（“预期的方法是‘planefit’或‘rangefloodfill’”)结束%将车辆分段为传感器给定半径内的点传感器位置= [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))，“输出大小”,“全部”）;其他的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 * conj(q2);%转换为欧拉角yawPitchRoll=欧拉（q，“ZYX”,“点”）;%放弃俯仰角和横滚角估计值。仅使用航向角估计值IMU方向的％。yawpectroll（2：3）= 0;转换回旋转矩阵q =四元数(yawPitchRoll,'euler',“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.HandLevisibility =“回调”；结束结束