地面点云提取：Autoware预处理ray_ground_filter节点解析 + 解决ray_ground_filter无输出的问题

---

一、解决Autoware的ray_ground_filter节点无点云输出的问题

在autoware/core_perception/points_preprocessor/nodes/ray_ground_filter的包ray_ground_filter存在无点云输出的问题，下面解决它并具体分析一下ray_ground_filter的代码，参考：https://adamshan.blog.csdn.net/article/details/82901295#t6
（1）首先通过终端报错发现是tf的问题：

[ERROR] [1689559482.301474720]: Failed transform from base_link to velodyne
[ WARN] [1689559483.304340852]: Lookup would require extrapolation into the past.  Requested time 1671007347.691825151 but the earliest data is at time 1689559474.270626140, when looking up transform from frame [velodyne] to frame [base_link]

报错的源码在这句：

transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,in_cloud_ptr->header.stamp, ros::Duration(1.0));

它用的是in_cloud_ptr->header.stamp，就是说它寻找的tf关系是在我们接收输入点云的时刻比如时间为100的时刻的tf关系，但有时当我们寻找时间为100时的tf关系时已经找不到了，系统存储的tf关系已经被更新了，存储的最早的可能就只有103时刻的了，这时候就会报错或者抛出异常，对于transformPointCloud而言就是返回false，虽然合乎情理，但是我就想要他的tf变换，不是100时刻的也行，105时刻的也行，这时候我们就要用ros::Time(0）获取当前时刻tf，这样不会再找不到吧，ros::Duration(5.0)也可以设大点，让它能找到更多的时间范围。

transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,ros::Time(0), ros::Duration(5.0));

成功解决：

另外注意调整其以下几个参数（下一节会具体描述）：
clipping_height：去除高于底盘1.2米的点，原算法是去除高于雷达0.2米的点，但是Autoware利用TF将点云转换到base_link坐标系（为了省去代码中的雷达高度参数），因此这里是1.2（加上雷达高度1m）；
min_point_distance：去除雷达2米范围内的点云；
radial_divider_angle：将雷达点云坐标转换到柱坐标，并对360度进行微分，每一份的角度为0.18度
concentric_divider_distanc：上面的角度微分一份近似的可以看作一条射线，在射线上根据水平距离再进行微分
general_max_slope，local_max_slope：全局和局部的最大坡度（角度）
min_point_distance：一条射线上两个相邻点的最小高度差
reclass_distance_threshold：重新分类时两个点的最小距离阈值

二、ray_ground_filter节点代码分析

过滤地面是激光雷达感知中一步基础的预处理操作，因为我们环境感知通常只对路面上的障碍物感兴趣，且地面的点对于障碍物聚类容易产生影响，所以在做Lidar Obstacle Detection（障碍物探测）之前通常将地面点和非地面点进行分离。

2.1.监听bask_link和velodyne之间的TF

作用是利用TF将点云转换到bask_link坐标系，这样就不需要原算法中的车身高度：

bool RayGroundFilter::TransformPointCloud(const std::string &in_target_frame,
                                          const sensor_msgs::PointCloud2::ConstPtr &in_cloud_ptr,
                                          const sensor_msgs::PointCloud2::Ptr &out_cloud_ptr)
{
  if (in_target_frame == in_cloud_ptr->header.frame_id)
  {
    *out_cloud_ptr = *in_cloud_ptr;
    return true;
  }

  geometry_msgs::TransformStamped transform_stamped;
  try
  {
    // 监听开始了，但是坐标之间的变换关系还未处理完毕，在此期间访问tf便会出现不存在的报错
    // 将函数的超时参数设定为5s，即最多有5s的时间让listener处理坐标关系
    transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
                                                   ros::Time(0), ros::Duration(5.0));
    // transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
    //                                                in_cloud_ptr->header.stamp, ros::Duration(5.0));
  }
  catch (tf2::TransformException &ex)
  {
    ROS_WARN("%s", ex.what());
    return false;
  }
  // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped);
  Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
  pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr);
  out_cloud_ptr->header.frame_id = in_target_frame;
  return true;
}

2.2 裁切过高点云

在分割地面之前，可以滤除掉过高的非地面点，以减少点云的数量，提升整体的处理速率，而且过高的障碍物对于小车来说并不算障碍物。具体的裁切高度由clipping_height指定。在这里面我加了一句代码去掉NAN值，如果有NAN值可能造成错误：

void RayGroundFilter::ClipCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, const double in_clip_height,
                                pcl::PointCloud<pcl::PointXYZI>::Ptr out_clipped_cloud_ptr)
{
  pcl::ExtractIndices<pcl::PointXYZI> extractor;
  extractor.setInputCloud(in_cloud_ptr);
  pcl::PointIndices indices;
// #pragma omp for语法OpenMP的并行化语法，即希望通过OpenMP并行化执行这条语句后的for循环，从而起到加速的效果。
#pragma omp for
  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
  {
    // 添加判断,去除空点
    if (in_cloud_ptr->points[i].z > in_clip_height || isnan(in_cloud_ptr->points[i].x) || isnan(in_cloud_ptr->points[i].y) || isnan(in_cloud_ptr->points[i].z))
    {
      indices.indices.push_back(i);
    }
  }
  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
  extractor.setNegative(true); // true removes the indices, false leaves only the indices
  extractor.filter(*out_clipped_cloud_ptr);
}

2.3 消除雷达近身反射点的影响

具体指标由min_point_distance参数指定。去除过近处区域雷达稀疏点云，避免车身点云的影响：

