LOAM 代码解读

参考：LOAM:3D激光里程计及环境建图的方法和实现（一） - 知乎 (zhihu.com)

整体流程

LOAM的整体思想就是将复杂的SLAM问题分为：1. 高频的运动估计； 2. 低频（低一个数量级）的环境建图。

流程1：

Lidar接收数据，首先进行Point Cloud Registration，Lidar Odometry以10Hz的频率进行运动估计和坐标转换，Lidar Mapping以1Hz的频率构建三维地图。这样做主要是为了保证系统的实时性。

scanRegistration（将雷达点划分线束以及计算曲率分别保存）

主要功能是：对点云和IMU数据进行预处理，用于特征点的配准。其实就做了一个工作：那就是根据点的曲率c来将点划分为不同的类别（边/面特征或不是特征）

从main函数看出，该节点通过订阅器subLaserCloud订阅 /svelodyne_points消息，并由回调函数laserCloudHandler进行处理，订阅器缓存区大小为2；订阅器subImu订阅 /imu/data消息，由回调函数imuHanler处理，缓存区大小为50。此外，该节点通过发布器pubLaserCloud发布以不同线分类的点云的指针/velodyne_cloud_2，缓存区大小为2；发布器pubCornerPointSharp和pubCornerPointLessSharp发布特征角点 /laser_cloud_sharp和/laser_cloud_less_sharp，缓存区大小为2；发布器pubSurfPointFlat和pubSurfPointLessFlat发布特征角点 /laser_cloud_flat和/laser_cloud_less_flat，缓存区大小为2；发布器pubImuTrans发布IMU信息 /imu_trans，缓存区大小为5。下面从两个回调函数分别介绍这个节点的代码。

laserCloudHandler

该回调函数是这一节点的重点部分，主要功能是对接收到的点云进行预处理，完成分类。具体分类内容为：一是按照不同线（雷达线束），将点云保存在点云指针中；二是对其进行特征分类。

首先剔除异常点并计算起始和终止位置的角度。然后遍历所有点，根据角度计算结果将其划分为不同的线（雷达线束）：计算角度-->计算起始和终止位置-->插入IMU数据-->将点插入容器中。

imuHandler

这个回调函数相较于点云处理的那个真是亲切加和蔼，相信大多数同学都能读懂。

laserOdometry

主要功能就是利用相邻两帧的点云数据进行配准，即完成t时刻和t+1时刻点云数据的关联，并估计lidar的相对运动关系。

1. 初始化

2. 点云配准与运动估计

设定整个L-M运动估计的迭代次数为25次，以保证运算效率。迭代部分又可分为：对特征边/面上的点进行处理，构建Jaccobian矩阵，L-M运动估计求解。

主要思路就是：构建约束方程 -> 约束方程求偏导构建Jaccobian矩阵 -> L-M求解（借助欧拉角）。很简单吧！所以不要看代码有千八百行，但实际上也没那么复杂。

找t+1时刻的某个特征边/面上的点在t时刻下对应的配准点，论文作者给出如上图的思路。特征线：利用KD树找点i在t时刻点云中最近的一点j，并在j周围（上下几条线的范围内）找次近点l，于是我们把（j，l）称为点i在t时刻点云中的对应。特征面：与特征线类似，先找最近点j，在j周围找l，在j周围找m，将（j，l，m）称为点i在t时刻点云中的对应。

3. 坐标转换

laserMapping

经过前两个节点的处理可以完成一个完整但粗糙的视觉里程计，可以概略地估计出Lidar的相对运动。如果不受任何测量噪声的影响，这个运动估计的结果足够精确，没有任何漂移，那我们可以直接利用估计的Lidar位姿和对应时刻的量测值完成建图。但这就如同现实中不存在一个不受外力就能匀速直线运动的小球一样，量测噪声是不可避免的，因此Lidar位姿估计偏差一定存在。我们可以利用已构建地图对位姿估计结果进行修正。

建图节点起到了什么作用？在回答上一个问题时也已经提到了，它的作用就是优化Lidar里程计的位姿估计结果。怎么做呢？没错，就是利用地图。试想一下，你在得到第一帧点云时你的lidar就扫到了数万个点，此时Lidar的位置我们把它作为（0,0,0），在不考虑测量噪声的情况下这数万个点都是相对精确的，我们把这数万个点所构成的环境作为此时的地图。而后Lidar运动了一小段，我们通过Lidar里程计的方法估算了它的相对运动，于是就可以将此时的Lidar位姿及此时的点按照我们估计的相对运动情况，转换到上一时刻（建立地图时的坐标系）的坐标系下。只不过由于里程计估计误差，地图可能拼歪了。既然这样，我是如果把地图完整的拼上去，是不是就可以优化此时Lidar的位姿了呢？这就是建图节点起到的关键作用。只不过拿当前扫描的点云和地图中所有点云去配准，这个计算消耗太大，因此为了保证实时性，作者在这里采用了一种低频处理方法，即调用建图节点的频率仅为调用里程计节点频率的十分之一。

transformMaintenance

实际它就是融合后的Lidar轨迹，有优化结果了就拿这一时刻的优化结果作为轨迹，没有优化结果只有里程计结果了，就直接拿里程计结果作为这一时刻的轨迹。