使用3-D LIDAR点云执行SLAM

此示例使用：

打开直播脚本

此示例演示了如何实现同时定位和映射使用点云处理算法收集的3-D LIDAR传感器数据的（SLAM）算法和姿势图优化。该示例的目的是估计机器人的轨迹，并从3-D LiDAR点云和估计的轨迹创建环境的3-D占用映射。

演示的SLAM算法估计使用基于正态分布变换（NDT）的点云登记算法的轨迹，并且每当机器人重新留置一个地方时，使用信任区域解算器使用SE3姿态图优化来减少漂移。

加载数据并设置可调参数

加载从Clearpath™哈士奇机器人收集的3-D LIDAR数据在停车库中。LIDAR数据包含一个单元格数组N-B-3矩阵，在哪里N是捕获的LIDAR数据中的3-D点，列表示XYZ.- 与每个捕获的点相关联的组。

加载pclouds.mat

点云注册算法的参数

使用点云登记算法指定用于估计轨迹的参数。maxlidarrange.指定3-D激光扫描仪的最大范围。

maxlidarrange = 20;

在室内环境中捕获的点云数据包含位于地面和天花板的点，这使得点云登记算法混淆。使用以下参数从点云中删除某些点：

参考矢量- 正常到地面平面。
Maxdistance.- 卸下地面和天花板时的最大距离。
maxangulardistance.- 拟合地面和天花板时的参考矢量的最大角度偏差。

参考矢量= [0 0 1];maxdistance = 0.5;maxangulardistance = 15;

为了提高登记算法的效率和准确性，点云使用随机抽样来缩小采样比例指定的样本比randomsampleratio.。

RandomSampleratio = 0.25;

Gridstep.指定NDT注册算法中使用的体素网格尺寸。只有在机器人移动到大于的距离之后，才会接受扫描distancemovedthreshold.。

gridstep = 2.5;distancemovedthreshold = 0.3;

调整特定机器人和环境的这些参数。

循环闭合估计算法的参数

指定循环闭合估计算法的参数。循环闭合仅在由当前机器人位置围绕的半径搜索LoopClosuresearchRadius.。

LoopClosuresearchRadius = 3;

循环闭合算法基于2-D子图和扫描匹配。每次都会创建一个子邮件nscanspersubmap.（每台副部长的扫描数）接受扫描。循环闭合算法无视最近的子行为搜索循环候选时扫描。

nscanspersubmap = 3;子行为= 50;

环形区域Z.- 限制annularregionlimits.从点云中提取。在点云平面拟合算法Idenfity的点云面算法之外，在地板和天花板上的这些限制范围内的点。

annularregionlimits = [-0.75 0.75];

在循环候选之间估计相对姿势的最大可接受的根均方平方误差（RMSE）指定rmsethreshold.。选择较低的值，以估计精确的环路闭合边缘，对姿势图优化具有高影响力。

rmsethreshold = 0.26;

通过扫描匹配分数接受循环关闭的阈值loopclosurethreshold.。插入后调用姿态图优化优化interval.将闭合边循环到姿势图中。

loopclosurethreshold = 150;优化interval = 2;

初始化变量

设置姿势图，占用图和必要的变量。

用于存储估计的相对姿势的％3D PoseGraph对象pgraph = posegraph3d;％默认序列化的6Y-6信息矩阵的右上角三角形infomat = [1,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0,1,0,1];自上次姿势图优化和地图改进以来增加了百分比的环路闭合边缘numloopclosuressincelastoptimization = 0;姿态图形优化后％true，直到下一个扫描mapupdated = false;如果扫描被接受，％等于1扫描扫描= 0;％3D占用网格对象用于创建和可视化3D地图mapresolution = 8;每米％细胞OMAP =占领ymap3d（MapResolution）;

preal分配处理点云，lidar扫描和副衬的变量。创建一个下采样的点云集，以便快速可视化地图。

PCProcessed = Cell（1，长度（Pclouds））;lidarscans2d = cell（1，长度（pclouds））;含量=细胞（1，长度（pclouds）/ nscanspersbmap）;pcstoview = cell（1，长度（pclouds））;

为显示目的创建变量。

％设置为1以在构建过程中可视化创建的地图和姿势图ViewMap = 1;％设置为1以在构建过程中可视化已处理的点云viewpc = 0;

设置随机种子以保证一致的随机抽样。

RNG（0）;

如果需要，初始化图形窗口。

