经过一段时间的钻研和积累，终于能够复现出一套完整的二维激光SLAM算法了，在复现的过程中，并没有参考某一个开源的框架（如hector，Cartographer等）进行编写，而是在理解了原理以后，根据原理进行实现，这个过程比起照着开源写一遍，碰到了更多的bug，但是在修复bug的过程中也收获了更多，写一篇文章简单记录一下这个过程。
首先还是惯例，先放效果图：
可以看到建图效果还是非常不错的，在gazebo中机器人已经是以一个较快的速度运动了，但是建图的速度完全能够跟上，并且准确度也比较高，没有出现叠图和漂移的情况。
我们查看一下终端输出：
整体个SLAM过程的整体用时只需要0.03秒不到就能够完成，一般的雷达扫描频率都在10HZ左右，换算过来就是0.1秒，因此我们的SLAM算法在雷达的一次扫描周期内可以完成3次运算，实时性绝对拉满没的说，对比了同样的环境还跑了hector和gmapping作为对比，hector的总体运行时间大概是0.2秒左右，而gmapping到了0.4秒，光速度上来说，我们的算法是有绝对优势的。
下面简单讲解一下整个项目的实现过程。
目前而言，激光SLAM的主流框架都是前端，后端，回环三个部分，前端进行扫描匹配，后端进行优化，回环消除累计误差，我们就从这三个角度分别介绍我们的算法实现过程。
前端
本项目的前端直接套用了上一篇博客实现的雷达里程计，通过PL-ICP算法进行帧间匹配求得相对位姿变换，这一部分可以参考前一篇文章，不多叙述。
SLAM项目：从0开始复现2D激光里程计，并利用自己的雷达或者gazebo运行，详细解释原理及代码实现过程_weixin_43890835的博客-CSDN博客
后端
在激光SLAM中，后端主要进行优化和建图，主流的SLAM框架都会单独分一个线程来执行后端，因为不管是建图还是优化，都需占用相当的计算资源，为了保证实时性，采取多线程处理是一个不错的方案，同时为了有意的去限制后端优化和建图的规模，也有很多小tricks，比如建立keyframe，采用滑动窗口，丢掉观测只优化位姿的位姿图等等。
在建图部分，本文采用了最经典的占据栅格地图，其本质就是将连续的地图离散化，变成一个一个的栅格，如果被激光hit，则设定那个格子为占据，否则则设定为空闲，没有被激光扫到的格子状态为未知，如果用三个值来表示的话，占据就是100，空闲就是0，未知就是-1。随后我们维护一个数组，把每个格子的状态放进去，在建图的时候把这个一维数组进行二维展开，再根据每个格子的状态用颜色进行区分（这部分系统会自动帮我们完成），就完成了一个最简单的占据栅格地图。
在建立地图的时候，有几点需要特别注意
首先就是坐标变换，这一步可以通过TF去理解，TF就是ROS中各个坐标系之间的变换关系，在ROS中几个坐标系的关系可以通过下面这篇文章快速学习了解，这里也不做多赘述。
激光SLAM与ROS中，map、odom、laser_link、base_link几个坐标系和坐标转换的理解_Soumes的博客-CSDN博客
我们知道MAP坐标系在ROS中就是代表世界坐标，而为雷达作为传感器，其数据处于laser坐标系下，如果我们直接用雷达的数据更新地图，那么在MAP坐标系下观察，这个地图的建立位置和更新位置都是错误的，最直接的表现就是地图无法跟随机器人的运动进行更新，而是有自己的想法，想在哪里更新就在哪里更新，为了防止这个情况的出现，我们需要做好laser-base_link-odom-map的坐标变换，把laser的数据变换到odom下，再用来更新地图，这样才能够保持和MAP的坐标系一致（因为map-odom一般为静态变换）
其次是地图的各个参数和建立位置，由于我们在建立地图的时候将连续的地图进行了离散处理，所以我们一定会引入一个离散系数，比如1m我们将其离散为10个格子，那么离散系数就是0.1，在设置地图参数，如地图的长宽，地图的原点坐标时，一定要记得乘以这个系数，否则因为尺度不同会导致很多问题。第二个是地图的建立位置，我们需要保持机器人在地图的中心，所以地图的建立位置应该以机器人的位置为参考，这样在RVIZ中进行可视化时才方便查看。
最后是地图更新的tricks，一般地图更新有两种方式，第一种是给每个栅格设定初值，如果被占据，那么就增大初值，如果是空闲，就减少初值，在发布地图时设定一个阈值，超过某个阈值则认为是占据，小于则认为是空闲，这种方式的好处就是可以消除偶然误差，引入动态能力，假如某一时刻，激光扫描出现了误差，误认为前方的空闲位置有障碍物，或者前方有人经过，导致了那一帧数据中是有障碍的，那么用这种方法，经过几次扫描之后，连续为那个误判的空格赋空闲值，那么这个偶然误差，或者动态障碍就会在几次扫描后重新变为空闲。第二种方式就是一刀切，如果扫到某一时刻有格子被占据，就给打100分，空闲就给打0分，并且不进行更新，这种方法的缺点就是如果出现偶然误差或者动态障碍，那么其导致的栅格会永远以被占据的状态留在地图上，好处就是由于不需要对地图进行反复的更新，地图建立的速度很快，在没有误差的情况下地图会比第一种方式建立的的·更“坚定”（就是不会变来变去的）
优化
待补充
回环
待补充