原来的计划是从这篇文章开始自己搭一个新的SLAM框架的，结果由于水平不够，代码不知道从哪下手，不知道如何设计代码结构，弄了一周没啥进展．

后来就想着先把Karto拆解成比较独立的几个模块，像Cartographer那样，分别实现扫描匹配部分，建图部分．并新增使用imu与里程计的估计值，作为激光雷达扫描匹配的先验．

但是由于Karto的代码耦合度实在是太高，拆解的工作量太大了，又弄了一周还是没啥进展，无奈只能放弃。所以这篇文章的间隔有点久。。。

所以学习SLAM还是得一步一步学习，不能好高骛远．搭一套SLAM不光要对算法实现，数学模型有深入理解，同时也要对代码架构有一定的理解．

之后我静下心来，在工作之余来看Karto的代码．其实去年3月份的时候看过一遍Karto的代码，只不过后来忘记了．花了一周的时间，将Karto代码的前端和建图部分的代码看完了．

我认为，Karto最重要的贡献就是引入了后端优化与回环检测．在Karto之前诞生的激光SLAM如GMapping，Hector，都是没有后端优化与回环检测的．

本篇文章先带领大家体验Karto的前端与建图部分，并对其扫描匹配的实现进行简单讲解．

1 Karto简介

Karto与GMapping类似，都是先有c++下的实现，然后有人使用ROS对原本的c++库进行了封装．

Karto的出现在激光SLAM的历史上是个里程碑的事件．在Karto之前的激光SLAM中，如GMapping, Hector 都只有前端部分，也就是使用雷达数据进行扫描匹配以及建图2个功能．

Karto将后端优化与回环检测引入到激光SLAM中，通过位姿图结构的优化，来减小累计误差．

Karto的c++版本的github地址为
https://github.com/ros-perception/open_karto
这是ROS官方的github仓库里的代码．Open_karto这个项目中的类的定义，全集中在两个头文件 Karto.h 与 Mapper.h中，Karto.h 有7000多行，Mapper.h 有2000多行，这就是我想将它拆开的原因，读代码读起来简直太难受了．

github上还有一个open_karto的项目
https://github.com/skasperski/OpenKarto
这个项目中的类的定义是分开的，不知道是不是原作者的仓库．

ROS官方为了在ROS 中使用Karto，实现了一个叫做slam_karto的项目，为如何在ROS中使用open_karto的例子，代码与slam_gmapping很像．地址为
https://github.com/ros-perception/slam_karto

其实slam_karto的效果还可以，但是由于使用ROS官方的代码中的参数配置很随意，导致建出的图的效果一般，最终使得这个slam的热度始终不高．

再加上后来cartographer的出现，Karto更是无人问津．其实cartographer的子图的概念在karto中就已经存在了．

本篇文章将简要介绍下Karto的前端与建图，并简要介绍下Karto的扫描匹配是如何实现的．

之后会对Karto的回环检测进行分析，最后对Karto的后端优化进行分析．

由于Karto中并没有实现后端的具体求解方法，而是将后端的求解方法定义成立了接口类，以方便后续进行具体的实现．

所以，我的计划是首先分析下slam_karto中使用的sparse-bundle-adjustment 优化求解方法，之后再分别使用G2O，Ceres，与GTSAM这三个比较出名的优化库进行后端优化求解的实现．以达到学习后端优化的目的．

2 代码

2.1 获取代码

代码已经提交在github上了，github的地址为
https://github.com/xiangli0608/Creating-2D-laser-slam-from-scratch

推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.

本篇文章对应的代码为 Creating-2D-laser-slam-from-scratch/lesson6

其中include文件夹与src文件夹中的内容就是ROS官方的slam_karto的代码的内容，我并没有做过多更改，只是将其后端优化部分与发布后端约束的可视化部分暂时去掉了，这部分的内容会在后续文章中讲解．

2.2 代码解析

接下来简单说明一下Karto的前端匹配与建图部分的代码实现．更具体的分析可以去看我2020年的文章

karto探秘之文章索引 https://blog.csdn.net/tiancailx/article/details/105437590

在这些文章分别讲解了 slam_karto的代码，Karto中的参数的含义解析，Karto中比较重要的数据结构，Karto的扫描匹配，Karto的回环检测与后端优化，以及Karto的栅格地图的生成．对各个关键函数进行了比较详细的注释，并对其方法进行了简要说明．

2.2.1 扫描匹配方法简介

获取先验位姿

首先，通过TF获取里程计的值，作为当前scan的预测位姿，将这个预测位姿当做扫描匹配的先验，之后根据这个先验位姿，进行当前scan与局部地图的匹配．

生成局部地图

Karto的扫描匹配与Hector的扫描匹配方式不同。Hector是通过将当前scan与整张地图进行匹配，Karto的扫描匹配是通过当前scan与局部地图进行匹配．

局部地图是如何生成的呢？

Karto中使用了滑动窗口法来进行局部地图的生成。

滑动窗口法生成局部地图

Karto使用了一个队列保存最近24米范围内的所有雷达数据。

每新加入一帧雷达数据，就要判断一下队列头部的雷达数据与当前雷达位置间的距离是否大于24米，如果大于24米就将其删掉，并继续判断队列头部雷达数据对应位置与当前位置间的距离，只到所有雷达数据与当前位置小于24米为止。

这就是滑动窗口法，通过窗口的滑动，始终维持一定范围内的雷达数据。

通过将滑动窗口内的所有雷达数据写成分辨率为0.01的栅格地图，这样就生成了局部地图。