％如果要在顺序处理时查看点云如果ViewPC == 1 PPlayer = PCPlayer（[ -  50 50]，[ -  50 50]，[ -  10 10]，'Markersize'，10）;结尾％如果要在构建过程中查看创建的地图和PoseGraph如果ViewMap == 1 ax = newplot;％数字轴手柄查看（20,50）;网格上;结尾

姿势图优化的轨迹估计和细化

机器人的轨迹是机器人姿势的集合（在3-D空间中的位置和方向）。使用3-D LIDAR SLAM算法在每个三维LIDAR扫描采集实例中估计机器人姿势。3-D LIDAR SLAM算法具有以下步骤：

点云过滤
点云下采样
点云注册
循环关闭查询
姿态图优化

迭代地处理点云以估计轨迹。

count = 0;％计数器以跟踪添加的扫描数量DISP（'估计机器人轨迹......'）;

估算机器人轨迹......

为了i = 1：长度（pclouds）％按顺序读取点云pc = pclouds {i};

点云过滤

点云过滤完成以从获取的扫描中提取感兴趣的区域。在该示例中，感兴趣区域是具有地面和天花板的环形区域。

删除与人类驱动程序对应的机器人后面的最大范围内的无效点和不必要的点。

IND =（-MAXLIDARRANGE ......＆-maxlidarrange ......＆（ABS（PC（:,2））> ABS（0.5 * PC（：，1））| PC（：，1）> 0））;pcl = pointcloud（pc（ind，:)）;

从地面平面上删除点。

[〜，〜，异常值] =......PCFITPLANE（PCL，MAXDISTANCE，参考矢量，MAXANGULARDISTANCE）;pcl_wogrd = select（pcl，异常值，'输出'那'满的'）;

去除天花板上的点。

[〜，〜，异常值] =......PCFITPLANE（PCL_WOGRD，0.2，参考矢量，MAXANGULARDISTANCE）;pcl_wogrd = select（pcl_wogrd，异常值，'输出'那'满的'）;

在环形区域中选择点。

ind =（pcl_wogrd.location（:,3） annularregionlimits（1））;pcl_wogrd = select（pcl_wogrd，IND，'输出'那'满的'）;

点云下采样

点云下采样提高了点云注册算法的速度和准确性。应为特定需求进行调整下来采样。随机采样算法从下面的下面的下下的采样变体进行凭证选择。

pcl_wogrd_sampled = pcdownsample（pcl_wogrd，'随机的'，randomsampleratio）;如果viewpc == 1％可视化下采样点云查看（pplayer，pcl_wogrd_sampled）;暂停（0.001）结尾

点云注册

点云注册估计当前扫描和先前扫描之间的相对姿势（旋转和翻译）。始终接受第一次扫描（进一步处理并存储），但是在转换超过指定阈值之后仅接受其他扫描。Posegraph3d.用于存储估计的接受的相对姿势（轨迹）。

如果count == 0.％首先可以tform = [];Scanaccepted = 1;别的如果count == 1 tform = pcregisterndt（pcl_wogrd_sampled，prevpc，gridstep）;别的tform = pcregisterndt（pcl_wogrd_sampled，prevpc，gridstep，......'InitialTransform'，prevtform）;结尾resiple = [tform2trvec（tform.t'）tform2quat（tform.t'）];如果SQRT（Norm（有关（1：3）））> DistancemovedThreshold AddrelativePosse（PGraph，有影响）;Scanaccepted = 1;别的扫描扫描= 0;结尾结尾

循环关闭查询

循环关闭查询确定了当前的机器人位置是否已被访问过。通过将当前扫描与先前扫描匹配在当前机器人位置的小半径内匹配当前扫描来执行搜索LoopClosuresearchRadius.。由于LIDAR Odometry的低漂移，在小半径内搜索就足够了，因为针对所有先前扫描的搜索都是耗时的。循环关闭查询包含以下步骤：

从中创建蛋白组nscanspersubmap.连续扫描。
将当前扫描与副联匹配匹配LoopClosuresearchRadius.。
如果匹配分数大于匹配分数，则接受匹配项loopclosurethreshold.。代表被接受的子群的所有扫描被视为可能的循环候选者。
估计可能的环路候选和电流扫描之间的相对姿势。仅当RMSE小于时，才接受相对姿势作为循环闭合约束rmsethreshold.。

