镭神16线激光雷达利用ROS系统进行平面分割测试

试验目的

笔者手里有一个镭神16线激光雷达，用此激光雷达做一个测试试验，测试同等级的激光雷达的点云数据相比谁的更稳定，数据量谁的更多一些，后来考虑：不需要很麻烦的测试步骤，仅仅需要两步就可以了，第一步：把雷达驱动安装，在RVIZ中进行可视化；第二步：利用平面分割算法将地面影响消除，然后再将过滤地面的点云数据传输到欧式聚类算法中进行聚类。这样可以有效的观察到激光雷达点云数量的多少。这一切操作都在ROS系统中（不介绍ROS的安装了）。

雷达驱动安装

找到驱动的方法有很多，可以在Github网站上寻找代码，也可以在官方网站中找到驱动包。这里需要的步骤有 ：

1. 硬件安装；
2. 网线连接笔记本和雷达，网线地址一般是192.168.1.102
3. 配置yaml文件，需要设置相对应的型号的雷达；
4. 编译ROS工作空间，catkin_make ,然后source一下
5. 最后roslaunch开启驱动

// 开启驱动命令
roslaunch rslidar_sdk start.launch
//这里的rslidar_sdk是launch文件所在的文件夹，start.launch是开启文件

平面分割算法

对于雷达点云数据，将地面分割出去，便于后续的数据处理。
1. ROS初始化；

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pcl_test");//初始化ROS节点

  ros::NodeHandle nh;	//该句柄是roscpp的“万能接口类”，常用于创建topic server等，可以指定命名空间（全局、普通和私有）

  PclTestCore core(nh);	//整个程序定义的类，赋予core这个对象
  // core.Spin();
  return 0;
}

2. ROS设定发布者和订阅者，使用该算法，仅仅需要改一下订阅者，改成接收自己的激光雷达topic，

PclTestCore::PclTestCore(ros::NodeHandle &nh)
{
 sub_point_cloud_ = nh.subscribe("/rslidar_points", 10, &PclTestCore::point_cb, this);	//订阅来自激光雷达的点云数据

 pub_ground_ = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points_ground", 10);	//发布只含地面的点云数据
 pub_no_ground_ = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points_no_ground", 10);	//发布不含地面的点云数据

 ros::spin();	//循环等待，一直订阅发布的作用
}

3. 点云裁剪，要分割地面和非地面，那么过高的区域不需要考虑，对点云高度进行裁剪，lidar的高度是1.78米，因此裁掉1.28米以上的部分。

void PclTestCore::clip_above(double clip_height, const pcl::PointCloud<pcl::PointXYZI>::Ptr in,
                          const pcl::PointCloud<pcl::PointXYZI>::Ptr out)
{
 pcl::ExtractIndices<pcl::PointXYZI> cliper;

 cliper.setInputCloud(in);
 pcl::PointIndices indices;
#pragma omp for //该语法OpenMP的并行化语法，即希望通过OpenMP并行化执行这条语句的for循环，从而起到加速的效果
 for (size_t i = 0; i < in->points.size(); i++)
 {
     if (in->points[i].z > clip_height)
     {
         indices.indices.push_back(i);
     }
 }
 cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
 cliper.setNegative(true); //ture to remove the indices
 cliper.filter(*out);
}

4. 消除车身自身对雷达反射的影响，我们对近距离的点云进行过滤

void PclTestCore::remove_close_pt(double min_distance, const pcl::PointCloud<pcl::PointXYZI>::Ptr in,
                               const pcl::PointCloud<pcl::PointXYZI>::Ptr out)
{
 pcl::ExtractIndices<pcl::PointXYZI> cliper;

 cliper.setInputCloud(in);
 pcl::PointIndices indices;
#pragma omp for
 for (size_t i = 0; i < in->points.size(); i++)
 {
     double distance = sqrt(in->points[i].x * in->points[i].x + in->points[i].y * in->points[i].y);

     if (distance < min_distance)
     {
         indices.indices.push_back(i);
     }
 }
 cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
 cliper.setNegative(true); //ture to remove the indices
 cliper.filter(*out);
}

5. 这两个坡度阈值的单位为度（degree）我们通过这两个坡度阈值以及当前点的半径，求得高度阈值，通过判断当前点的高度是否在加减高度阈值范围内，来判断是否地面。

该算法的核心：如上图把激光雷达纵截面画了出来，这是一个数学问题（工程类问题最终就是数学问题！）点云投射到地面，通过点与激光雷达的夹角和点和激光雷达的直线距离计算出来相邻的点的高度差，如果在阈值范围内，那么判断为地面。

void PclTestCore::classify_pc(std::vector<PointCloudXYZIRTColor> &in_radial_ordered_clouds,
                             pcl::PointIndices &out_ground_indices,
                             pcl::PointIndices &out_no_ground_indices)
{
   out_ground_indices.indices.clear();
   out_no_ground_indices.indices.clear();
#pragma omp for
   for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++) //sweep through each radial division 遍历每一根射线
   {
       float prev_radius = 0.f;
       float prev_height = -SENSOR_HEIGHT;
       bool prev_ground = false;
       bool current_ground = false;
       for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++) //loop through each point in the radial div
       {
           float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius;
           float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance;
           float current_height = in_radial_ordered_clouds[i][j].point.z;
           float general_height_threshold = tan(DEG2RAD(general_max_slope_)) * in_radial_ordered_clouds[i][j].radius;

           //for points which are very close causing the height threshold to be tiny, set a minimum value
           if (points_distance > concentric_divider_distance_ && height_threshold < min_height_threshold_)
           {
               height_threshold = min_height_threshold_;
           }

           //check current point height against the LOCAL threshold (previous point)
           if (current_height <= (prev_height + height_threshold) && current_height >= (prev_height - height_threshold))
           {
               //Check again using general geometry (radius from origin) if previous points wasn't ground
               if (!prev_ground)
               {
                   if (current_height <= (-SENSOR_HEIGHT + general_height_threshold) && current_height >= (-SENSOR_HEIGHT - general_height_threshold))
                   {
                       current_ground = true;
                   }
                   else
                   {
                       current_ground = false;
                   }
               }
               else
               {
                   current_ground = true;
               }
           }
           else
           {
               //check if previous point is too far from previous one, if so classify again
               if (points_distance > reclass_distance_threshold_ &&
                   (current_height <= (-SENSOR_HEIGHT + height_threshold) && current_height >= (-SENSOR_HEIGHT - height_threshold)))
               {
                   current_ground = true;
               }
               else
               {
                   current_ground = false;
               }
           }

           if (current_ground)
           {
               out_ground_indices.indices.push_back(in_radial_ordered_clouds[i][j].original_index);
               prev_ground = true;
           }
           else
           {
               out_no_ground_indices.indices.push_back(in_radial_ordered_clouds[i][j].original_index);
               prev_ground = false;
           }

           prev_radius = in_radial_ordered_clouds[i][j].radius;
           prev_height = in_radial_ordered_clouds[i][j].point.z;
       }
   }
}

附录全部代码：https://download.csdn.net/download/xiaobaiwsc/86398602