数据文件='航空地图.tar.gz'；航路点文件=“gTruthWayPoints.mat”;%使用辅助功能在每个航路点生成激光雷达扫描[pClouds，orgMap，gTruthWayPts]=helperCreateDataset（数据文件，航路点文件）；

%创建一个图形窗口来可视化地面真实地图和路径点hFigGT=图；axGT=轴(“家长”hFigGT,“颜色”,“黑”）;%想象地面真相航路点pcshow（GTP、，“红色”,“MarkerSize”, 150,“家长”，axGT）保持在可视化被机器人覆盖的原始地图pcshow (orgMap“MarkerSize”10“家长”，axGT）保持远离的%自定义轴标签xlabel（axGT，‘X（m）’) ylabel (axGT“Y (m)”) zlabel (axGT“Z (m)”)标题（axGT，“地面真实地图和机器人轨迹”)

%为玩家指定限制xlimits=[-9090]；ylimits=[-9090]；zlimits=[-625-587]；%创建一个pcplayer对象来可视化激光雷达扫描lidarPlayer = pcplayer (xlimits ylimits zlimits);%自定义pcplayer轴标签xlabel（lidarPlayer.Axes，‘X（m）’)ylabel（lidarPlayer.Axes，“Y (m)”)zlabel（lidarPlayer.Axes，“Z (m)”)标题（lidarPlayer.Axes，“激光雷达扫描”)%循环并可视化数据对于l = 1:长度(pClouds)提取激光雷达扫描ptCloud=pClouds（l）；%更新激光雷达显示视图(lidarPlayer ptCloud)暂停(0.05)结束

skipFrames = 3;

gridStep=1.5；%在米

邻居= 60;

匹配阈值=0.1；匹配比率=0.97；

maxDistance=1；maxNumTrails=3000；

inlierRatio = 0.1;

alignGridStep=1.2；

loopClosureSearchRadius=7.9；

nScansPerSubmap = 3;subMapThresh = 15;loopClosureThreshold = 0.6;

rmseThreshold=0.6；

pGraph = poseGraph3D;%6乘6信息矩阵的默认序列化右上三角形infoMat = [1 0 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1];

%分配内存来存储子映射子映射=单元（地板（长度（pClouds）/（skipFrames*nScansPerSubmap）），1；%分配内存以存储每个子贴图姿势subMapPoses = 0(圆(长度(pClouds) / (skipFrames * nScansPerSubmap)), 3);初始化变量以存储接受的扫描及其转换submap创建百分比pcAccepted=pointCloud.empty（0）；tformAccepted=rigid3d.empty（0）；%初始化变量以存储基于特征方法的相对姿势%无姿势图形优化fpfhTform=rigid3d.empty（0）；用于跟踪添加的扫描次数的计数器数= 1;

%设置为1可在构建过程中可视化已处理的激光雷达扫描viewPC=0；%创建pcplayer对象以查看激光雷达扫描，同时%顺序处理它们，如果viewPC是启用的如果viewPC==1 pplayer=pcplayer（xlimits、ylimits、zlimits）；%自定义播放器轴标签包含(pplayer。轴,‘X（m）’) ylabel (pplayer。轴,“Y (m)”)zlabel（pplayer.Axes，“Z (m)”)标题（pplayer.Axes，“已处理扫描”)结束%创建一个体形窗口以可视化估计的轨迹hFigTrajUpdate=图形；axTrajUpdate=轴(“家长”hFigTrajUpdate,“颜色”,“黑”）;标题(axTrajUpdate,“传感器姿态轨迹”)

rng (“默认”)%在pcmatchfeature中设置随机种子以保证一致的结果对于FrameIdx = 1: skipFrames:长度(pClouds)

%向下采样当前扫描curScan=pcdownsample（pClouds（FrameIdx）），“gridAverage”，gridStep）；如果viewPC = = 1%可视化下采样点云视图(pplayer curScan)结束

%提取FPFH特征curFeature=extractFPFHFeatures（curScan，“纽曼尼斯堡”，邻居）；如果FrameIdx==1%更新验收扫描及其形式pcAccepted(计数,1)= curScan;tformAccepted(计数,1)= rigid3d;%将初始姿态更新为地面真实值的第一个航路点%比较fpfhTform（count，1）=rigid3d（眼睛（3），gTruthWayPts（1，：）；其他的%通过将当前扫描与上一次扫描相匹配来识别对应关系indexPairs=pcmatchfeatures（curFeature、prevFeature、curScan、prevScan、，．..“MatchThreshold”matchThreshold,“不合格率”, matchRatio);matchedPrevPts =选择(prevScan indexPairs (:, 2));matchedCurPts =选择(curScan indexPairs (: 1));%估计当前扫描和上一次扫描之间的相对姿势%使用通信tform1 = estimateGeometricTransform3D (matchedCurPts。的位置,．..matchedPrevPts。的位置,“刚性”,“最大距离”，最大距离，．..“MaxNumTrials”，maxNumTrails）；执行ICP注册以微调相对姿态tform = pcregistericp (curScan prevScan,“初始化转换”tform1,．..“内部比率”，内插率）；

relPose = [tform2trvec(tform.T') tform2quat(tform.T')];%将相对姿势添加到姿势图添加相对位置（pGraph，relPose）；

%更新计数器和存储接受扫描和他们的姿势计数=计数+1；pcAccepted（count，1）=curScan；accumPose=pGraph.nodes（高度（pGraph.nodes））；tformAccepted（count，1）=rigid3d（（trvec2tform）（累加（1:3））*．..quat2tform (accumPose (4))));%更新估计姿势fpfhTform(数)= rigid3d (tform。T * fpfhTform(把1).T);结束

