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 =存在(dataFolder,“dir”）;matfilesExist =存在(lidarFileName,'文件') & &存在(imuFileName'文件'）．..& &存在(gpsFileName'文件'）;如果~ folderExists mkdir (dataFolder);结束如果~ matfilesExist disp (“下载lidarPointClouds。垫(613 MB)…') websave(lidarFileName, baseDownloadURL +“lidarPointClouds.mat”、选择);disp (“下载imuOrientations。垫(1.2 MB)……”) websave(imuFileName, baseDownloadURL +“imuOrientations.mat”、选择);disp (“下载gpsSequence。垫(3 KB)……”) websave(gpsFileName, baseDownloadURL +“gpsSequence.mat”、选择);结束

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

来自Mat文件的％加载GPS序列data =负载(gpsFileName);gpsSequence = data.gpsSequence;%显示前几行GPS数据头(gpsSequence)

来自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。格式=“年代”；gpsFrameDuration。格式=“年代”；imuFrameDuration。格式=“年代”；计算帧速率Lidarrate = 1 /秒（Lidarframeduration）;GPSRate = 1 /秒（GPSFRAMERTION）;模仿= 1 /秒（IMUFREMARDURION）;%显示帧持续时间和速率流('激光雷达:%s， %3.1f Hz\n'char (lidarFrameDuration), lidarRate);流('GPS: %s， %3.1f Hz\n'char (gpsFrameDuration), gpsRate);流('IMU: %s， %3.1f Hz\n'char (imuFrameDuration), imuRate);

%创建一个地理播放器来可视化流地理坐标拉脱中心= 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];%创建一个pc播放器从激光雷达传感器可视化流点云lidarPlayer = pcplayer(xlimits, ylimits, zlimits);％自定义播放器轴标签包含(lidarPlayer。轴,'x（m）') ylabel (lidarPlayer。轴,“Y (m)”) zlabel (lidarPlayer。轴,“Z (m)”）标题（Lidarplayer.axes，“激光雷达传感器数据”）%在屏幕上对齐玩家helperAlignPlayers ({gpsPlayer, lidarPlayer});通过GPS读数外回路(信号变慢)为g = 1:高度(gpsSequence从时间表中提取地理坐标纬度= gpsSequence.Latitude (g);经度= gpsSequence.Longitude (g);%更新当前位置在GPS显示plotPosition (gpsPlayer,经度和纬度);%计算当前和下一次GPS读取之间的时间跨度时间跨度=时间范围(gpsSequence.Time(g)， gpsSequence.Time(g+1));％提取在此时间跨度期间记录的LIDAR帧lidarFrames = lidarPointClouds(timeSpan，:);激光雷达读数(更快的信号)为l = 1:高度(lidarFrames)提取点云ptcloud = lidarframes.pointCloud（L）;更新激光雷达显示视图(lidarPlayer ptCloud);%暂停以减慢显示速度暂停(0.01)结束结束

%隐藏球员隐藏(gpsPlayer)隐藏(lidarPlayer)%选择一帧激光雷达数据来演示注册流程frameNum = 600;ptCloud = lidarPointClouds.PointCloud (frameNum);%显示和旋转自我视图以显示激光雷达数据Helpervisualizeegoview（Ptcloud）;

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

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

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

regGridStep = 5;tform = pcregisterndt(movingdownsampling, fixeddownsampling, regGridStep); / /指定采样时间movingReg = pctransform(movingProcessed, tform);在注册之前和之后，％可视化对齐hFigAlign =图;subplot(121) pcshowpair(movingProcessed, fixedProcessed) title(之前注册的) view(2) subplot(122) pcshowpair(movingReg, fixedProcessed) title(注册后的）查看（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。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),．..“绿色”，“填充”）;Helperaddlegend（MapBuilder.axes，．..｛“地图点”，“估计轨迹”，“地面实况轨迹”}）;

%关闭地图显示updateDisplay (mapBuilder,真实);

％重置Map Builder以清除以前构建的地图重置(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 = helperComputeInitialEstimateFromIMU(imureads, tform);结束%使用点云更新地图tform = updateMap(mapBuilder, ptCloud, initTform);%更新地图显示updateDisplay (mapBuilder closeDisplay);结束在新地图上叠加地面真实轨迹hold（mapbuilder.axes，“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),．..“绿色”，“填充”）;Helperaddlegend（MapBuilder.axes，．..｛“地图点”，“估计轨迹”，“地面实况轨迹”}）;%捕获用于发布的快照snapnow;%关闭打开数字关闭([hFigFixed、hFigAlign hFigAccum]);updateDisplay (mapBuilder,真实);

功能helperAlignPlayers(球员)validateattributes(球员,{“细胞”}， {'向量'}）;hasAxes = cellfun (@ (p) isprop (p,“轴”),球员,'统一输出',真正的);如果〜所有（Lasaxes）错误（'预计所有观众都有一个轴属性'）;结束screensize = get（groot，“拉”）;screenMargin = [50,100];cellfun(@getPlayerSize, players，'统一输出'、假);playerSizes = cell2mat (playerSizes);maxHeightInSet = max (playerSizes(1:3:结束));％垂直安排玩家，以便最高的球员是100像素%。location = round([screenMargin(1)， screenSize(4)-screenMargin(2)- maxheight set]);为N = 1: numel(players) player = players{N};hFig =祖先(球员。轴,“图”）;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(球员。轴,“视角”）;Camva（Player.Axes，90）;Campos（Player.axes，[0 0 0]）;Camtarget（Player.axes，[-1 0 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：THETA：359％旋转点云θptCloud = pctransform(ptCloud, rotateByTheta);％显示点云视图(球员,ptCloud);暂停(0.05)结束结束

功能ptCloudProcessed = helperProcessPointCloud (ptCloud)%检查点云是否有组织isOrganized = ~ ismatrix (ptCloud.Location);%如果点云是有组织的，使用基于范围的洪水填充算法％（SegmentGroundFromlidardata）。否则，使用平面配件。groundSegmentationMethods = [“planefit”，“rangefloodfill”];方法= groundSegmentationMethods (isOrganized + 1);如果方法==.“planefit”%线段地面作为主导平面，参考法向量％指向正Z方向，使用PCFitplane。组织%点云，考虑使用segmentGroundFromLidarData代替。maxdistance = 0.4;%米maxangdistance = 5;%度refVector = [0,0,1];％z方向[〜，groundindices] = pcfitplane（ptcloud，maxdistance，refvector，maxangdistance）;elseif方法==.“rangefloodfill”使用基于范围的洪水填充分割地面。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))，“OutputSize”，“全部”）;其他的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,“欧拉”，“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)如果~是空的(hFig) &&是有效的(hFig)HandleVisibility =“回调”；结束结束