上篇文章讲解了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技术。

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

从零开始搭二维激光SLAM --- Karto的后端优化与回环检测功能对比测试与分析相关推荐

Karto的后端优化与回环检测功能对比测试与分析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达上篇文章讲解了Karto的前端是如何工作的. 这篇文章将slam_karto中的后端优化部分的代码添加 ...
从零开始搭二维激光SLAM --- 基于GMapping的栅格地图的构建
上篇文章讲解了如何在ROS中发布栅格地图,以及如何向栅格地图赋值．这篇文章来讲讲如何将激光雷达的数据构建成栅格地图．雷达的数据点所在位置表示为占用,从雷达开始到这点之间的区域表示为空闲． 1 GM ...
从零开始搭二维激光SLAM --- 激光雷达数据效果对比
我们知道,不同品牌的激光雷达产生的数据是不一样的,那这些不同点是如何影响建图效果的呢? 这篇文章就是来分析这个问题,将从不同光强下的点云效果,不同夹角下的点云效果,以及 1 激光雷达的技术指标激光雷 ...
从零开始搭二维激光SLAM --- 基于ICP的帧间匹配
上一篇文章讲解了如何将激光雷达的sensor_msgs/LaserScan格式转换成pcl::PointCloud< pcl::PointXYZ>格式, 本篇文章将要讲解如何使用这个格式调 ...
从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析
这篇文章将带领大家推导一下hector slam论文中的公式．之后再对这部分公式对应的代码进行讲解下． markdown打公式太费劲了,所以我用手写了．(懒) 然后csdn又限制了图片文件大小,我是照 ...
二维激光SLAM( 使用Laser Scan Matcher )
目录一.Laser Scan Matcher安装配置二.二维激光定位一.Laser Scan Matcher安装配置 ROS自带的laser_scan_matcher库,使用的是CSM(Cano ...
3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析
3D激光SLAM:LeGO-LOAM---两步优化的帧间里程计及代码分析前言利用地面点优化利用角点优化代码部分 gazebo测试前言 LeGO-LOAM的全称是 Lightweight an ...
【SLAM十四讲】ch11 回环检测词袋法实验得出相似分数后计算PR曲线 VPR实验编辑中
[SLAM十四讲]ch11 回环检测词袋法实验得出相似分数后计算PR曲线 [SLAM十四讲]ch11 回环检测词袋法实验得出相似分数后计算PR曲线 DBow3库安装 ch11编译 ch11 词 ...
SLAM之视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
综述 | SLAM回环检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨任旭倩来源丨计算机视觉life 在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一 ...

从零开始搭二维激光SLAM --- Karto的后端优化与回环检测功能对比测试与分析