上篇文章讲解了Karto的前端是如何工作的.

这篇文章将slam_karto中的后端优化部分的代码添加进lesson6中，我将带着大家一起体验后端优化的作用与功能．

1 室外数据集

由于上一篇文章使用的数据包的场景太小了，室内只有30m的范围，来回走一圈前端产生的累计误差不明显，所以，我上传了一个新的数据集．

由于之前没做好规划，整个教程弄了好几个数据集，其实只需要一个室内的，一个室外的就够了．．．我自我检讨．

1.1 数据集简介

新数据集的名字为 lesson6-rslidar-outdoor-gps.bag．

这是在室外环境下，绕着园区的一排花坛走了一圈，使用了速腾的16线雷达，RTK级别的GPS，lpms的6轴的IMU，四轮差速小车作为实验平台．

记录了

  • imu数据, topic名字为imu
  • 里程计数据, topic名字为odom_scout
  • GPS的经纬度数据, topic名字为 fix
  • 双天线得到的航向数据, topic名字为 heading
  • 16线雷达的点云数据, topic名字为rslidar_points
  • 由pointcloud2Laserscan包形成的单线雷达数据, topic名字为front_scan
  • TF数据: odom->footprint的tf
  • tf_static数据: footprint->base_link->front_laser_link, imu_link的 tf_static

详细信息如下所示