局部地图不包括当前的scan，所以扫描匹配是当前scan与过去的一段时间内scan形成的栅格地图间的匹配。

由于局部地图的范围很小，占用内存很小，所以可以将局部地图设置成0.01米的分辨率，来达到更高精度的匹配结果。

扫描匹配

Hector中的扫描匹配是通过求解最小二乘的方式进行的。

Karto没有这么做，Karto的扫描匹配方法是类似于通过暴力求解的方式，遍历一定范围内所有的平移与旋转的位姿，选出得分最大的一个位姿．

最大的得分可能对应多个位姿，这时就对这些位姿求平均值，来作为匹配后的位姿。

在遍历所有的旋转雷达数据的时候，Karto使用了角度查找表来进行加速搜索。

角度查找表

Karto并没有真正的将scan进行旋转，而是将一帧scan的每个点进行旋转，将旋转之后的点对应于栅格地图的索引记录在二维数组中。

数组的第一维代表着scan旋转的角度，第二维代表着这帧scan经过旋转后的点在栅格地图中的索引。

每个旋转角度对应一个角度查找表。一个角度查找表中存储的是当前scan的每一个数据点 经过旋转后对应着栅格地图中格子的索引值。

这样，在计算得分的时候就可以通过查表的方式获取旋转后的点所在的栅格了，加速了计算。

位姿的得分

位姿的得分是通过雷达点对应于栅格地图中的格子的占用值来确定的，所有雷达点对应格子的占用值的总和除以点的个数乘以100．

例如：
一帧雷达只有2个点，第一个雷达点的位置在栅格地图中的一个占用状态的栅格上，对应的占用值为100，第二个点雷达点的位置在栅格地图中的一个空闲状态的栅格上，对应的占用值就是0，那么这帧雷达数据的得分即为：（100 + 0）/ ( 2 * 100) = 0.5

对距离先验位姿远的位姿进行惩罚

由于是遍历一定范围内的所有位姿，这些位姿与先验位姿就会有远有近。

Karto在计算得分时，根据当前位姿与先验位姿的距离进行了惩罚，也就是乘上不同的权重。

距离先验位姿越近的位姿的权重越高，距离越远的位姿越不准，权重越低，权重是乘在得分上的．

保存位姿与当前scan

经过扫描匹配后，最终得到了当前scan在map坐标系下的最优位姿，将这帧scan与这帧scan对应的位姿保存下来，用来建图。

2.2.2 建图思路简介

Karto的建图的思路与Hector有较大区别。

Hector是将匹配后的scan按照匹配后得到的位姿直接写成地图，不会对scan进行保存。

Karto是将所有匹配后的scan保存下来，按照移动的距离与时间来更新地图。

每次更新地图都是将之前的地图删除掉，重新生成一张最新的地图。

地图是通过将所有保存下来的scan写成地图得到的。

上述就是Karto的扫描匹配与生成地图的实现的简要介绍，是我按照我自己的理解将全部的过程高度概括的。在生成局部地图的时候还用到了核函数，来降低噪点的影响，但是这块我有点没看懂，就不说了。

3 运行

3.1 数据集

本篇文章使用的数据包为之前使用过的lesson1.bag, 请在网盘获得

链接：https://pan.baidu.com/s/1OOgMHkykjPyqkyopG7IsGw
提取码：slam

并且，我将文章使用的所有数据包的网盘链接都记录在了腾讯文档中，其地址如下：

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

3.2 运行

下载之后将launch中的bag_filename更改成您实际的目录名。

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

roslaunch lesson6 karto_slam.launch

3.3 运行结果与分析

启动之后，会在rviz中显示出如下画面．

这张地图是用里程计与雷达共同建的结果，而里程计的角度是通过IMU赋予的，所以相当于三者的融合建图．由于只用了前端匹配和建图两个模块，可见上述地图右侧在长走廊的尽头发生了明显的偏转．

上述地图是使用12米范围内的雷达数据建成的，接下来我们将更改这个参数为20米，再看看建图效果．

手动更改lesson6/config/mapper_params.yaml文件，更改参数use_scan_range为20，再次运行程序将得到如下所示的地图．

这个参数代表这对雷达数据使用的距离，如果是12，就只使用12米范围内的雷达数据，如果是20，就使用20米范围内的雷达数据．

同时，Karto的局部地图也和这个参数有关，局部地图的大小是这个参数的2倍多一点，多出来的部分是给核函数用的．

由于这个雷达是2000块钱的雷达，最远距离为25m，距离远的数据点的精度很差，而Karto的建图部分是使用所有的雷达点生成地图．

所以当这个参数改成20米后，由于雷达的数据在远距离的跳动，使得地图生成了更多的毛刺，也使得走廊两侧的墙壁黑色的外边还有白色地图的现象．

所以，SLAM的参数真的很影响建图效果，每个场景，不同的厂家的雷达，都需要不同的参数已达到最好的建图效果．

4 总结与Next

本篇对Karto的前端部分进行了理论说明，并对Karto的一个参数进行了对比分析，对比了参数分别为12与20时的建图效果．

本系列教程之前的文章一直在写SLAM的前端部分和建图部分，接下来的文章将开始介绍SLAM的后端部分，先从Karto的后端的代码，以及Karto使用的 sparse bundle adjustment 优化算法开始分析．

由于Karto的后端的接口留出来了，可以很方便的进行后端优化算法的替换．所以借着Karto的代码框架，将分别使用G20，Ceres，以及GTSAM三个优化库进行后端优化模块的实现，已达到对这三个库的学习的目的．

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

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

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