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 (“超时”，Inf）；lidarFileName=dataFolder+“lidarPointClouds.mat”；imuFileName=dataFolder+“imuOrientations.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+“imuOrientations.mat”、选择);disp (“下载gpsSequence。垫(3 KB)……”)websave（gpsFileName，baseDownloadURL+“gpsSequence.mat”、选择);终止

%从MAT文件加载激光雷达数据data =负载(lidarFileName);lidarPointClouds = data.lidarPointClouds;%显示前几行激光雷达数据头(lidarPointClouds)

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

从mat文件加载IMU录音data =负载(imuFileName);imuOrientations = data.imuOrientations;%将IMU录制转换为四元数类型imuOrientations=转换器变量（imuOrientations，“定位”，“四元数”）;%显示IMU数据的前几行头部（定向）

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

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

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

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

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

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

regGridStep = 5;tform = pcregisterndt(movingdownsampling, fixeddownsampling, 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 (“降采样率”, downsamplePercent);%设置随机数种子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.经度，．．.gpsSequence。高度，origin);%估计轨迹开始时的大致方向，以对齐局部轨迹%激光雷达坐标系theta =中值(atan2d(yNorth(1:15)， xEast(1:15)));R = [cosd(90-theta) sind(90-theta) 0;信德(90 -θ)cosd(90 -θ)0;0 0 1);旋转ENU坐标与激光雷达坐标系统对齐地面轨迹=[xEast，yNorth，zUp]*R；

(mapBuilder。轴,“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),．．.“绿色”，“填充”）;helperAddLegend (mapBuilder。轴,．．.｛“地图点”，“估计轨迹”，“地面实况轨迹”}）;

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

重置地图生成器以清除先前生成的地图重置（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。轴,“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),．．.“绿色”，“填充”）;helperAddLegend (mapBuilder。轴,．．.｛“地图点”，“估计轨迹”，“地面实况轨迹”}）;%捕获用于发布的快照snapnow;%关闭打开数字关闭（[hFigFixed，hFigAlign，hFigAccum]）；更新显示（mapBuilder，true）；

函数HelperAlignPlayer（玩家）验证属性（玩家、{“细胞”}， {“向量”})hasAxes=cellfun（p）isprop（p，“轴”)，玩家，“UniformOutput”,真正的);如果~ (hasAxes)错误(“希望所有观众都能拥有Axes属性”）;终止拉=得到(大的,“拉”）;screenMargin = [50,100];cellfun(@getPlayerSize, players，“UniformOutput”、假);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.外部位置（1:2）=位置；向右设置下一个位置位置=位置+[50+hFig.外部位置（3），0]；终止函数sz=getPlayerSize（查看器）%获取父图形容器h=祖先（viewer.Axes，“数字”）;深圳= h.OuterPosition (3:4);终止终止

函数球员= helperVisualizeEgoView (ptCloud)%创建pcplayer对象xlimits=ptCloud.xlimits；ylimits=ptCloud.ylimits；zlimits=ptCloud.zlimits；player=pcplayer（xlimits，ylimits，zlimits）；%关闭轴线轴（player.Axes，“关”）;%设置摄像头以显示自我视图camproj(球员。轴,“透视图”）;camva(球员。轴,90);坎波斯(球员。轴,[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)。否则，使用平面配件。地面分段方法=[“planefit”，“rangefloodfill”];方法= groundSegmentationMethods (isOrganized + 1);如果方法= =“planefit”%以参考法向量分段地面作为主平面%指向正z方向，使用pcfitplane。为组织%点云，考虑使用segmentGroundFromLidarData代替。maxDistance = 0.4;%米maxAngDistance = 5;%度refVector=[0,0,1]；% z方向[~， grounddinices] = pcfitplane(ptCloud, maxDistance, refVector, maxAngDistance);elseif方法= =“rangefloodfill”%使用基于范围的泛洪填充分割地面。groundIndices = segmentGroundFromLidarData (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 = helperComputeInitialEstimateFromIMU(imureads, prevTform)%使用先前估计的转换初始化转换tform=prevTform；%如果没有IMU读数，返回如果高度(imuReadings) < = 1返回；终止% IMU方向读数报告为四元数表示%到主体框架的旋转偏移。计算方向变化%在间隔期间第一个和最后一个报告的IMU方向之间%激光雷达扫描的结果。q1 = imuReadings.Orientation (1);q2 = imuReadings.Orientation(结束);计算第一次和最后一次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）如果~是空的(hFig) &&是有效的(hFig)HandleVisibility =“回调”；终止终止