hdl-graph-slam思想解读（1）-整体框架

hdl-graph-slam是一个较为经典的3d 激光slam开源算法，其代码架构简洁，且调用通用Eigen和g2o库进行核心运算，非常适合对slam整个架构的学习。
hdl-graph-slam不仅融合了imu、gps、平面、激光等多种输入，同时也带闭环检测等后端处理，即完整的一个slam架构。

功能介绍

hdl-graph-slam是一个非常优秀的SLAM框架，其功能较为全面（但不一定最优），包括平面检测及提取、点云预处理、激光里程计、闭环检测、gps和imu数据融合、后端优化等。

点云预处理

其主要目的是降采样和剔除异常噪点，可提升后续处理的速度和准确性；包含以下三个步骤：

    // 有效距离滤波
    pcl::PointCloud<PointT>::ConstPtr filtered = distance_filter(src_cloud);
    // 降采样
    filtered = downsample(filtered);
    // 噪点滤波
    filtered = outlier_removal(filtered);

1. 剔除较远和较近的点云，从实际考虑，激光雷达测得过远或者过近的点可靠性较低，可先进行剔除；
2. 由于一般选用的激光雷达获得的点云，在近处较为密集，而远处较为稀疏。降采样的目的可降低不同距离点云数量尽可能均衡，防止局部权重较大；不影响后端的性能下减少点云数目，从而提高运算速度。源码中调用的是开源pcl库中的VoxelGrid进行下采样；
3. 激光点云中会存在一定的孤立的点云簇，由于其个数和特征不明显且容易对真实结果进行干扰，需将其异常点进行剔除。同样采用开源pcl库中的StatisticalOutlierRemoval和RadiusOutlierRemoval两种方法进行噪点剔除（实际上不一定是真实的噪点，而是孤立的，离群的点统称）。

平面检测

在3d的激光点云中，很多情况下都需要提取地面信息，而hdl中其主要目的是假设地面为平面的话，则可以根据点云进行提取并计算其参数，用于后端的约束，如此可以保证闭环在高度方向上和姿态角的准确性。但存在一个前提，即使用此功能需保证本体的确在平面上进行SLAM，否则不仅不能提高准确性，反而会增加误差；
平面检测思想也较为简单，其核心基本上调用开源库进行处理，分成两步：

1. 点云预处理，先去除显然与平面特性不符合点云，包括高度与地面差距较大的点云、法线与地面法线夹角较大的点两类；
2. 平面拟合，剩下的点可认为是较大可能为地平面上的点，采用pcl库中的RANSAC 算法拟合平面方法，拟合平面参数。为确保平面正确，也需要判断平面与真实地面夹角是否过大，平面中有效点云个数是否足够多等。

前端激光里程计

采用点云帧间匹配的方法提供累加的里程计信息；
hdl前端处理采用的方法十分简单，采用当前帧与上一时刻的点云进行匹配，调用pcl现成库，可支持以下几种帧间匹配方法，包括ICP, GICP, NDT三大类。
激光里程计流程如下：

1. 接收点云数据和里程计数据；
2. 新接收的点云进行降采样；
3. 当前点云若为第一帧，则进行记录保存，放入匹配库中参考帧中；
4. 如果非第一帧，则与参考帧进行点云匹配，如采用NDT匹配方法进行匹配；
5. 增加异常判断，即如果两帧匹配点云转移矩阵相差过大，则抛弃；
6. 增加运动滤波功能，即间隔一定距离时作为关键帧，进行记录同时更新参考点云帧；
7. 实时累计位姿变换，发布激光里程计信息；

其他

实时获取gps和imu数据，主要用于后端约束优化处理。gps可提供全局静态位置，但是误差较大，而imu可提供高速的推算功能，但存在累计误差，通过后端优化，将其进行融合约束。

后端融合

后端调用g2o中图优化思想进行后端处理，关键帧作为顶点，而平面，gps，imu，闭环等信息作为边进行约束。