baseDownloadURL =“https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip”；dataFolder = fullfile (tempdir,“kit_velodyneslam_data_scenario1”, filesep);选择= weboptions (“超时”，Inf）；zipFileName=dataFolder+“scenario1.zip”；%获取scenario1文件夹中PNG文件的完整文件路径。pointCloudFilePattern = fullfile (dataFolder,“场景1”,“扫描* . png”）;numExpectedFiles = 2513;folderExists =存在(dataFolder,“dir”）;如果~ folderExists%在临时目录中创建一个文件夹来保存下载的zip文件%的文件。mkdir（数据文件夹）；disp('正在下载scenario1.zip（153 MB）…') websave(zipFileName, baseDownloadURL，选项);解压下载的文件解压缩(zipFileName dataFolder);elseiffolderExists&&numel（dir（pointCloudFilePattern））%如果在临时目录中减少了数据，请重新下载。disp ('正在下载scenario1.zip（153 MB）…') websave(zipFileName, baseDownloadURL，选项);%解压缩下载的文件。解压缩(zipFileName dataFolder)结束

datasetTable = helperReadDataset(dataFolder, pointcloudfileppattern);pointCloudTable = datasetTable(:， 1);insDataTable = datasetTable(:， 2:end); / /数据表

ptCloud = helperReadPointCloudFromFile (pointCloudTable.PointCloudFileName {1});disp (ptCloud)

: disp (insDataTable (1))

%指定玩家的限制Xlimits = [-45 45];%米ylimits=[-45]；zlimits=[-1020]；%创建流式点云显示对象lidarPlayer=pcplayer（xlimits、ylimits、zlimits）；%自定义播放器轴标签xlabel（lidarPlayer.Axes，“X (m)”) ylabel (lidarPlayer。轴,“Y (m)”) zlabel (lidarPlayer。轴,“Z (m)”)标题(lidarPlayer。轴,“激光雷达传感器数据”)跳过每一帧，因为这是一个长序列skipFrames=2；numFrames=height（pointCloudTable）；为n = 1: skipFrames: numFrames读点云fileName=pointCloudTable.PointCloudFileName{n}；ptCloud=helperReadPointCloudFromFile（fileName）；%可视化点云视图(lidarPlayer ptCloud);暂停(0.01)结束

隐藏（lidarPlayer）%设置随机种子以确保重复性rng (0);创建一个空视图集vSet = pcviewset;创建一个图形用于视图集显示hFigBefore =图(“姓名”,“视图设置显示”）;hAxBefore =轴(hFigBefore);%初始化点云处理参数downsamplePercent = 0.1;regGridSize = 3;%初始化转换absTform=rigid3d；%对参考系的绝对变换relTform = rigid3d;%连续扫描之间的相对转换viewId = 1;skipFrames = 5;numFrames =身高(pointCloudTable);displayRate = 100;%每100帧更新一次为n = 1: skipFrames: numFrames读点云文件名= pointCloudTable.PointCloudFileName {n};ptCloudOrig = helperReadPointCloudFromFile(文件名);%过程点云% -切割并拆除接地面%-分割并移除车辆ptCloud = helperProcessPointCloud (ptCloudOrig);%向下采样处理的点云ptCloud = pcdownsample (ptCloud,“随机”, downsamplePercent);firstFrame = (n = = 1);如果firstFrame%将第一次点云扫描作为视图添加到视图集中vSet=添加视图（vSet、viewId、absTform、，“点云”, ptCloudOrig);viewId = viewId + 1;ptCloudPrev = ptCloud;继续；结束%使用INS估计注册的初始转换initTform=helperComputeInitialEstimateFromINS（relTform，．..insDataTable (n-skipFrames: n));%计算注册当前点云的刚性转换%前点云relTform=pcregisterndt（ptCloud、ptCloudPrev、regGridSize、，．..“初始化转换”, initTform);%更新绝对变换到参考帧(第一个点云)absTform = rigid3d(relTform。T * absTform。T);%将当前点云扫描作为视图添加到视图集vSet=添加视图（vSet、viewId、absTform、，“点云”, ptCloudOrig);%添加从上一个视图到当前视图的连接，表示%它们之间的相对变换vSet = addConnection(vSet, viewId-1, viewId, relTform);viewId=viewId+1；ptCloudPrev=ptCloud；initTform=relTform；如果n> 1&&mod（n，显示速率）==1个绘图（vSet，“父”，hAxBefore）；刷新屏幕使现代化结束结束

%显示前几个视图和连接头(vSet.Views)头(vSet.Connections)

ptClouds=vSet.Views.PointCloud；absPoses=vSet.Views.AbsolutePose；mapGridSize=0.2；ptCloudMap=pcalign（ptClouds，absPoses，mapGridSize）；

