一、解决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 forfor (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 indicesextractor.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 forfor (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 indicesextractor.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 forfor (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 forfor (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 groundif (!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值）是否在地面加减高度阈值范围内来判断当前点是为地面。效果如下图所示（白色的是地面点，红色的是障碍物）：
在这里插入图片描述