激光slam算法笔记（一）

前言

对a-loam的流程以及一部分细节发布自己的见解

a-loam

我们指导a-loam是在loam的基础上进行简化之后得到的版本，它的src包中只包含四个cpp文件，算是比较简单的slam算法了，这四个cpp分别是：kittiHelper.cpp、scanRegistration.cpp、laserOdometry.cpp、laserMapping.cpp

kittiHelper.cpp

这个cpp文件的主要功能是读取kitti odometry的数据集的数据，里面数据有很多种，但是对于我们这个算法有价值的是其中的点云/velodyne_points，其余的四类都是提供给rviz的，目的是进行可视化。

scanRegistration.cpp

这个cpp文件会接收上一个cpp文件输入的点云/velodyne_points，然后根据曲率选出四类点：sharp、less_sharp 、flat、 less_flat

在这里我们谈一谈每一次输入的点云，首先我们假设这个激光雷达的视野范围是足够大的，然后水平视角是360度。那么如果我们使用的是一个单线的激光雷达的话，一个周期我们的得到的就是一个周围水平一圈的点云，但是我们只是得到了一个二维的点云，但是我们知道我们是做三维激光slam的，所以我们需要使用多线的激光雷达，对于每一个线束，它们的水平视角都是360度，然后每一个线束都会有一个基于水平面的俯仰角（都在正负15度之间），这样我们就可以得到三维的点云了。

值得注意的是，我们从kittiHelper.cpp接收的点云/velodyne_points其实是一个个线束的子点云，我们需要将其排序合并成一个整体点云，在合并的过程中，按照我们的习惯，会忽略每一个子点云的前5个和后5个点，因为开始和结束处的点云容易产生不闭合的“接缝”。

再然后我们会计算每个点的曲率，再根据曲率选出2个sharp 20个 less_sharp 4个flat 40个less_flat。同时为了避免特征点过于密集，每当我们选中一个点之后我们会将这个点周围某个阈值距离内的点忽略掉。在之后，我们把选出的特征点和经过处理的当前帧点云传入laserOdometry.cpp

laserOdometry.cpp

在这个cpp文件中我们会进行帧与帧之间的特征点匹配，计算当前帧的位姿。当然如果是第一帧，我们不进行匹配，因为没有参照嘛，只是将它的特征点保存起来。

然后对于后续点云中的角点，我们会首先找到在上一帧中和它距离最近的点a，然后在寻找a所在线束的相邻线束中的距离最近的点b，这两个点形成一条直线，通过计算得到该角点经过位姿转换之后与这条直线的距离，然后对每个角点都进行相同的处理，然后使得总体距离最小化，进行整体优化（ICP）得到最优位姿。

对于点云中的平面点也是类似的道理，不同的是我们在我们找到点a之后，我们需要再去找点b和点c，因为我们想要形成一个平面，两点成线，最少三点才能成面嘛。一般来说，a和b是同一线束上的，c是相邻线束上的点。
然后对于形成的平面，得到平面的法向量，然后计算得到该平面点经过位姿转换之后和这个平面的距离，也是对每个平面点都这么处理，最后再整体优化。

在完成上述操作之后，会将得到的位姿、特征点以及点云传入laserMapping.cpp

laserMapping.cpp

laserMapping.cpp接收来自laserOdometry.cpp的位姿、特征点和点云，注意此时的位姿是一个粗略估计的位姿，经过laserMapping.cpp后会发布精估位姿。

首先我们会进行坐标系之间的转换，因为我们的点云中的点是在雷达坐标系中的，而我们由以前的所有帧形成的地图是在世界坐标系下的。然后我们去求当前帧的精估位姿（因为利用了以前的所有帧进行位姿估计，所以得到的结果会更加精确），当然因为我们的无人车在不断运动，所以我们的地图可能会很大，我们如果使用全部的地图的话是很浪费的，所以我们一般情况下只选取当前帧附近的某个距离的内的点云作为参照的地图点云，这个距离一般取决于激光雷达的观测距离，超出观测距离的点云数据对于求位姿是没有意义的，因为它们在当前帧中根本没有对应，比如：论文里面的选取范围是周围的10m的立方体，而在源代码中是使用的周围5x5x3的cube（分别在i、j轴正负扩展两个，在k轴正负扩展一个），每一个cube是50m。

截取了局部地图之后，我们进行类似laserOdometry.cpp中的点到线和点到面的距离计算，但是这里，我们是寻找当前某个特征点的最邻近的五个点，对于角点，我们以这五个点的均值点为中心，以5个点的主方向向量（类似于PCA方法）为方向，作直线，求该角点与直线的距离；对于平面点，我们寻找这五个点组成的平面的法方向，求该平面点到平面的距离，然后也是使距离最小化，得到精估位姿。

最后将当前帧的特征点加入到地图中，进行融合，然后对于加入新特征点的cube进行降采样，然后更新点云地图并发布。