持有(hAxBefore“开”); pcshow（ptCloudMap）；保持（hAxBefore，“关闭”）;关上(hAxBefore.Parent)

%设置随机种子以确保重复性rng (0);创建一个空视图集vSet = pcviewset;%创建一个环路闭合检测器loopDetector = scanContextLoopDetector;创建一个图形用于视图集显示hFigBefore =图(“姓名”,“视图设置显示”）;hAxBefore =轴(hFigBefore);%初始化转换absTform=rigid3d；%对参考系的绝对变换relTform = rigid3d;%连续扫描之间的相对转换maxTolerableRMSE = 3;允许接受一个循环闭包候选的最大RMSEviewId = 1;为n = 1: skipFrames: numFrames读点云文件名= pointCloudTable.PointCloudFileName {n};ptCloudOrig = helperReadPointCloudFromFile(文件名);%过程点云% -切割并拆除接地面%-分割并移除车辆ptCloud = helperProcessPointCloud (ptCloudOrig);%向下采样处理的点云ptCloud = pcdownsample (ptCloud,“随机”, downsamplePercent);firstFrame = (n = = 1);如果firstFrame%将第一次点云扫描作为视图添加到视图集中vSet=添加视图（vSet、viewId、absTform、，“点云”, ptCloudOrig);%从第一个点云提取扫描上下文描述符描述符= scanContextDescriptor (ptCloudOrig);%将第一个描述符添加到循环闭包检测器addDescriptor（loopDetector、viewId、descriptor）viewId=viewId+1；ptCloudPrev=ptCloud；继续；结束%使用INS估计注册的初始转换initTform=helperComputeInitialEstimateFromINS（relTform，．..insDataTable (n-skipFrames: n));%计算注册当前点云的刚性转换%前点云relTform=pcregisterndt（ptCloud、ptCloudPrev、regGridSize、，．..“初始化转换”, initTform);%更新绝对变换到参考帧(第一个点云)absTform = rigid3d(relTform。T * absTform。T);%将当前点云扫描作为视图添加到视图集vSet=添加视图（vSet、viewId、absTform、，“点云”, ptCloudOrig);%添加一个从前一个视图到当前视图的连接%它们之间的相对变换vSet = addConnection(vSet, viewId-1, viewId, relTform);%从点云中提取扫描上下文描述符描述符= scanContextDescriptor (ptCloudOrig);将描述符添加到循环闭包检测器中addDescriptor (loopDetector viewId描述符)%检测候选循环闭包loopViewId = detectLoop (loopDetector);%找到了一个循环候选项如果~isempty（loopViewId）loopViewId=loopViewId（1）；从视图集检索点云loopView = findView(vSet, loopViewId);ptCloudOrig = loopView.PointCloud;%过程点云ptCloudOld = helperProcessPointCloud (ptCloudOrig);下采样点云ptCloudOld = pcdownsample (ptCloudOld,“随机”, downsamplePercent);%使用注册来估计相对姿势[relTform， ~， rmse] = pcregisterndt(ptCloud, ptCloudOld，．..regGridSize,“最大迭代次数”, 50);acceptLoopClosure = rmse <= maxTolerableRMSE;如果接受环闭合为简便起见，使用一个常数，小的信息矩阵%环闭合边infoMat = 0.01 * eye(6);添加一个与循环闭包相对应的连接vSet=addConnection（vSet、loopViewId、viewId、relTform、infoMat）；结束结束viewId=viewId+1；ptCloudPrev=ptCloud；initTform=relTform；如果n>1 && mod(n, displayRate) == 1 hG = plot(vSet，“父”，hAxBefore）；刷新屏幕使现代化结束结束

G = createPoseGraph (vSet);disp (G)

%更新轴限制将重点放在循环闭包连接上xlim（hAxBefore，[-5050]）；ylim（hAxBefore，[-10050]）；找到并突出显示循环闭包连接loopEdgeIds =找到(abs (diff (G.Edges。EndNodes, 1, 2)) > 1);突出(hG,“边缘”loopEdgeIds,“EdgeColor”,“红色”,“线宽”, 3)

optimG = optimizePoseGraph (G,“g2o-levenberg-marquardt”); vSetOptim=updateView（vSet，optimG.Nodes）；

绘图（vSetOptim，“父”，hAxBefore）

mapGridSize = 0.2;ptClouds = vSetOptim.Views.PointCloud;absPoses = vSetOptim.Views.AbsolutePose;ptCloudMap = pcalign(ptClouds, abspses, mapGridSize);hFigAfter =图(“姓名”,“视图设置显示(优化后)”）;hAxAfter =轴(hFigAfter);pcshow (ptCloudMap“父”, hAxAfter);%覆盖视图集显示持有在绘图（vSetOptim，“父”, hAxAfter);helperMakeFigurePublishFriendly (hFigAfter);

函数datasetTable = helperReadDataset(dataFolder, pointcloudfileppattern)%helperReadDataset将Velodyne SLAM数据集读取到时间表中%datasetTable=helperReadDataset（dataFolder）从将dataFolder中指定的%文件夹放入时间表中。这个函数%期望来自Velodyne SLAM数据集的数据。%%另请参见文件数据存储，helperReadINSConfigFile。%创建文件数据存储以按正确的顺序读取文件fileDS=fileDatastore（pointCloudFilePattern，“ReadFcn”,．..@helperReadPointCloudFromFile);%从数据存储中提取文件列表pointCloudFiles = fileDS.Files;imuConfigFile = fullfile (dataFolder,“场景1”,“imu.cfg”）;insDataTable = helperReadINSConfigFile (imuConfigFile);%从INS配置文件中删除坏行indatatable (1447，:) = [];%删除不使用的列datasetTable=removevars（insDataTable，．..{“Num_Satellites”,“纬度”,“经度”,“高度”,“欧米茄”标题,．..“欧米茄沥青”,“欧米茄卷”,“V_X”,“V_Y”,“V_ZDown”}）;datasetTable = addvars(datasetTable, pointCloudFiles，“之前”1.．..“NewVariableNames”,“PointCloudFileName”）;结束

函数ptCloud=helperProcessPointCloud（ptCloudIn，方法）%helperProcessPointCloud处理pointCloud移除地面和自我车辆%ptCloud=helperProcessPointCloud（ptCloudIn，方法）进程% ptCloudIn通过移除地平面和自我飞行器。%方法可以是“planefit”或“rangefloodfill”。%%参见pcfitplane, pointCloud/findNeighborsInRadius。参数ptCloudIn（1,1）点云方法字符串{mustBeMember（方法，[“planefit”，“rangefloodfill”]）}=“洪水填充”结束isOrganized = ~ ismatrix (ptCloudIn.Location);如果（方法）==“洪水填充”& & isOrganized)%分割地面使用洪水填充的范围图像groundFixedIdx=分段groundfromLidardata（ptCloudIn，．..“ElevationAngleDelta”11);其他的以参考法线为主导平面的线段地面指向正z方向的%矢量maxDistance = 0.4;maxAngularDistance = 5;referenceVector = [0 0 1];[~， groundFixedIdx] = pcfitplane(ptCloudIn, maxDistance，．..参考向量，最大角度距离）；结束如果isOrganized groundFixed = false(size(ptCloudIn.Location,1)，size(ptCloudIn.Location,2));其他的(ptCloudIn groundFixed = false。统计,1);结束groundFixed（groundFixedIdx）=真；%将车辆分段为传感器给定半径内的点传感器位置= [0 0 0];半径= 3.5;egoFixedIdx = findNeighborsInRadius(ptCloudIn, sensorLocation, radius);如果isOrganized egoFixed = false(size(ptCloudIn.Location,1)，size(ptCloudIn.Location,2));其他的(ptCloudIn egoFixed = false。统计,1);结束egoFixed (egoFixedIdx) = true;%保留无地面和自我车辆的点云子集如果isOrganized indices = ~groundFixed & ~egoFixed;其他的索引= find(~groundFixed & ~egoFixed);结束ptCloud = select(ptCloudIn, indices);结束

函数initTform=helpercomputeinitialestimatefromfromins（initTform，insData）%如果没有可用的INS读数，返回如果isempty (insData)返回；结束%INS读数的X指向前方，Y指向左侧%和Z向上。下面的翻译说明了转换为激光雷达%框架。insToLidarOffset=[0-0.79-1.73]；%请参阅DATAFORMAT.txtTnow=[-insData.Y（结束）、insData.X（结束）、insData.Z（结束）].+INSTOLIDARDOFFSET'；Tbef=[-insData.Y（1）、insData.X（1）、insData.Z（1）].“+insToLidarOffset”；%由于车辆预计将沿地面移动，因此侧倾的变化%和音高是最小的。忽略横摇和俯仰的变化，只使用航向。Rnow = rotmat(quaternion([insData.Heading(end) 0 0])，“欧拉”,“ZYX股票”,“点”），“点”)；Rbef=rotmat（四元数（[insData.标题（1）0]，“欧拉”,“ZYX股票”,“点”），“点”）;T = [Rbef Tbef;0 0 0 1] \ [Rnow Tnow;0 0 0 1];initTform = rigid3d (T。');结束

函数helperMakeFigurePublishFriendly (hFig)如果~isempty（hFig）和&isvalid（hFig）hFig.Handleviability=“回调”；结束结束