主要内容

航空激光雷达使用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 = rigid3d.empty (0);%初始化变量来存储相对的提出了从基于功能特性的方法%不构成图优化fpfhTform = rigid3d.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大满贯估计:

  1. 点云将采样

  2. 点云注册

  3. Submap创建

  4. 关闭循环查询

  5. 构成图优化

激光雷达扫描迭代过程来估计机器人的轨迹。

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

点云将采样

点云将采样可以改善的点云配准算法的速度。将采样应该针对您的特定需求。

% Downsample当前扫描curScan = pcdownsample (pClouds (FrameIdx),“gridAverage”,gridStep);如果viewPC = = 1%显示采样点云视图(pplayer curScan)结束

点云注册

登记点云之间的相对姿态估计当前的扫描和以前的扫描。注册的例子使用这些步骤:

  1. 从每个扫描使用提取FPFH描述符extractFPFHFeatures函数

  2. 标识点通讯通过对比描述符使用pcmatchfeatures函数

  3. 估计相对构成从通讯使用点estimateGeometricTransform3D函数

  4. 对相对姿态使用pcregistericp函数

%提取FPFH特性curFeature = extractFPFHFeatures (curScan,“NumNeighbors”、邻居);如果FrameIdx = = 1%更新及其tform接受扫描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.Location,…matchedPrevPts.Location,“刚性”,“MaxDistance”maxDistance,…“MaxNumTrials”,maxNumTrails);%进行ICP登记调整相对姿势tform = pcregistericp (curScan prevScan,“InitialTransform”tform1,…“InlierRatio”,inlierRatio);

将刚性变换转换成一个xyz -位置和四元数姿态,将其添加到构成图。

relPose = [tform2trvec tform.T”) tform2quat (tform.T '));%添加相对对构成图addRelativePose (pGraph relPose);

商店接受扫描及其构成用于创建子映射。

%更新计数器和商店接受扫描和他们的姿势数=计数+ 1;pcAccepted(计数,1)= curScan;accumPose = pGraph.nodes(高度(pGraph.nodes));tformAccepted(计数,1)= rigid3d (trvec2tform (accumPose (1:3)) *…quat2tform (accumPose (4))));%更新估计姿势fpfhTform(数)= rigid3d (tform。T * fpfhTform(把1).T);结束

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;结束

关闭循环查询

使用一个关闭循环查询识别之前访问过的地方。循环关闭查询包括找到当前的扫描和之前的子映射之间的相似性。如果你找到一个类似的扫描,然后关闭当前的扫描是一个循环的候选人。循环关闭候选人评估包括以下步骤:

  1. 估计之前的子映射之间的匹配和当前扫描使用helperEstimateLoopCandidates函数。submap匹配,如果当前的扫描和之间的RMSE submap小于指定值loopClosureThreshold。前面的扫描由所有匹配的子映射是循环关闭候选人。

  2. 计算当前扫描和循环之间的相对姿态关闭候选人使用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.T”)…tform2quat (pose_Tform.T '));结束结束结束%检查回路是否关闭候选人是有效的如果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 = rigid3d.empty (0);为idx = 1: pGraph。NumNodes姿势= pGraph.nodes (idx);rigidPose = rigid3d ((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|estimateGeometricTransform3D|pcdownsample|pctransform|pcregistericp|pcalign|tform2trvec(导航工具箱)|tform2quat(导航工具箱)

对象

lasFileReader|pointCloud|pcplayer|occupancyMap3D(导航工具箱)|poseGraph3D(导航工具箱)|rigid3d

引用

[1]斯塔尔,斯科特。“塔斯卡卢萨,艾尔:季节性洪水动力学和无脊椎动物社区。”National Center for Airborne Laser Mapping, December 1, 2011. OpenTopography(https://doi.org/10.5069/G9SF2T3K)。

激光雷达工具箱文档

金宝app

试着用MATLAB仿真软金宝app件,和其他产品下载188bet金宝搏

得到审判现在