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

%创建一个图形窗口来可视化地面真实地图和路径点hFigGT =图;axGT =轴(“父”hFigGT,“颜色”,“黑”）;可视化地面真实路径点pcshow (gTruthWayPts“红色”,“MarkerSize”, 150,“父”axGT)举行在可视化被机器人覆盖的原始地图pcshow (orgMap“MarkerSize”10“父”axGT)举行从%自定义轴标签包含(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。轴,“Y (m)”)zlabel（lidarPlayer.Axes，“Z (m)”)标题(lidarPlayer。轴,激光雷达扫描的)%循环并可视化数据对于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。轴,“Z (m)”)标题(pplayer。轴,“处理扫描”)终止%创建一个图形窗口来可视化估计的轨迹hFigTrajUpdate =图;axTrajUpdate =轴(“父”hFigTrajUpdate,“颜色”,“黑”）;标题(axTrajUpdate,“传感器姿态轨迹”)

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

%向下采样当前扫描curScan = pcdownsample (pClouds (FrameIdx),“gridAverage”, gridStep);如果viewPC = = 1%可视化向下采样点云视图(pplayer curScan)终止

%提取FPFH特征curFeature=extractFPFHFeatures（curScan，“NumNeighbors”、邻居);如果FrameIdx = = 1%更新验收扫描及其形式pcAccepted(计数,1)= curScan;tformAccepted(计数,1)= rigid3d;%更新初始姿态到地面真相的第一个路径点%比较fpfhTform(计数,1)= rigid3d(眼(3),gTruthWayPts (1:));其他的%通过匹配当前扫描和前一次扫描来识别对应关系indexPairs=pcmatchfeatures（curFeature、prevFeature、curScan、prevScan、，…“MatchThreshold”matchThreshold,“RejectRatio”, matchRatio);matchedPrevPts =选择(prevScan indexPairs (:, 2));matchedCurPts =选择(curScan indexPairs (: 1));%估计当前扫描和上一次扫描之间的相对姿势%使用通讯tform1 = estimateGeometricTransform3D (matchedCurPts。的位置,…matchedPrevPts。的位置,“刚性”,“最大距离”maxDistance,…“MaxNumTrials”, maxNumTrails);执行ICP注册以微调相对姿态tform = pcregistericp (curScan prevScan,“InitialTransform”tform1,…“InlierRatio”，内插率）；

relPose = [tform2trvec(tform.T') tform2quat(tform.T')];在姿势图中添加相对姿势addRelativePose (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);终止

currSubMapId =地板(数/ nScansPerSubmap);如果快速眼动(计数,nScansPerSubmap) = = 0%对齐激光雷达扫描数组以创建子映射submap {currSubMapId} = pcalign (…pcAccepted((count - nScansPerSubmap + 1):count,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 loopClosureThreshold);如果~isempty(loopSubmapIds) rmseMin = inf;%从匹配的子映射id估计当前扫描的最佳匹配对于k = 1:长度(loopSubmapIds)%检查submap中的每个扫描对于kf = 1:nScansPerSubmap probableLoopCandidate =…loopSubmapIds(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,“颜色”,“黑”）;plot3 (fpfhEstimatedTraj (: 1) fpfhEstimatedTraj (:, 2), fpfhEstimatedTraj (:, 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);将相对姿态转换为刚性转换estimatedTforms = 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 (globalMap“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 (globalMap“父”axMap2)包含(axMap2“X (m)”) ylabel (axMap2“Y (m)”) zlabel (axMap2“Z (m)”)标题（axMap2，“估计地图”)