我手写了个SLAM算法！

1、前言
前一段时间看过我文章的都知道，我打算写一个SLAM源码阅读的文章，然后，我就去读了Gmapping的源码，感受良多，不足的地方是源码太乱了，阅读起来真的不香。于是就有了这篇文章，在我仔细阅读之后，我在源码的结构基础之上，进行大刀阔斧的删减和更改之后得到一个易于阅读的建图算法功能包，极大的降低了代码量，极大的提升了阅读体验。在这里将该算法功能包分享给大家，希望需要的朋友，善待它。

之前文章链接：不可错过的gmapping算法使用与详细解释

ps：为什么是gmapping呢？因为他相对简单呗，直观容易理解，个人感觉，先吃透个简单的比较好。

2、gmapping源代码和修改后的代码的对比
2.1、文件数量的对比：
gmapping开源代码的文件树如下图：非常多且乱

 

my_slam_gmapping代码的文件树如下图：

 

2.2、文件大小对比：
gmapping开源代码的文件占 872kb 空间。

my_slam_gmapping代码的文件仅占 148kb 的空间，仅为原本代码的 1/6 左右大小。

2.3、建图效果对比：
主板使用的是TX2（Ubuntu 16.04），激光雷达是RPLIDAR A2M8（10HZ），相同的环境，相同的参数，同一台车。

gmapping算法功能包建图效果如下图：

 

my_slam_gmapping算法功能包建图效果如下图：

 

两个算法功能包参数设置一致，效果几乎无差别，说明我写的算法功能包，在效果上还是可以替代gmapping源码的。

这样的话，大家不就拥有了一套自己能轻松读懂，并且可以在实际场景中使用，而且也容易对算法进行修改了，岂不美哉！

3、如何使用该算法功能包
3.1、my_slam_gmapping功能介绍
my_slam_gmapping算法功能包除了图片中划掉的发布机器人姿态分布熵的topic之外，其他功能还是gmapping算法一致。

 

3.2、my_slam_gmapping使用说明
若ROS 版本为ROS Kinetic，则可以直接将my_slam_gmapping功能包复制到各自工作空间的src文件夹下，然后catkin_make编译，再之后使用roslaunch命令启动功能包，命令如下：

roslaunch my_slam_gmapping my_slam_gmapping.launch

1
若ROS 版本为其他版本，不可以直接使用，但是可以把该功能包的文件复制到各自的版本中自建的功能包里，自行编译也可行。

其他操作和gmapping算法功能包无差别，这里就不介绍了。

4、my_slam_gmapping代码阅读
以下高清图在分享文件包中有。先仔细看完下面的图，对理解代码十分有好处。

4.1、代码文件说明

4.2、代码中各种对象的说明

 

 

4.3、代码运行流程
核心代码运行流程：

5、代码获取方式
在公众号：小白学移动机器人，发送：手写SLAM算法，即可获得下载链接。

此次分享是我对该代码修改的第一版，删减的并不是非常非常彻底，但是核心代码已经完全进行了删减和修改，并且对核心代码进行了详细的中文注释。之后的分享考虑将激光雷达运动畸变去除模块，直接加到该算法中，并算法进行彻底修改和优化。

考虑到，过多的注释可能会影响阅读体验，此次分享也有核心代码未注释版的算法功能包。