void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, double in_min_distance,
                                       pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr)
{
  pcl::ExtractIndices<pcl::PointXYZI> extractor;
  extractor.setInputCloud(in_cloud_ptr);
  pcl::PointIndices indices;

#pragma omp for
  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
  {
    if (sqrt(in_cloud_ptr->points[i].x * in_cloud_ptr->points[i].x +
             in_cloud_ptr->points[i].y * in_cloud_ptr->points[i].y) < in_min_distance)
    {
      indices.indices.push_back(i);
    }
  }
  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
  extractor.setNegative(true); // true removes the indices, false leaves only the indices
  extractor.filter(*out_filtered_cloud_ptr);
}

2.4 角度和距离微分（转换到柱坐标）

Ray Ground Filter算法的核心是以射线（Ray）的形式来组织点云。我们现在将点云的 (x, y, z)三维空间降到（x,y）平面来看，计算每一个点到车辆x正方向的平面夹角 θ, 我们对360度进行微分，分成若干等份，每一份的角度为0.18度，这个微分的等份近似的可以看作一条射线，每条射线上的点又根据水平距离（点到lidar的水平距离，半径）0.001再进行微分。

为了方便地对点进行半径和夹角的表示，作者自定义了数据结构PointXYZIRTColor来代替pcl::PointCloudXYZI：

struct PointXYZIRTColor
{
    pcl::PointXYZI point;
 
    float radius; //xy平面与雷达的欧氏距离
    float theta;  //xy的角度微分
 
    size_t radial_div;     //线圈的索引
    size_t concentric_div; //扫描线的索引
 
    size_t original_index; //与源雷达点云对应的索引
};

用radial_div和concentric_div分别描述角度微分和距离微分。对点云进行水平角度微分之后，可得到：360/0.18 = 2000条射线，将这些射线中的点按照距离的远近进行排序：

void RayGroundFilter::ConvertXYZIToRTZColor(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
    const std::shared_ptr<PointCloudXYZIRTColor> &out_organized_points,
    const std::shared_ptr<std::vector<pcl::PointIndices>> &out_radial_divided_indices,
    const std::shared_ptr<std::vector<PointCloudXYZIRTColor>> &out_radial_ordered_clouds)
{
  out_organized_points->resize(in_cloud->points.size());
  out_radial_divided_indices->clear();
  out_radial_divided_indices->resize(radial_dividers_num_);
  out_radial_ordered_clouds->resize(radial_dividers_num_);

  // 遍历当前帧的所有点
  for (size_t i = 0; i < in_cloud->points.size(); i++)
  {
    PointXYZIRTColor new_point;
    // 半径和方位角
    auto radius = static_cast<float>(
        sqrt(in_cloud->points[i].x * in_cloud->points[i].x + in_cloud->points[i].y * in_cloud->points[i].y));
    auto theta = static_cast<float>(atan2(in_cloud->points[i].y, in_cloud->points[i].x) * 180 / M_PI);
    if (theta < 0)
    {
      theta += 360;
    }
    if (theta >= 360)
    {
      theta -= 360;
    }
    // 角度的微分
    auto radial_div = (size_t)floor(theta / radial_divider_angle_);
    // 半径的微分
    auto concentric_div = (size_t)floor(fabs(radius / concentric_divider_distance_));

    new_point.point = in_cloud->points[i];
    new_point.radius = radius;
    new_point.theta = theta;
    new_point.radial_div = radial_div;
    new_point.concentric_div = concentric_div;
    new_point.red = (size_t)colors_[new_point.radial_div % color_num_].val[0];
    new_point.green = (size_t)colors_[new_point.radial_div % color_num_].val[1];
    new_point.blue = (size_t)colors_[new_point.radial_div % color_num_].val[2];
    new_point.original_index = i;

    out_organized_points->at(i) = new_point;

    // radial divisions更加角度的微分组织射线
    out_radial_divided_indices->at(radial_div).indices.push_back(i);
    // 每个角度/射线上包含的所有点云
    out_radial_ordered_clouds->at(radial_div).push_back(new_point);
  } // end for

// order radial points on each division
#pragma omp for
  for (size_t i = 0; i < radial_dividers_num_; i++)
  {
    // 将同一根射线上的点按照半径（距离）排序
    std::sort(out_radial_ordered_clouds->at(i).begin(), out_radial_ordered_clouds->at(i).end(),
              [](const PointXYZIRTColor &a, const PointXYZIRTColor &b)
              { return a.radius < b.radius; }); // NOLINT
  }
}

2.5 地面判断(核心部分)

通过判断射线中前后两点的坡度是否大于我们事先设定的坡度阈值，从而判断点是否为地面点:

void RayGroundFilter::ClassifyPointCloud(const std::vector<PointCloudXYZIRTColor> &in_radial_ordered_clouds,
                                         const pcl::PointIndices::Ptr &out_ground_indices,
                                         const pcl::PointIndices::Ptr &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 = 0.f;
    // 上一个以及当前点是否为地面点
    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
    {
      // 与上一个点水平距离（第一个点是与雷达/base_link的距离）
      float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius;
      // 通过这两个坡度阈值以及当前点的半径（到lidar的水平距离）求得高度阈值，
      // 通过判断当前点的高度（即点的z值）是否在地面加减高度阈值范围内来判断当前点是为地面。
      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 <= general_height_threshold && current_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 <= height_threshold && current_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;
    }
  }
}

这里有两个重要参数，一个是local_max_slope_，是我们设定的同条射线上邻近两点的坡度阈值，一个是general_max_slope_，表示整个地面的坡度阈值，这两个坡度阈值的单位为度（degree），我们通过这两个坡度阈值以及当前点的半径（到lidar的水平距离）求得高度阈值，通过判断当前点的高度（即点的z值）是否在地面加减高度阈值范围内来判断当前点是为地面。效果如下图所示（白色的是地面点，红色的是障碍物）：