如果Scanaccepted == 1 count = count + 1;PCProcessed {count} = pcl_wogrd_sampled;lidarscans2d {count} = examplehelpercreate2dscan（pcl_wogrd_sampled）;为更快的循环关闭查询创建％子部分。如果REM（计数，nscanspersubmap）== 0 supmaps {count / nscanspersubmap} = examplehelpercreatesubmap（lidarscans2d，......Pgraph，count，nscanspersubmap，maxlidarrange）;结尾％LopsubMapID包含匹配的子图ID，如果否则为空。如果（楼层（count / nscanspersubmap）> submapthresh）[围页毛头，〜] = examplehelperestimateloopcandidates（pgraph，......计数，副词，lidarscans2d {count}，nscanspersubmap，......LoopClosuresearchRadius，Loopclosurethreshold，Subpapthresh）;如果〜isempty（覆盖覆盆子）rmsemin = Inf;％估计最佳匹配到当前扫描为了k = 1：长度（围页覆盆子）递增次数内的每一个扫描为了j = 1：nscanspersubmap probableLoopCandidate =......Loopsubmapids（k）* nscanspersubmap  -  j + 1;[looptform，〜，rmse] = pcregisterndt（pcl_wogrd_sampled，......PCProcessed {probableLoopCandidate}，gridstep）;％更新最佳循环关闭候选如果RMSE 结尾如果rmsemin 休息;结尾结尾结尾％检查循环候选是否有效如果rmsemin ％循环关闭约束crely = [tform2trvec（looptform.t'）tform2quat（looptform.t'）];addrelativepose（pgraph，criple，Infomat，......Loopcandidate，count）;numloopclosuressincelastoptimization = numloopclosuressincelastoptimization + 1;结尾结尾结尾

姿态图优化

姿态图优化在接受足够数量的环路边缘后运行以减少轨迹估计的漂移。在每个环路闭合优化之后，由于姿势估计中的不确定性在优化之后减少了循环闭合搜索半径。

如果（numloopclosuressincelastoptimization ==优化interval）||......（（numloopclosuressincelactoptimization> 0）&&（i ==长度（pclouds）））如果loopclosuresearchradius〜= 1 disp（'做姿势图优化以减少漂移。）;结尾％姿态图优化Pgraph = OptimizePoseGraph（PGraph）;LoopClosuresearchRadius = 1;如果ViewMap == 1位置= pgraph.nodes;姿态图优化后％重建映射OMAP =占领ymap3d（MapResolution）;为了n = 1 :( pgraph.numnodes-1）插入点Cloud（OMAP，位置（n，:)，pcstoview {n} .removeinvalidpoints，maxlidarrange）;结尾mapupdated = true;斧头= newplot;网格上;结尾numloopclosuressincelastoptimization = 0;优化后％降低优化频率％触嘴优化interval =优化interval * 7;结尾

在构建过程中可视化地图和姿态图。此可视化昂贵，因此仅在必要时使其能够设置viewmap.1.如果启用可视化，则每15个添加扫描后，绘图会更新。

pctoview = pcdownsample（pcl_wogrd_sampled，'随机的'，0.5）;pcstoview {count} = pctoview;如果ViewMap == 1％插入点云到乘坐地图位置= pgraph.nodes（count）;InsertPointCloud（OMAP，位置，PCTOView.removeInvalidPoints，MaxLidArrange）;如果（REM（COUN-1,15）== 0）|| MAPUPDATED ExampleHelpervisualizeMapand2apraph（Omap，Pagher，AX）;结尾mapupdated = false;别的％提供反馈以知道示例正在运行如果（REM（COUNT-1,15）== 0）FPRINTF（'。'）;结尾结尾

更新以前的相对姿势估计和点云。

prevpc = pcl_wogrd_sampled;prevtform = tform;结尾结尾

做姿势图优化以减少漂移。

构建和可视化3-D占用地图

点云插入占领yap.使用估计的全局姿势。在迭代所有节点之后，显示完整的地图和估计的车辆轨迹。

如果（ViewMap〜= 1）||（numloopclosuressincelastoptizimization> 0）NodeSpositions =节点（PGraph）;％创建3D占用网格omapoview = itemancymap3d（mapresolution）;为了i = 1 :(大小（nodeSpositions，1）-1）pc = pcstoview {i};位置= nodeSpositions（i，:);％插入点云到乘坐地图InsertPointCloud（OMAPTOVIEW，POSION，PC.REMOVEINVALIDPOINTS，MAXLIDARRANGE）;结尾数字;AxisFinal = Newplot;examplehelpervisualizemapand2appraph（omaproview，pgraph，axisfinal）;结尾