Overview

这个模块是根据占据栅格地图occupancy_grid_map来对点云进行滤波的，根据占用网格图的实现方式，它也可以被称为时间序列中的离群值滤波器，因为占用网格图表达的是时间序列中的占用概率。 关于这里涉及到的方法，在官方文档中有简单的介绍occupancy_grid_map_outlier_filter，甚至附上了两个YouTube上的视频，简单来说就是把点云根据occupancy grid map分成了两部分 （low occupancy probability和high occupancy probability） ，这里还有一点就是不是所有的low occupancy probability都是outliers，还会根据距离信息多一步判断

（Class）RadiusSearch2dfilter

看名字这个是利用半径进行2D滤波的，应该涉及到了前面提到的针对low occupancy probability的情况

（Class Constructor）RadiusSearch2dfilter::RadiusSearch2dfilter

这里面涉及到了几个参数的加载

Name	Type	Description
radius_search_2d_filter/search_radius	float	Radius when calculating the density
radius_search_2d_filter/min_points_and_distance_ratio	float	Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink.
radius_search_2d_filter/min_points	int	Minimum number of point clouds per radius
radius_search_2d_filter/max_points	int	Maximum number of point clouds per radius

然后还实例化了一个很常用的PCL的KD树对象，应该在滤波的时候会用到

（mFunc）RadiusSearch2dfilter::filter

这个函数分成了两种参数输入方式，第一种的话输入包括了输入的点云，一个位置；然后输出的就是满足条件的点和离散点。具体的话就是找到给定位置Pose的距离满足要求的点

kd_tree_->setInputCloud(xy_cloud);
for (size_t i = 0; i < xy_cloud->points.size(); ++i) {
  const float distance =
    std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y);
  const int min_points_threshold = std::min(
    std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
    max_points_);
  const int points_num =
    kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists, min_points_threshold);
  if (min_points_threshold <= points_num) {
    output.points.push_back(xyz_cloud.points.at(i));
  } else {
    outlier.points.push_back(xyz_cloud.points.at(i));
  }
}

第二种和第一种的方法是一样的，不过这里是直接输入了high_conf_input和low_conf_input两个点云，然后相当于给一起处理了

xy_cloud->points.resize(low_conf_xyz_cloud.points.size() + high_conf_xyz_cloud.points.size());
for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) {
  xy_cloud->points[i].x = low_conf_xyz_cloud.points[i].x;
  xy_cloud->points[i].y = low_conf_xyz_cloud.points[i].y;
}
for (size_t i = low_conf_xyz_cloud.points.size(); i < xy_cloud->points.size(); ++i) {
  xy_cloud->points[i].x = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].x;
  xy_cloud->points[i].y = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].y;
}

（Class）OccupancyGridMapOutlierFilterComponent

本模块对应的主要节点函数

（Class Constructor）OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent

构造函数里加载了另一部分参数

Name	Type	Description
map_frame	string	map frame id
base_link_frame	string	base link frame id
cost_threshold	int	Cost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle.
enable_debugger	bool	Whether to output the point cloud for debugging.
use_radius_search_2d_filter	bool	Whether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map.

然后这里因为输入的点云必须和栅格地图匹配上执行后边的步骤才有意义，所以这里也是使用到了之前用到过的那种同步技巧

pointcloud_sub_.subscribe(this, "~/input/pointcloud", rmw_qos_profile_sensor_data);
occupancy_grid_map_sub_.subscribe(
  this, "~/input/occupancy_grid_map", rclcpp::QoS{1}.get_rmw_qos_profile());
sync_ptr_ = std::make_shared<Sync>(SyncPolicy(5), occupancy_grid_map_sub_, pointcloud_sub_
sync_ptr_->registerCallback(std::bind(
  &OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2, this,
  std::placeholders::_1, std::placeholders::_2));

（mFunc）OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2

这里滤波的过程是通过调用filterByOccupancyGridMap函数实现的

filterByOccupancyGridMap(*input_ogm, ogm_frame_pc, high_confidence_pc, low_confidence_pc);

然后根据参数配置，来决定要不要执行距离的滤波

if (radius_search_2d_filter_ptr_) {
  auto pc_frame_pose_stamped = getPoseStamped(
    *tf2_, input_ogm->header.frame_id, input_pc->header.frame_id, input_ogm->header.stamp);
  radius_search_2d_filter_ptr_->filter(
    high_confidence_pc, low_confidence_pc, pc_frame_pose_stamped.pose, filtered_low_confidence_pc,
    outlier_pc);
} else {
  outlier_pc = low_confidence_pc;
}

然后就是对要发布的点云坐标转换再发布就好了

（mFunc）OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap

这个里面其实也很简单，就是根据阈值和对应栅格的值来判断要存进哪个里面，也没什么特殊的

for (sensor_msgs::PointCloud2ConstIterator<float> x(pointcloud, "x"), y(pointcloud, "y"),
     z(pointcloud, "z");
     x != x.end(); ++x, ++y, ++z) {
  const auto cost = getCost(occupancy_grid_map, *x, *y);
  if (cost) {
    if (cost_threshold_ < *cost) {
      high_confidence.push_back(pcl::PointXYZ(*x, *y, *z));
    } else {
      low_confidence.push_back(pcl::PointXYZ(*x, *y, *z));
    }
  } else {
    high_confidence.push_back(pcl::PointXYZ(*x, *y, *z));
  }
}

（Func）getCost

根据输入的网格和对应的xy索引，返回cost这么一个值的过程，其实主要的过程是计算xy对应的网格index，具体的cost值就是nav_msgs::msg::OccupancyGrid类型中对应的data

if (map_min_x < x && x < map_max_x && map_min_y < y && y < map_max_y) {
  unsigned int map_cell_x{};
  unsigned int map_cell_y{};
  map_cell_x = std::floor((x - map_position.x) / map_resolution);
  map_cell_y = std::floor((y - map_position.y) / map_resolution);
  size_t index = map_cell_y * map.info.width + map_cell_x;
  return map.data.at(index);
}