本人最近在研究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
void handle_pc_msg(const typename pcl::PointCloud::Ptr &pc_in,const typename pcl::PointCloud::Ptr &pc_out) {// to new pointcloudfor (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
void add_ring(const typename pcl::PointCloud::Ptr &pc_in,const typename pcl::PointCloud::Ptr &pc_out) {// to new pointcloudint 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
void add_time(const typename pcl::PointCloud::Ptr &pc_in,const typename pcl::PointCloud::Ptr &pc_out) {// to new pointcloudint 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::Ptr pc_in(new pcl::PointCloud());pcl::fromROSMsg(pc_msg, *pc_in);if (output_type == "XYZIRT") {pcl::PointCloud::Ptr pc_out(new pcl::PointCloud());handle_pc_msg(pc_in, pc_out);add_ring(pc_in, pc_out);add_time(pc_in, pc_out);publish_points(pc_out, pc_msg);} else if (output_type == "XYZIR") {pcl::PointCloud::Ptr pc_out(new pcl::PointCloud());handle_pc_msg(pc_in, pc_out);add_ring(pc_in, pc_out);publish_points(pc_out, pc_msg);} else if (output_type == "XYZI") {pcl::PointCloud::Ptr pc_out(new pcl::PointCloud());handle_pc_msg(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 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