航空激光雷达使用FPFH描述符
这个例子演示了如何实现同步定位和映射(大满贯)空中映射算法使用3 d功能。这个例子的目的是估计一个机器人的轨迹,并创建一个点云环境的地图。
SLAM算法在这个例子估计轨迹之间找到一个粗对齐downsampled接受扫描,使用快速点特征直方图(FPFH)描述符提取的每一点,然后应用迭代最近点(ICP)算法来调整对齐。三维构成图的优化,从导航工具箱™,减少估计的漂移轨迹。
创建和可视化数据
从一片空中创建合成激光雷达扫描数据,从OpenTopography下载网站[1]。加载一个MAT-file包含一系列的路径点在空中数据定义了机器人的轨迹。从数据在每一个路标计算激光雷达扫描使用helperCreateDataset
函数。函数输出激光雷达扫描计算在每一个路标的数组pointCloud
原始地图对象,由机器人和地面真理锚点。
外部=“aerialMap.tar.gz”;wayPointsfile =“gTruthWayPoints.mat”;%生成一个激光雷达扫描使用helper函数在每一个路标[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);%定制pcplayer轴标签包含(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;
将采样激光雷达扫描可以提高算法速度。箱网格过滤器downsamples平均每个单元格内的所有点的点云单点。指定的单个细胞的大小一盒网格过滤器,在米。
gridStep = 1.5;%在米
FPFH描述符提取downsampled每个有效点的点云。再指定社区大小(资讯搜索方法用于计算描述符。
邻居= 60;
设置阈值和比例匹配FPFH描述符,用来确定对应点。
matchThreshold = 0.1;matchRatio = 0.97;
设置路径的最大距离和数量之间的相对姿态估计连续扫描。
maxDistance = 1;maxNumTrails = 3000;
指定的百分比内围层考虑微调相对的姿势。
inlierRatio = 0.1;
指定每个单元格大小的一盒网格过滤器,用于创建地图通过调整激光雷达扫描。
alignGridStep = 1.2;
for循环关闭估计参数
指定当前机器人周围的半径位置搜索循环关闭的候选人。
loopClosureSearchRadius = 7.9;
循环闭包算法是基于三维映射中创建和匹配。submap包含指定数量的接受扫描nScansPerSubmap
。循环闭包算法也忽视了指定数量的最近的扫描subMapThresh
循环,而寻找候选人。指定根均方误差(RMSE)阈值loopClosureThreshold,
接受一个submap匹配。一个较低的分数可以显示一个更好的匹配,但分数随传感器数据和预处理。
nScansPerSubmap = 3;subMapThresh = 15;loopClosureThreshold = 0.6;
指定的最大可接受的根均方误差(RMSE)估计的相对姿态之间循环的候选人rmseThreshold
。选择一个较低的值会导致更精确的循环闭合边缘,具有高影响构成图优化,但分数随传感器数据和预处理。
rmseThreshold = 0.6;
初始化变量
创建一个图,使用poseGraph3D
(导航工具箱)对象,存储之间估计相对姿态接受激光雷达扫描。
pGraph = poseGraph3D;%的缺省序列化右上角三角形6-by-6信息矩阵infoMat = [1 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 1);
计算所需Preallocate和初始化变量。
%分配内存来存储子映射submap =细胞(地板(长度(pClouds) / (skipFrames * nScansPerSubmap)), 1);%分配内存来存储每一个submap姿势subMapPoses = 0(圆(长度(pClouds) / (skipFrames * nScansPerSubmap)), 3);%初始化变量来存储扫描及其变换了submap创建百分比pcAccepted = pointCloud.empty (0);tformAccepted = rigidtform3d.empty (0);%初始化变量来存储相对的提出了从基于功能特性的方法%不构成图优化fpfhTform = rigidtform3d.empty (0);%计数器跟踪扫描的数量增加了数= 1;
创建变量来可视化处理激光雷达扫描和估计的轨迹。
%设置为1可视化构建过程中激光雷达扫描处理viewPC = 0;%创建pcplayer对象查看激光雷达扫描%的处理顺序,如果启用了viewPC如果viewPC = = 1 pplayer = pcplayer (xlimits、ylimits zlimits);%定制球员轴标签包含(pplayer.Axes“X (m)”)ylabel (pplayer.Axes“Y (m)”)zlabel (pplayer.Axes“Z (m)”)标题(pplayer.Axes“处理扫描”)结束%图窗口创建一个可视化估计轨迹hFigTrajUpdate =图;axTrajUpdate =轴(父= hFigTrajUpdate颜色=“黑”);标题(axTrajUpdate,“传感器构成的轨迹”)
轨迹估计和细化
机器人的轨迹是一家集提出了由其在三维空间位置和方向。估计机器人构成的三维激光雷达扫描使用3 d激光雷达SLAM算法实例。使用这些流程执行3 d大满贯估计:
点云将采样
点云注册
Submap创建
关闭循环查询
构成图优化
激光雷达扫描迭代过程来估计机器人的轨迹。
rng (“默认”)%设置随机种子在pcmatchfeatures保证一致的结果为FrameIdx = 1: skipFrames:长度(pClouds)
点云将采样
点云将采样可以改善的点云配准算法的速度。将采样应该针对您的特定需求。
% Downsample当前扫描curScan = pcdownsample (pClouds (FrameIdx) gridAverage = gridStep);如果viewPC = = 1%显示采样点云视图(pplayer curScan)结束
点云注册
登记点云之间的相对姿态估计当前的扫描和以前的扫描。注册的例子使用这些步骤:
从每个扫描使用提取FPFH描述符
extractFPFHFeatures
函数标识点通讯通过对比描述符使用
pcmatchfeatures
函数估计相对构成从通讯使用点
estgeotform3d
函数对相对姿态使用
pcregistericp
函数
%提取FPFH特性curFeature = extractFPFHFeatures (curScan NumNeighbors =邻居);如果FrameIdx = = 1%更新及其tform接受扫描pcAccepted(计数,1)= curScan;tformAccepted(计数,1)= rigidtform3d;%更新初始对第一个地面实况的路标%的比较fpfhTform(计数,1)= rigidtform3d ([0 0 0], gTruthWayPts (1:));其他的%确定通讯通过匹配当前扫描之前的扫描indexPairs = pcmatchfeatures (curFeature prevFeature、curScan prevScan,…MatchThreshold = MatchThreshold RejectRatio = matchRatio);matchedPrevPts =选择(prevScan indexPairs (:, 2));matchedCurPts =选择(curScan indexPairs (: 1));%之间估计相对构成当前扫描和以前的扫描%使用通讯tform1 = estgeotform3d (matchedCurPts.Location,…matchedPrevPts.Location,“刚性”MaxDistance = MaxDistance,…MaxNumTrials = maxNumTrails);%进行ICP登记调整相对姿势tform = pcregistericp (curScan、prevScan InitialTransform = tform1,…InlierRatio = InlierRatio);
将刚性变换转换成一个xyz -位置和四元数姿态,将其添加到构成图。
relPose = [tform2trvec (tform.A) tform2quat (tform.A)];%添加相对对构成图addRelativePose (pGraph relPose);
商店接受扫描及其构成用于创建子映射。
%更新计数器和商店接受扫描和他们的姿势数=计数+ 1;pcAccepted(计数,1)= curScan;accumPose = pGraph.nodes(高度(pGraph.nodes));tformAccepted(计数,1)= rigidtform3d (trvec2tform (accumPose (1:3)) *…quat2tform (accumPose (4))));%更新估计姿势fpfhTform(数)= rigidtform3d (fpfhTform(把1)。* tform.A);结束
Submap创建
创建子映射调整顺序,彼此接受扫描组指定的大小nScansPerSubmap
,使用pcalign
函数。使用submap会导致更快的循环关闭查询。
currSubMapId =地板(数/ nScansPerSubmap);如果快速眼动(计数,nScansPerSubmap) = = 0%对齐激光雷达扫描数组创建submapsubmap {currSubMapId} = pcalign (…pcAccepted ((count - nScansPerSubmap + 1):统计,1),…tformAccepted ((count - nScansPerSubmap + 1):统计,1),…alignGridStep);%分配中心扫描冒充构成子映射subMapPoses (currSubMapId:) = tformAccepted(计数-…地板(nScansPerSubmap / 2), 1) .Translation;结束
关闭循环查询
使用一个关闭循环查询识别之前访问过的地方。循环关闭查询包括找到当前的扫描和之前的子映射之间的相似性。如果你找到一个类似的扫描,然后关闭当前的扫描是一个循环的候选人。循环关闭候选人评估包括以下步骤:
估计之前的子映射之间的匹配和当前扫描使用
helperEstimateLoopCandidates
函数。submap匹配,如果当前的扫描和之间的RMSE submap小于指定值loopClosureThreshold。
前面的扫描由所有匹配的子映射是循环关闭候选人。计算当前扫描和循环之间的相对姿态关闭候选人使用ICP配准算法。最低的循环关闭候选人RMSE是最好的可能的匹配当前扫描。
循环关闭候选人被接受为一个有效的循环关闭只有当RMSE低于指定的阈值。
如果currSubMapId > subMapThresh mostRecentScanCenter = pGraph.nodes(高度(pGraph.nodes));%的估计可能的循环关闭候选人通过匹配电流%与submap扫描[loopSubmapIds, ~] = helperEstimateLoopCandidates (curScan submap,…subMapPoses、mostRecentScanCenter currSubMapId subMapThresh,…loopClosureSearchRadius loopClosureThreshold);如果~ isempty (loopSubmapIds) rmseMin =正;%估计当前扫描的最佳匹配的匹配子映射id为k = 1:长度(loopSubmapIds)在submap %检查每一个扫描为kf = 1: nScansPerSubmap probableLoopCandidate =…loopSubmapIds (k) * nScansPerSubmap - kf + 1;[pose_Tform ~, rmse] = pcregistericp (curScan,…pcAccepted (probableLoopCandidate));%更新最好关闭循环的候选人如果rmse < rmseMin rmseMin = rmse;matchingNode = probableLoopCandidate;构成= [tform2trvec (pose_Tform.A)…tform2quat (pose_Tform.A)];结束结束结束%检查回路是否关闭候选人是有效的如果rmseMin < rmseThreshold%添加相对构成的回路关闭候选人构成图addRelativePose (pGraph姿势,infoMat matchingNode,…pGraph.NumNodes);结束结束结束%更新以前的点云和特性prevScan = curScan;prevFeature = curFeature;%可视化估计从接受扫描轨迹。显示(pGraph id =“关闭”父母= axTrajUpdate);drawnow结束
构成图优化
减少使用估计的漂移轨迹optimizePoseGraph
(导航工具箱)函数。一般来说,第一个节点构成的构成图代表了原点。比较估计轨道与地面真理中转地点,指定第一个地面实况路标作为第一个节点的姿势。
pGraph = optimizePoseGraph (pGraph FirstNodePose = [gTruthWayPts (: 1) 1 0 0 0]);
想象轨迹比较
可视化估计使用功能没有造成图优化轨迹,构成图优化的特性,和地面实况数据。
%得到姿势估计轨迹图pGraphTraj = pGraph.nodes;%得到估计轨迹特征登记没有构成%图优化fpfhEstimatedTraj = 0(统计,3);为i = 1:计数fpfhEstimatedTraj(我:)= fpfhTform .Translation;结束%图窗口创建一个可视化地面真理和估计%的轨迹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 = rigidtform3d.empty (0);为idx = 1: pGraph。NumNodes姿势= pGraph.nodes (idx);rigidPose = rigidtform3d ((trvec2tform(姿势(1:3))* quat2tform(构成(4))));estimatedTforms (idx, 1) = rigidPose;结束%从点云处理创建全球地图及其相对的姿势globalMap = pcalign (pcAccepted estimatedTforms 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。位置= (0.08 - 0.2 0.4 - 0.55);pcshow (orgMap、家长= axMap1)轴在包含(axMap1“X (m)”)ylabel (axMap1“Y (m)”)zlabel (axMap1“Z (m)”)标题(axMap1“地面实况图”)axMap2 =次要情节(1、2、2、颜色=“黑”父母= hFigMap);axMap2。位置= (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
(导航工具箱)|rayIntersection
(导航工具箱)|addRelativePose
(导航工具箱)|optimizePoseGraph
(导航工具箱)|显示
(导航工具箱)|extractFPFHFeatures
|pcmatchfeatures
|estgeotform3d
|pcdownsample
|pctransform
|pcregistericp
|pcalign
|tform2trvec
(导航工具箱)|tform2quat
(导航工具箱)
对象
lasFileReader
|pointCloud
|pcplayer
|occupancyMap3D
(导航工具箱)|poseGraph3D
(导航工具箱)|rigidtform3d
引用
[1]斯塔尔,斯科特。“塔斯卡卢萨,艾尔:季节性洪水动力学和无脊椎动物社区。”National Center for Airborne Laser Mapping, December 1, 2011. OpenTopography(https://doi.org/10.5069/G9SF2T3K)。