CurrSubMap=楼层（计数/nScansPerSubmap）；如果rem（计数，nScansPerSubmap）==0%对齐激光雷达扫描阵列以创建子贴图submap {currSubMapId} = pcalign (．..pcAccepted（（计数-nScansPerSubmap+1）：计数，1），．..tformAccepted((count - nScansPerSubmap + 1):count,1)，．..校准步骤）；%将中心扫描姿势指定为子贴图的姿势submappose (currSubMapId，:) = tformAccepted(count -．..地板(nScansPerSubmap / 2), 1) .Translation;结束

如果currSubMapId > subMapThresh mostRecentScanCenter = pGraph.nodes(height(pGraph.nodes));通过匹配电流估计可能的环路闭合候选%用子贴图扫描[loopSubmapIds, ~] = helperEstimateLoopCandidates (curScan submap,．..次级对手，最新世纪扫描中心，currSubMapId，subMapThresh，．..loopClosureSearchRadius、LoopClosureStorhold）；如果~isempty(loopSubmapIds) rmseMin = inf;%从匹配的子映射id估计当前扫描的最佳匹配对于k=1：长度（循环子磁盘）%检查submap中的每个扫描对于kf=1:nScansPerSubmap probableLoop候选者=．..loopsubpids（k）*nScansPerSubmap-kf+1；[pose_-Tform，~，rmse]=pcregistericp（curScan，．..PCA已接受（可能是候选）；%更新最佳循环闭包候选者如果rmse < rmseMinmatchingNode = probableLoopCandidate;构成= [tform2trvec pose_Tform.T”)．..tform2quat（pose_Tform.T'）；结束结束结束%检查循环闭包候选项是否有效如果rmseMin将候选环的相对位姿添加到位姿图addRelativePose (pGraph姿势,infoMat matchingNode,．..珠心）；结束结束结束%更新以前的点云和功能prevScan=curScan；prevFeature=curFeature；%可视化从接受的扫描估计的轨迹。显示(pGraph“id”,“关闭”,“家长”, axTrajUpdate);drawnow结束

pGraph = optimizePoseGraph (pGraph,“FirstNodePose”，[gTruthWayPts(1，:) 1 0 0 0]);

从姿态图中得到估计的轨迹pGraphTraj = pGraph.nodes;从特征配准中得到估计的轨迹，而不需要姿态%图优化fpfhEstimatedTraj = 0(统计,3);对于i = 1:count fpfhEstimatedTraj(i，:) = fpfhTform(i).Translation;结束%创建一个图形窗口，以可视化地面真实情况和估计结果%轨迹hFigTraj=图形；axTraj=轴(“家长”，hFigTraj，“颜色”,“黑”)图3（FpfEstimatedTraj（：，1），FpfEstimatedTraj（：，2），FpfEstimatedTraj（：，3），．..的r *,“家长”，axTraj）保持在图3（pGraphTraj（：，1），pGraphTraj（：，2），pGraphTraj（：，3），“b.”,“家长”axTraj) plot3 (gTruthWayPts (: 1), gTruthWayPts (:, 2), gTruthWayPts (:, 3),“去”,“家长”，axTraj）保持远离的轴平等的视图(axTraj 2)包含(axTraj‘X（m）’) ylabel (axTraj“Y (m)”) zlabel (axTraj“Z (m)”)标题（axTraj，“轨迹比较”)传说(axTraj“无姿势图优化的估计轨迹”,．..“姿态图优化的估计轨迹”,．..“地面实况轨迹”,“位置”,“最佳户外”)

%从姿势中获得估计的轨迹estimatedTraj = pGraphTraj (:, 1:3);将相对姿态转换为刚性转换EstimatedForms=rigid3d.empty（0）；对于idx = 1: pGraph。NumNodes pose = pGraph.nodes(idx);rigidPose = rigid3d((trvec2tform(pose(1:3)) * quat2tform(pose(4:7)))');estimatedTforms (idx, 1) = rigidPose;结束%从处理过的点云及其相对姿势创建全局贴图globalMap = pcalign (pcAccepted estimatedTforms alignGridStep);%创建地物窗口以可视化估计的地图和轨迹hFigTrajMap =图;axTrajMap =轴(“家长”，hFigTrajMap，“颜色”,“黑”); pcshow（估计数），“红色”,“MarkerSize”, 150,“家长”，axTrajMap）保持在pcshow（全球地图，“MarkerSize”10“家长”，axTrajMap）保持远离的%自定义轴标签包含(axTrajMap‘X（m）’) ylabel (axTrajMap“Y (m)”) zlabel (axTrajMap“Z (m)”)标题（axTrajMap，'估计的机器人轨迹和生成的地图')

%创建一个数字窗口来显示地面真实地图和估计地图hFigMap =图(“位置”，[0 0 700 400]);axMap1 =次要情节(1、2、1,“颜色”,“黑”,“家长”, hFigMap);axMap1。Position = [0.08 0.2 0.4 0.55];pcshow (orgMap“家长”axMap1)包含(axMap1‘X（m）’)ylabel（axMap1，“Y (m)”)zlabel（axMap1，“Z (m)”)标题(axMap1“地面实况图”) axMap2 = subplot(1,2,2，“颜色”,“黑”,“家长”, hFigMap);axMap2。Position = [0.56 0.2 0.4 0.55];pcshow（全球地图，“家长”，axMap2）xlabel（axMap2，‘X（m）’)ylabel（axMap2，“Y (m)”)zlabel（axMap2，“Z (m)”)标题（axMap2，“估计地图”)