LIO-SAM实现地面分割思路

本人最近在研究lio-sam，不得不感叹作者Tixiao Shan的实力，以及在SLAM方面的建树。从此抛弃了Cartographer投入到lio-sam的怀抱中。

前言

硬件：速腾16线激光雷达+九轴IMU+华测GPS+四轮差速底盘
建图：LIO-SAM
定位：LIO-SAM_based_relocalization
导航：Move-Base + TEB

效果

整体效果很绝。使用过LIO-SAM的都知道，LIO-SAM三维地图创建的时候地面上的点云也会保存在地图中，因此为了好看，也为了提高滤波源图的效果，搞了下地面分割，话不多说，先上对比图。

地面分割前：

地面分割后：

效果显著，我的思路来源于AdamShan大佬的地面分割源码和velodyne2rslidar源码。

velodyne2rslidar

使用lio-sam时候基本都会接触到的这个源码，用于解决由于穷买不起Velodyne的问题，使用速腾雷达转换为Velodyne格式一样可以输出使用。

先说velodyne2rslidar代码，在这里面将学习如何实现自定义点云数据的传递，例如Velodyne的类型为X、Y、Z、I、RING、TIME，而RSLIDAR的类型为X、Y、Z、I、RING、TIMESTAMP，如何通过代码实现不同类型的数据传递与接收。

简单阅读下面这段代码，首先创建一个模板类，接收模板类型的点云，
针对XYZI的类型实现：new_point.x = pc_in->points[point_id].x;点的传递；
针对ring的实现：pc_out->points[valid_point_id++].ring = pc_in->points[point_id].ring;
针对time的实现：pc_out->points[valid_point_id++].time = float(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp);的传递。

template<typename T_in_p, typename T_out_p>
void handle_pc_msg(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
                   const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {

    // to new pointcloud
    for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
        if (has_nan(pc_in->points[point_id]))
            continue;
        T_out_p new_point;
//        std::copy(pc->points[point_id].data, pc->points[point_id].data + 4, new_point.data);
        new_point.x = pc_in->points[point_id].x;
        new_point.y = pc_in->points[point_id].y;
        new_point.z = pc_in->points[point_id].z;
        new_point.intensity = pc_in->points[point_id].intensity;
//        new_point.ring = pc->points[point_id].ring;
//        // 计算相对于第一个点的相对时间
//        new_point.time = float(pc->points[point_id].timestamp - pc->points[0].timestamp);
        pc_out->points.push_back(new_point);
    }
}

template<typename T_in_p, typename T_out_p>
void add_ring(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
              const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
    // to new pointcloud
    int valid_point_id = 0;
    for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
        if (has_nan(pc_in->points[point_id]))
            continue;
        // 跳过nan点
        pc_out->points[valid_point_id++].ring = pc_in->points[point_id].ring;
    }
}

template<typename T_in_p, typename T_out_p>
void add_time(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
              const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
    // to new pointcloud
    int valid_point_id = 0;
    for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
        if (has_nan(pc_in->points[point_id]))
            continue;
        // 跳过nan点
        pc_out->points[valid_point_id++].time = float(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp);
    }
}

void rsHandler_XYZIRT(const sensor_msgs::PointCloud2 &pc_msg) {
    pcl::PointCloud<RsPointXYZIRT>::Ptr pc_in(new pcl::PointCloud<RsPointXYZIRT>());
    pcl::fromROSMsg(pc_msg, *pc_in);

    if (output_type == "XYZIRT") {
        pcl::PointCloud<VelodynePointXYZIRT>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIRT>());
        handle_pc_msg<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
        add_ring<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
        add_time<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
        publish_points(pc_out, pc_msg);
    } else if (output_type == "XYZIR") {
        pcl::PointCloud<VelodynePointXYZIR>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIR>());
        handle_pc_msg<RsPointXYZIRT, VelodynePointXYZIR>(pc_in, pc_out);
        add_ring<RsPointXYZIRT, VelodynePointXYZIR>(pc_in, pc_out);
        publish_points(pc_out, pc_msg);
    } else if (output_type == "XYZI") {
        pcl::PointCloud<pcl::PointXYZI>::Ptr pc_out(new pcl::PointCloud<pcl::PointXYZI>());
        handle_pc_msg<RsPointXYZIRT, pcl::PointXYZI>(pc_in, pc_out);
        publish_points(pc_out, pc_msg);
    }
}

地面分割

思路：lio-sam的使用时，使用rslidar2velodyne，先将格式转换，这样直接输入到lio-sam接收就可以生成分割前的地图。所以我们需要作的是在格式转换之后，对输入到lio-sam之前的点云进行处理，我们在namespace pcl里创建一个自定义点云类接收PointCloud2传递来的的velodyne格式pcl::VelodynePointXYZIRT，将pcl::VelodynePointXYZIRT数据传递给自建结构体PointXYZIRT_RTColor，用于点云分割函数计算，计算完成后再以PointCloud2发布ROS话题即可。

1.创建自定义点云格式

namespace pcl
{
    struct VelodynePointXYZIRT
    {
        PCL_ADD_POINT4D;

        float intensity;
        uint16_t ring;
        float time;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    } EIGEN_ALIGN16;
}

2.创建自定义结构体

    struct PointXYZIRT_RTColor
    {
        pcl::PointXYZI point;

        uint16_t ring;
        double timestamp;

        float radius; // xy平面与雷达的欧氏距离
        float theta;  // xy的角度微分

        size_t radial_div;     // 线圈的索引
        size_t concentric_div; // 扫描线的索引

        size_t original_index; // 与源雷达点云对应的索引
    };
    typedef std::vector<PointXYZIRT_RTColor> PointCloudXYZIRT_RTColor;

其余部分仿照AdamShan源码修改即可。

3.注意

（1）concentric_divider_distance_参数为水平方向激光发射器间隔，rslidar16间隔为0.1度（5Hz）或0.4度（20Hz）；
（2）SENSOR_HEIGHT参数为激光雷达高度，这个高度设置可以相比真实高度低1-5cm，效果也可以接受，地面偶尔会产生点云，但是该参数设置高于了真实高度，那么意味着真实的地面被过多的裁剪，那么在遇到颠簸或坡度较大的环境时，可能造成lio-sam输入的轨迹飘移，感觉lio-sam的回环有问题，点云不一定会完全补偿修复重合，所以我个人建议这个参数低于真实高度；
（3）自己在写自定义结构体的时候是非常关键的，因为如果读者直接用大佬的代码去作为lio-sam的输入，会报错ring time缺失，因此整个代码的关键就在于结构体中接收velodyne传递过来的XYZIRT；

参考

LIO-SAM：源码
地面分割：无人驾驶汽车系统入门（二十四）：教程
网友笔记：网友学习笔记1、网友笔记2