点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

上篇文章讲解了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.bagversion:     2.0duration:    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 GBmessages:    41623compression: 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，关闭后端优化与回环检测功能．

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文件的如下两行设置成如下所示，启用后端优化功能，但不启用回环检测功能．

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

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

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

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

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

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

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

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

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

3.3.1 运行结果

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

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

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

这次的地图在圆弧型建筑这没有产生叠图现象!!!

非常棒!

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

那这是为什么呢？

3.3.2 回环检测

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

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

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

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

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

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

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

4 总结与Next

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

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

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

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

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

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

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~