从文件加载激光扫描数据

载入从室内环境的移动机器人上收集的激光扫描数据集。两次扫描之间的平均位移约为0.6米。

的offlineSlamData.mat文件包含扫描可变，其中包含此示例中使用的所有激光扫描

负载('offlineslamdata.mat');

为了便于说明，给出了机器人的平面图和近似路径。这张图片显示了被映射的相对环境和机器人的近似轨迹。

运行SLAM算法，构建优化地图，绘制机器人轨迹

创建一个lidarSLAM对象并设置地图分辨率和最大LIDAR范围。此示例使用ClearPath Robotics™的Jackal™机器人。机器人配备了SICK™TIM-511激光扫描仪，最大范围为10米。设置略小于最大扫描范围（8M）的最大LIDAR范围，因为激光读数在最大范围内的近距离更准确。将网格图分辨率设置为每米的20个单元格，这给出了5厘米的精度。

maxLidarRange = 8;mapResolution = 20;slalg = lidarSLAM(mapResolution, maxLidarRange);

以下循环闭合参数凭经验设置。使用更高的循环闭合阈值有助于抑制环路关闭识别过程中的误报。然而，请记住，高分匹配可能仍然是一个糟糕的匹配。例如，在具有类似或重复特征的环境中收集的扫描更有可能产生误报。使用较高的循环闭合搜索RADIUS允许该算法在循环闭合的电流姿势估计周围搜索更宽的地图。

slamAlg。LoopClosureThreshold = 210;slamAlg。LoopClosureSearchRadius = 8;

使用初始10扫描观察地图建筑过程

逐步添加扫描slamAlg对象。扫描号码被打印，如果添加到地图。如果扫描之间的距离太小，对象会拒绝扫描。先添加前10个扫描来测试你的算法。

为i=1:10 [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});如果Isscancepted fprintf（'添加了扫描％d \ n',我);结束结束

新增扫描1新增扫描2新增扫描3新增扫描4新增扫描5新增扫描7新增扫描8新增扫描9新增扫描10

通过绘制扫描图和跟踪的姿态来重建场景slamAlg。

图;显示(slamAlg);标题({“环境地图”,“最初10次扫描的姿势图”});

观察闭环效果和优化过程

在循环中继续添加扫描。当机器人移动时，应自动检测回路闭合。当一个闭环被识别时，位姿图优化被执行。输出optimizationInfo有一个领域,缺乏形状，表示何时发生姿态图形优化。

绘制扫描和姿态，每当一个环路闭合被识别，并验证视觉结果。该图显示重叠扫描和第一个环路闭合的优化姿态图。环闭合边作为红色链接添加。

firstTimeLCDetected = false;图;为i=10:length(scans) [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});如果~ isScanAccepted继续;结束%显示第一个检测到的环路闭合，如果你想看到完成地图创建过程，移除下面的if条件如果optimizationInfo。缺乏形状&& ~firstTimeLCDetected show(slamAlg,“姿势”,'离开');持有在;展示（Slamalg.70707apraph）;持有离开;firstTimeLCDetected = true;drawnow结束结束标题('第一个循环关闭');

可视化构建的地图和机器人的轨迹

绘制所有扫描后的最终构建的地图slamAlg对象。前面的为loop应该已经添加了所有的扫描，尽管只绘制了初始的循环闭合。

图显示（斜坡）;标题({“最终建成的环境地图”,“机器人的轨迹”});

目视检查建成地图与原平面图的比较

扫描和姿态图的图像被覆盖在原始的平面图上。你可以看到，在添加了所有的扫描和优化姿态图后，地图很好地匹配了原来的平面图。

建立占用网格图

优化的扫描和姿态可以用来生成一个occupancyMap，将环境表示为一个概率的占用网格。

[扫描，优化] = ScanSandpose（Slamalg）;地图= buildmap（扫描，优化，mapresolution，maxlidarrange）;

可视化填充激光扫描和优化的姿势图填充的占用网格图。

图;显示(地图);持有在显示(slamAlg.PoseGraph'ids','离开');持有离开标题(“使用激光雷达SLAM构建占用网格地图”);