基于ffh描述符的航空激光雷达SLAM
这个例子演示了如何实现同时定位和测绘(SLAM)算法的航空测绘使用3-D特征。本例的目标是估计机器人的轨迹，并创建其环境的点云地图。
本例中的SLAM算法通过使用在每个点提取的快速点特征直方图（FPFH）描述符在下采样接受扫描之间找到粗略对齐来估计轨迹，然后应用迭代最近点（ICP）算法来微调对齐。3-D姿势图优化，来自导航工具箱™, 减少估计轨迹中的漂移。
创建和可视化数据
从OpenTopography网站下载的航空数据块创建合成激光雷达扫描[1]。加载一个MAT文件，其中包含定义机器人轨迹的空中数据上的一系列航路点。使用helperCreateDataset函数。函数输出在每个航路点计算的激光雷达扫描作为数组点云物体，机器人覆盖的原始地图和地面真实路径点。
外部=“aerialMap.tar.gz”；wayPointsfile =“gTruthWayPoints.mat”；使用辅助函数在每个航路点生成一个激光雷达扫描[pClouds, orgMap gTruthWayPts] = helperCreateDataset(丢失,wayPointsfile);
在机器人覆盖的原始地图上可视化地面真实路径点。
%创建地物窗口以可视化地面真实地图和航路点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“地面真实地图和机器人轨迹”）
可视化提取的激光雷达扫描，使用pcplayer对象。
%为玩家指定限制Xlimits = [-90 90];Ylimits = [-90 90];Zlimits = [-625 -587];%创建一个pcplayer对象来可视化激光雷达扫描lidarPlayer=pcplayer（xlimits、ylimits、zlimits）；%自定义pc播放器轴标签包含(lidarPlayer。轴,“X (m)”) ylabel (lidarPlayer。轴,‘Y（m）’) zlabel (lidarPlayer。轴,“Z (m)”)标题(lidarPlayer。轴,激光雷达扫描的）%循环并可视化数据为l=1：长度（pClouds）%提取激光雷达扫描ptCloud = pClouds (l);%更新激光雷达显示视图(lidarPlayer ptCloud)暂停(0.05)结束
设置可调参数
指定轨迹和闭环估计的参数。针对特定的机器人和环境调整这些参数。
点云配准参数
指定在接受注册的每次扫描之间跳过的激光雷达扫描的数量。由于连续扫描有很高的重叠，跳过一些帧可以在不影响精度的情况下提高算法的速度。
skipFrames = 3;
下采样激光雷达扫描可以提高算法速度。方框网格通过将每个单元内的所有点平均为一个点来过滤下采样点云。指定方框网格过滤器的单个单元格的大小，单位为米。
gridStep = 1.5;%在米
对下采样点云中的每个有效点提取FPFH描述符。指定用于计算描述符的k-最近邻(KNN)搜索方法的邻域大小。
邻居= 60;
设置匹配FPFH描述符的阈值和比率，用于识别点对应关系。
matchThreshold = 0.1;matchRatio = 0.97;
设置连续扫描之间相对姿势估计的最大距离和轨迹数。
maxDistance = 1;maxNumTrails = 3000;
指定用于微调相对姿态的内嵌线的百分比。
内部比率=0.1；
指定方框网格过滤器的每个单元格的大小，用于通过对准激光雷达扫描来创建地图。
alignGridStep = 1.2;
环路闭合估计的参数
指定围绕当前机器人位置的半径，以搜索循环结束候选。
loopClosureSearchRadius = 7.9;
闭环算法基于三维子地图创建和匹配。子映射由指定数量的接受扫描组成nScansPerSubmap．循环闭包算法也忽略指定的最近扫描次数subMapThresh，同时搜索循环候选项。指定均方根误差(RMSE)阈值loopClosureThreshold,接受子映射作为匹配。较低的分数可能表明较好的匹配，但分数会根据传感器数据和预处理的不同而变化。
nScansPerSubmap = 3;subMapThresh = 15;loopClosureThreshold = 0.6;
指定最大可接受的均方根误差(RMSE)的估计相对姿态之间的循环候选人rmseThreshold．选择较低的值可以得到更精确的闭环边，这对位姿图优化有很大的影响，但评分会根据传感器数据和预处理的不同而有所不同。
rmseThreshold = 0.6;
初始化变量
创建一个姿势图，使用poseGraph3D(导航工具箱)对象，以存储接受的激光雷达扫描之间的估计相对姿势。
pGraph=poseGraph3D；%默认的6 × 6信息矩阵的右上方序列化三角形infoMat = [1 0 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1];
预分配和初始化计算所需的变量。
%分配内存来存储子映射submap =细胞(地板(长度(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);%自定义播放器轴标签xlabel（pplayer.Axes，“X (m)”)ylabel（pplayer.Axes，‘Y（m）’) zlabel (pplayer。轴,“Z (m)”)标题(pplayer。轴,“处理扫描”）结束%创建一个图形窗口来可视化估计的轨迹hFigTrajUpdate =图;axTrajUpdate =轴(“父”hFigTrajUpdate,“颜色”，“黑”）;标题(axTrajUpdate,“传感器构成的轨迹”）
轨迹估计与修正
机器人的轨迹是其姿势的集合，包括其在三维空间中的位置和方向。使用三维激光雷达SLAM算法从三维激光雷达扫描实例估计机器人姿势。使用以下过程执行三维SLAM估计：
点云将采样
点云注册
Submap创建
关闭循环查询
构成图优化
迭代处理激光雷达扫描以估计机器人的轨迹。
rng(“默认”）%在pcmatchfeature中设置随机种子以保证一致的结果为FrameIdx = 1: skipFrames:长度(pClouds)
点云将采样
点云下采样可以提高点云配准算法的速度。降采样应该根据您的具体需要进行调整。
%向下采样当前扫描curScan = pcdownsample (pClouds (FrameIdx),“gridAverage”, gridStep);如果viewPC = = 1%可视化向下采样点云视图(pplayer curScan)结束
点云注册
点云配准估计当前扫描和前一次扫描之间的相对位姿。该示例使用以下步骤进行注册:
属性通过比较描述符来标识点对应
pcmatchfeatures函数估计相对姿态从点对应使用
estimateGeometricTransform3D函数使用
pcregistericp函数
%提取FPFH特征curFeature = extractFPFHFeatures (curScan,“NumNeighbors”、邻居);如果FrameIdx = = 1%更新验收扫描及其tformpcAccepted（count，1）=curScan；tformAccepted（count，1）=rigid3d；%更新初始姿态到地面真相的第一个路径点%的比较fpfhTform(计数,1)= rigid3d(眼(3),gTruthWayPts (1:));其他的%通过匹配当前扫描和前一次扫描来识别对应关系indexPairs = pcmatchfeatures (curFeature prevFeature、curScan prevScan,．..“匹配阈值”matchThreshold,“RejectRatio”，匹配比率）；matchedPrevPts=select（prevScan，indexPairs（：，2））；matchedCurPts=select（curScan，indexPairs（：，1））；估计当前扫描和先前扫描之间的相对位姿%使用通讯tform1 = estimateGeometricTransform3D (matchedCurPts。的位置,．..匹配的位置，“刚性”，“MaxDistance”maxDistance,．..“MaxNumTrials”, maxNumTrails);执行ICP注册以微调相对姿态tform = pcregistericp (curScan prevScan,“InitialTransform”tform1,．..“InlierRatio”, inlierRatio);
将刚性变换转换为xyz -位置和四元数方向，将其添加到姿态图中。
relPose=[tform2trvec（tform.T'）tform2quat（tform.T'）]；在姿势图中添加相对姿势addRelativePose (pGraph relPose);
存储用于创建子映射的接受扫描及其姿态。
%更新计数器和存储接受扫描和他们的姿势Count = Count + 1;pcAccepted(计数,1)= curScan;accumPose = pGraph.nodes(高度(pGraph.nodes));tformAccepted(count,1) = rigid3d((trvec2tform(accumPose(1:3))) * . drawtext (count,1) = 1， '伏'，'伏')，color0000ff．..quat2tform (accumPose (4))));更新估计姿态fpfhTform（count）=rigid3d（tform.T*fpfhTform（count-1.T）；结束
子映射创建
通过以指定大小的组相互对齐顺序的、已接受的扫描来创建子贴图nScansPerSubmap,使用pcalign函数。使用子映射可以导致更快的循环闭包查询。
currSubMapId =地板(数/ nScansPerSubmap);如果快速眼动(计数,nScansPerSubmap) = = 0%对齐激光雷达扫描数组以创建子映射submap {currSubMapId} = pcalign (．..pcAccepted((count - nScansPerSubmap + 1):count,1)，．..tformAccepted（（计数-nScansPerSubmap+1）：计数，1），．..alignGridStep);%指定中心扫描姿态为子映射的姿态submappose (currSubMapId，:) = tformAccepted(count -．..地板(nScansPerSubmap / 2), 1) .Translation;结束
循环闭包查询
使用一个关闭循环查询识别以前去过的地方。循环闭包查询包括查找当前扫描和以前的子映射之间的相似性。如果您发现一个类似的扫描，那么当前扫描是一个循环闭包候选。循环闭包候选估计包括以下步骤:
使用
helperEstimateLoopCandidates函数。如果当前扫描和子映射之间的RMSE小于指定的值，则子映射是匹配的loopClosureThreshold。前面由所有匹配的子映射表示的扫描是循环闭包候选。使用ICP配准算法计算当前扫描和闭环候选之间的相对位姿。RMSE最低的闭环候选是当前扫描的最佳可能匹配。
只有当RMSE低于指定的阈值时，才接受循环闭包候选项作为有效的循环闭包。
如果currSubMapId>subMapThresh mostRecentScanCenter=pGraph.nodes（高度（pGraph.nodes））；%通过匹配电流估计可能的回路闭合候选%扫描与子映射[loopSubmapIds, ~] = helperEstimateLoopCandidates (curScan submap,．..subMapPoses、mostRecentScanCenter currSubMapId subMapThresh,．..loopClosureSearchRadius loopClosureThreshold);如果~isempty(loopSubmapIds) rmseMin = inf;%根据匹配的子映射ID估计当前扫描的最佳匹配为k = 1:长度(loopSubmapIds)%检查子映射内的每个扫描为kf = 1:nScansPerSubmap probableLoopCandidate =．..loopSubmapIds(k)*nScansPerSubmap - kf + 1;[pose_Tform ~, rmse] = pcregistericp (curScan,．..pcAccepted (probableLoopCandidate));%更新最佳循环闭包候选如果rmse．..tform2quat (pose_Tform.T '));结束结束结束检查候选循环闭包是否有效如果rmseMin < rmseThreshold将候选环的相对位姿添加到位姿图addRelativePose (pGraph姿势,infoMat matchingNode,．..pGraph.NumNodes);结束结束结束%更新以前的点云和要素prevScan = curScan;prevFeature = curFeature;%可视化从接受的扫描估计的轨迹。显示(pGraph“ID”，“关”，“父”，axTrajUpdate）；刷新屏幕结束
构成图优化
利用该方法减小估计轨迹中的漂移optimizePoseGraph(导航工具箱)函数。通常，姿态图中第一个节点的姿态表示原点。为了将估计的轨迹与地面真路径点进行比较，指定第一个地面真路径点作为第一个节点的位姿。
pGraph=优化posegraph（pGraph，“FirstNodePose”，[gTruthWayPts(1，:) 1 0 0 0]);
想象轨迹比较
利用未经过位姿图优化的特征、经过位姿图优化的特征和地面真实数据可视化估计轨迹。
从姿态图中得到估计的轨迹pGraphTraj=pGraph.nodes；从特征配准中得到估计的轨迹，而不需要姿态%图优化fpfhEstimatedTraj = 0(统计,3);为i=1:count fpfhEstimatedTraj（i，：）=fpfhTform（i）.翻译；结束%创建一个图形窗口来可视化地面真相和估计%的轨迹hFigTraj =图;axTraj =轴(“父”hFigTraj,“颜色”，“黑”）;plot3 (fpfhEstimatedTraj (: 1) fpfhEstimatedTraj (:, 2), fpfhEstimatedTraj (:, 3),．..“r*”，“父”axTraj)举行在plot3 (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“没有姿态图优化的估计轨迹”，．..“使用姿势图优化估计轨迹”，．..“地面实况轨迹”，“位置”，“bestoutside”）
构建和可视化生成的地图
转换和合并激光雷达扫描使用估计姿态创建一个累积点云图。
从姿态得到估计的轨迹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、EstimatedForms、alignGridStep）；%创建一个图形窗口来可视化估计的地图和轨迹hFigTrajMap =图;axTrajMap =轴(“父”hFigTrajMap,“颜色”，“黑”）;pcshow (estimatedTraj“红色”，“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.20.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“估计地图”）
另请参阅
功能
readPointCloud|insertPointCloud(导航工具箱)|光线交叉(导航工具箱)|addRelativePose(导航工具箱)|optimizePoseGraph(导航工具箱)|显示(导航工具箱)|extractFPFHFeatures|pcmatchfeatures|estimateGeometricTransform3D|pcdownsample|pctransform|pcregistericp|pcalign|tform2trvec(导航工具箱)|tform2quat(导航工具箱)
对象
lasFileReader|点云|pcplayer|occupancyMap3D(导航工具箱)|poseGraph3D(导航工具箱)|rigid3d
工具书类
[1]斯塔尔,斯科特。图斯卡卢萨:季节性洪水动态和无脊椎动物群落。国家机载激光测绘中心，2011年12月1日。OpenTopography (https://doi.org/10.5069/G9SF2T3K)．
你也可以从以下列表中选择一个网站:
