calculate visibility score in ring outlier filter

Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · May 14, 2024 · 8ef4df8 · 8ef4df8
1 parent 113ad51
commit 8ef4df8
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 14 deletions.
diff --git a/control/cgmres b/control/cgmres
diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
@@ -10,6 +10,8 @@ A method of operating scan in chronological order and removing noise based on th
 
 ![ring_outlier_filter](./image/outlier_filter-ring.drawio.svg)
 
+Another feature of this node is that it calculates visibility based on outlier pointcloud and publish score as a topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle.
+
 ## Inputs / Outputs
 
 This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md).
@@ -22,14 +24,20 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 
 ### Core Parameters
 
-| Name                         | Type    | Default Value | Description                                                                                              |
-| ---------------------------- | ------- | ------------- | -------------------------------------------------------------------------------------------------------- |
-| `distance_ratio`             | double  | 1.03          |                                                                                                          |
-| `object_length_threshold`    | double  | 0.1           |                                                                                                          |
-| `num_points_threshold`       | int     | 4             |                                                                                                          |
-| `max_rings_num`              | uint_16 | 128           |                                                                                                          |
-| `max_points_num_per_ring`    | size_t  | 4000          | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring`                      |
-| `publish_outlier_pointcloud` | bool    | false         | Flag to publish outlier pointcloud. Due to performance concerns, please set to false during experiments. |
+| Name                         | Type    | Default Value | Description                                                                                                                   |
+| ---------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------------------------------------------------- |
+| `distance_ratio`             | double  | 1.03          |                                                                                                                               |
+| `object_length_threshold`    | double  | 0.1           |                                                                                                                               |
+| `num_points_threshold`       | int     | 4             |                                                                                                                               |
+| `max_rings_num`              | uint_16 | 128           |                                                                                                                               |
+| `max_points_num_per_ring`    | size_t  | 4000          | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring`                                           |
+| `publish_outlier_pointcloud` | bool    | false         | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. |
+| `min_azimuth_deg`            | float   | 0.0           | The left limit of azimuth for visibility score calculation                                                                    |
+| `max_azimuth_deg`            | float   | 360.0         | The right limit of azimuth for visibility score calculation                                                                   |
+| `max_distance`               | float   | 12.0          | The limit distance for for visibility score calculation                                                                       |
+| `vertical_bins`              | int     | 128           | The number of vertical bin for visibility histogram                                                                           |
+| `horizontal_bins`            | int     | 36            | The number of horizontal bin for visibility histogram                                                                         |
+| `noise_threshold`            | int     | 2             | The parameter for determining whether it is noise                                                                             |
 
 ## Assumptions / Known limits
 

diff --git a/...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
@@ -30,7 +30,6 @@
 
 #include <memory>
 #include <string>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -69,7 +68,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
   int noise_threshold_;
   int vertical_bins_;
   int horizontal_bins_;
-  float max_azimuth_diff_;
 
   float min_azimuth_deg_;
   float max_azimuth_deg_;

diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
@@ -35,8 +35,6 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
     debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
     outlier_pointcloud_publisher_ =
       this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
-    image_pub_ =
-      image_transport::create_publisher(this, "ring_outlier_filter/debug/frequency_image");
     visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
       "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS());
     stop_watch_ptr_->tic("cyclic_time");
@@ -325,8 +323,20 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
   if (get_param(p, "vertical_bins", vertical_bins_)) {
     RCLCPP_DEBUG(get_logger(), "Setting new vertical_bins to: %d.", vertical_bins_);
   }
-  if (get_param(p, "max_azimuth_diff", max_azimuth_diff_)) {
-    RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_diff to: %f.", max_azimuth_diff_);
+  if (get_param(p, "horizontal_bins", horizontal_bins_)) {
+    RCLCPP_DEBUG(get_logger(), "Setting new horizontal_bins to: %d.", horizontal_bins_);
+  }
+  if (get_param(p, "noise_threshold", noise_threshold_)) {
+    RCLCPP_DEBUG(get_logger(), "Setting new noise_threshold to: %d.", noise_threshold_);
+  }
+  if (get_param(p, "max_azimuth_deg", max_azimuth_deg_)) {
+    RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_deg to: %f.", max_azimuth_deg_);
+  }
+  if (get_param(p, "min_azimuth_deg", min_azimuth_deg_)) {
+    RCLCPP_DEBUG(get_logger(), "Setting new min_azimuth_deg to: %f.", min_azimuth_deg_);
+  }
+  if (get_param(p, "max_distance", max_distance_)) {
+    RCLCPP_DEBUG(get_logger(), "Setting new max_distance to: %f.", max_distance_);
   }
 
   rcl_interfaces::msg::SetParametersResult result;