$ rosbag info lesson6-rslidar-outdoor-gps.bag 
path:        lesson6-rslidar-outdoor-gps.bag
version:     2.0
duration:    3:33s (213s)
start:       Dec 01 2020 15:44:09.49 (1606808649.49)
end:         Dec 01 2020 15:47:43.48 (1606808863.48)
size:        2.1 GB
messages:    41623
compression: none [2064/2064 chunks]
types:       geometry_msgs/QuaternionStamped [e57f1e547e0e1fd13504588ffc8334e2]
             nav_msgs/Odometry               [cd5e73d190d741a2f92e81eda573aca7]
             sensor_msgs/Imu                 [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/LaserScan           [90c7ef2dc6895d81024acba2ac42f369]
             sensor_msgs/NavSatFix           [2d3a8cd499b9b4a0249fb98fd05cfa48]
             sensor_msgs/PointCloud2         [1158d486dd51d683ce2f1be655c3c181]
             tf2_msgs/TFMessage              [94810edda583a504dfda3829e70d7eec]
topics:      /fix               1070 msgs    : sensor_msgs/NavSatFix          
             /front_scan        4127 msgs    : sensor_msgs/LaserScan          
             /heading            214 msgs    : geometry_msgs/QuaternionStamped
             /imu              21400 msgs    : sensor_msgs/Imu                
             /odom_scout        4279 msgs    : nav_msgs/Odometry              
             /rslidar_points    4126 msgs    : sensor_msgs/PointCloud2        
             /tf                6406 msgs    : tf2_msgs/TFMessage              (2 connections)
             /tf_static            1 msg     : tf2_msgs/TFMessage

这是一个数据比较全的数据包了，而且行走轨迹存在一个回环．

由于存在16线点云的数据，所以数据集是2G，压缩成zip后是700M．

1.2 数据集链接

数据集的链接为
https://pan.baidu.com/s/1UB7mjw3vhXlItsaqdNUw-g
提取密码为 slam

我就将我所有使用的数据集都放在腾讯文档里了, 腾讯文档的地址如下：

https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2

2 只使用Karto的前端进行室外2D建图

2.1 新增代码简述

本篇文章在上一篇文章的添加了后端优化的代码，相应的文件为：
lesson6/include/lesson6/spa_solver.h
lesson6/src/spa_solver.cpp

同时也对karto_slam.cc进行了更改，添加了根据参数来配置是否启用后端优化与回环检测的功能．

更改配置文件中的相关内容，可以实现是否启用后端优化，与是否启用回环检测功能．

新增了跑室外建图的launch与config文件:
lesson6/launch/karto_slam_outdoor.launch
lesson6/config/mapper_params_outdoor.yaml

2.2 运行代码

下载数据包之后需要将launch中的bag_filename更改成您实际存放bag的目录名。

首先，将mapper_params_outdoor.yaml文件的如下两行设置成false，关闭后端优化与回环检测功能．

# back-end
use_back_end: false     # 是否启用后端优化
do_loop_closing: false  # 是否启用回环检测

通过如下命令运行本篇文章对应的程序

roslaunch lesson6 karto_slam_outdoor.launch

2.3 结果分析

运行结束后将产生如下的地图，每次运行的效果不同，下图是运行效果最差的一次的截图．

这张地图空白区域的长度大概在100m．宽度大概为27m，分辨率是0.05m．

实际运行时Rviz中还会显示出一条红色的轨迹，这个轨迹是里程计的数据，里程计的数据存在初值和初始方向，所以里程计的数据与机器人的位姿没有重合．
在这里插入图片描述将这段圆弧型建筑构建成的地图放大，可以看到，产生了严重的叠图现象．

在这里插入图片描述

3 使用Karto的前端与后端进行室外2D建图

3.1 后端优化模块简介

由于前端的扫描匹配会存在累计误差, 这个误差随着时间的增长会逐渐增大.

而且, 扫描匹配是将当前scan与前一段时间内的scan形成的局部地图进行匹配, 没有使用到从开始建图到当前时刻的所有的scan数据.

所以, 大佬们想了个方法, 将所有的scan数据一起做个优化, 降低其累计误差. 这就是后端优化.

在图优化SLAM中，一般将前端和后端分开处理,

前端需要在一定范围内的地图中进行搜索与匹配, 找到当前scan的潜在约束关系.

后端优化的目的是 是寻找一个节点间的配置关系使得节点间约束的测量概率最大. 通俗点讲, 就是对所有节点, 及其约束 进行优化, 找到最接近实际情况的一组解, 这组解代表着新的节点位姿与新的节点间的约束.

后端优化后的位姿越准, 那么, 前端生成的局部地图就越准, 进行的扫描匹配的结果也就越准确, 越快速. 同时, 也能够将前端的累计误差减小.

因此，后端优化问题的精度和性能，对整个建图系统具有决定性的影响。

更加具体的介绍可以看下这篇文章

K. Konolige, G. Grisetti, R. K¨ummerle, B. Limketkai, R. Vincent, Efficient Sparse Pose Adjustment for 2D Mapping, In Proc. of Int. Conf. on Intelligent Robots and Systems (IROS), Oct. 2010.

那为什么之前不用后端优化呢?

由于将所有位姿与地图点放到一起做优化, 形成的矩阵的维度是非常大的, 没法求雅克比矩阵.

近些年, 计算速度与计算能力的提升缓解了这个问题, 但依然求雅克比矩阵很难.

直到2010年, 有大神发现了所有位姿与地图点形成的矩阵是非常稀疏的, 是一种箭头型的结构.

利用矩阵的稀疏性, 可以通过某种方式将这个稀疏的大矩阵重新设置成一个更紧凑, 维度更小的矩阵. 使得了雅克比矩阵可以求出来.

3.2 启用后端但不启用回环检测

首先，将mapper_params_outdoor.yaml文件的如下两行设置成如下所示，启用后端优化功能，但不启用回环检测功能．

# back-end
use_back_end: true     # 是否启用后端优化
do_loop_closing: false  # 是否启用回环检测

再次通过上边的roslaunch命令启动程序，运行结束后将产生如下结果．

在这里插入图片描述
上图的红色轨迹是里程计的数据，由于存在初值，所以里程计的值与机器人没有重合．

蓝色轨迹是由后端优化 优化之后的节点和边．

节点就是某一时刻的雷达数据，用红色小球表示．

边就是两个节点之间的约束，也就是两个节点间的坐标变换，用蓝色线条表示，

由于前端的扫描匹配存在累计误差，通过后端优化可以降低前端的累计误差．这些误差并不是被移除了，而是被平均分数到每个节点中去了．

首先，放大一下看看约束的效果．
在这里插入图片描述可以看到，即使是启用了后端优化功能，最终建完图之后，这块圆弧型建筑物的地图还是叠图了．

可见，后端优化模块并没有将累计误差完全消除掉．

3.3 启用后端同时启用回环检测

3.3.1 运行结果

再看看启用后端优化与启用回环检测的效果．

将mapper_params_outdoor.yaml文件的如下两行设置成如下所示，启用后端优化功能，但不启用回环检测功能．

# back-end
use_back_end: true     # 是否启用后端优化
do_loop_closing: true  # 是否启用回环检测

再次通过上边的roslaunch命令启动程序，运行结束后将产生如下结果．
在这里插入图片描述这次的地图在圆弧型建筑这没有产生叠图现象!!!

非常棒!

放大一下看看约束的效果，图中，轨迹的起始点与终止点有一条很短的蓝色的线，这就是回环约束．
在这里插入图片描述优化之后的图很整齐．最终的建图效果是这样的
在这里插入图片描述

那这是为什么呢？

3.3.2 回环检测

上面说的，后端优化不能完全消除累计误差，那么有没有一种方法可以消除累计误差呢？

答案就是回环检测．回环检测可以极大地消除累计误差，但也不是完全消除掉．

回环检测的意思就是，机器人行走了一段时间后，回到了一个曾经来到过的地方，这个地方对应的栅格地图，应该是同样的．

所以，可以通过某种方法检测机器人是否回到了曾经来到过的场景．如果检测到机器人回到了这个场景，那么当前的位姿与之前看到这个场景的位姿应该是接近的，并且可以根据相同的场景求得这两个位姿间的相对坐标变换．

可以将这个相对坐标变换，作为约束，也就是边，添加到后端优化中．这个约束就叫做回环约束．

由于这两个节点看到的场景就是是同一个现实场景，所以这个边的置信度非常高，权重很大，所以就可以极大地消除累计误差．

所以，回环检测的目的就是为了找到这个权重很高的约束，之后将这个约束放入到图结构中，一起做优化，来达到消除累计误差的目的．

4 总结与Next

代码里已经将slam_karto的后端优化模块放进来了,

本篇文章先对后端优化与回环检测模块进行了建图的对比测试, 从建图质量的角度分析了添加了后端优化与回环检测的功能的效果.

同时, 还简要介绍了后端优化的概念与回环检测的概念.

下篇文章先简要分析下slam_karto的回环检测与后端优化代码的实现方式.

再下篇文章将使用G2O来实现karto的后端优化的功能.

由于本人水平有限，难免会有些代码上的错误，如果您发现了错误或者不好的地方，请联系我，我会改正．

文章将在 公众号: 从零开始搭SLAM 进行同步更新，欢迎大家关注，可以在公众号中添加我的微信，进激光SLAM交流群，大家一起交流SLAM技术。

如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。

在这里插入图片描述