从文件加载激光扫描数据

加载由在室内环境中从移动机器人收集的激光扫描组成的下采样数据集。每两个扫描之间的平均位移约为0.6米。

这offlineslamdata.mat.文件包含扫描变量，它包含本例中使用的所有激光扫描

加载（“offlineSlamData.mat”);

提供了用于说明性目的的机器人的平面图和近似路径。此图像显示映射的相对环境以及机器人的近似轨迹。

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

创建一个Lidarslam.目标，并设置地图分辨率和最大激光雷达范围。本例使用Clearpath Robotics™的Jackal™机器人。机器人配备了SICK™TiM-511激光扫描仪，最大射程为10米。设置最大激光雷达范围略小于最大扫描范围(8m)，因为激光读数在最大范围附近不太准确。将网格地图分辨率设置为每米20个单元格，这将提供5cm的精度。

maxlidarrange = 8;mapresolution = 20;Slamalg = Lidarslam（Mapresolution，MaxLidarrange）;

下面的闭环参数是经验设置的。在环路闭合识别过程中，采用较高的环路闭合阈值有助于排除误报。然而，记住高分匹配可能仍然是糟糕的匹配。例如，在具有相似或重复特征的环境中收集的扫描更有可能产生假阳性。使用更高的环闭搜索半径允许算法搜索更大范围的地图周围的环闭的当前姿态估计。

Slamalg.LoopClosurethreshold = 210;Slamalg.LoopClosuresearchRadius = 8;

观察初始10次扫描的地图构建过程

的增量添加扫描斜坡目的。如果添加到地图，则打印扫描号。如果扫描之间的距离太小，则对象拒绝扫描。首先添加前10个扫描以测试您的算法。

为了i = 1：10 [iscanacceped，loopclosureinfo，优化info] = addscan（斜坡，扫描{i}）;如果isScanAccepted流(添加扫描%d \n'， 一世）;结尾结尾

添加扫描1添加扫描2添加扫描3添加扫描4添加扫描5添加扫描6添加扫描7添加扫描8添加扫描9添加扫描10

通过绘制扫描和追踪的扫描来重建场景斜坡。

数字;展示（史册）;标题（{'环境地图'那'初始10扫描的姿势图'}）;

观察闭环的效果和优化过程

继续在循环中添加扫描。当机器人移动时，应自动检测循环闭环。每当识别出环闭时，都会执行姿势图优化。输出优化in.有一个领域，IsPerformed，表示当姿态图优化发生时…

每当识别循环闭合并在视觉上验证结果时，请绘制扫描并姿势。该图显示了第一环闭合的覆盖扫描和优化的姿势图。作为红色链路添加环形闭合边缘。

firsttimelcdetected = false;数字;为了i = 10：长度（扫描）[iscanacceped，loopclosureinfo，优化info] = addscan（斜坡，扫描{i}）;如果〜Isscanceped继续；结尾％可视化第一个检测到的循环关闭，如果要查看％完整的地图构建过程，删除以下IF条件如果OptimizationInfo.isperformed &&〜Firsttimelcdeted Show（Slamalg，'姿势'那“关闭”);抓住在；显示(slamAlg.PoseGraph);抓住从；firsttimelcdetected = true;drawn结尾结尾标题（“第一循环关闭”);

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

在所有扫描添加到斜坡目的。以前的为了尽管仅绘制初始环路闭合，但循环应该添加所有扫描。

图显示(slamAlg);标题（{'环境的最终建造地图'那'机器人的轨迹'}）;

与原始平面图相比，目视检查内置地图

扫描和姿势图的图像覆盖在原始平面图上。您可以看到地图在添加所有扫描并优化姿势图后匹配原始楼层平面匹配。

构建占用网格图

优化的扫描和姿势可用于生成一个占领子，将环境表示为一个概率占用网格。

[scans, optimizedpose] = scansandpose (slamAlg);map = buildMap(扫描，优化的姿势，地图分辨率，maxLidarRange);

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

数字;显示地图）;抓住在展示（Slamalg.707apraph，“id”那“关闭”);抓住从标题（'占用网格地图使用LIDAR Slam'建造');