feat: adding switchable modes to LiDAR Visibility

sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md

@@ -14,11 +14,22 @@ Therefore, in order to use this node, the sensor driver must publish custom data
 
 Another feature of this node is that it publishes visibility as a diagnostic 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.
 
+In some complicated road scenes where normal objects also reflect the light in two stages, for instance plants, leaves, some plastic net etc, the visibility faces some drop in fine weather condition. To deal with that, optional settings of a region of interest (ROI) are added.
+
+1. `Fixed_xyz_ROI` mode: Visibility estimation based on the weak points in a fixed cuboid surrounding region of ego-vehicle, defined by x, y, z in base_link perspective.
+2. `Fixed_azimuth_ROI` mode: Visibility estimation based on the weak points in a fixed surrounding region of ego-vehicle, defined by azimuth and distance of LiDAR perspective.
+
+When select 2 fixed ROI modes, due to the range of weak points is shrink, the sensitivity of visibility is decrease so that a trade of between `weak_first_local_noise_threshold` and `visibility_threshold` is needed.
+
 ![outlier_filter-dual_return_overall](./image/outlier_filter-dual_return_overall.drawio.svg)
 
 The figure below describe how the node works.
 ![outlier_filter-dual_return_detail](./image/outlier_filter-dual_return_detail.drawio.svg)
 
+The below picture shows the ROI options.
+
+![outlier_filter-dual_return_ROI_setting_options](./image/outlier_filter-dual_return_ROI_setting_options.png)
+
 ## Inputs / Outputs
 
 This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md).
@@ -47,6 +58,16 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 | `general_distance_ratio`           | double | Threshold for ring_outlier_filter                                                                                         |
 | `weak_first_local_noise_threshold` | int    | The parameter for determining whether it is noise                                                                         |
 | `visibility_threshold`             | float  | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN |
+| `roi_mode`                         | string | The name of ROI mode for switching                                                                                        |
+| `min_azimuth_deg`                  | float  | The left limit of azimuth for `Fixed_azimuth_ROI` mode                                                                    |
+| `max_azimuth_deg`                  | float  | The right limit of azimuth for `Fixed_azimuth_ROI` mode                                                                   |
+| `max_distance`                     | float  | The limit distance for for `Fixed_azimuth_ROI` mode                                                                       |
+| `x_max`                            | float  | Maximum of x for `Fixed_xyz_ROI` mode                                                                                     |
+| `x_min`                            | float  | Minimum of x for `Fixed_xyz_ROI` mode                                                                                     |
+| `y_max`                            | float  | Maximum of y for `Fixed_xyz_ROI` mode                                                                                     |
+| `y_min`                            | float  | Minimum of y for `Fixed_xyz_ROI` mode                                                                                     |
+| `z_max`                            | float  | Maximum of z for `Fixed_xyz_ROI` mode                                                                                     |
+| `z_min`                            | float  | Minimum of z for `Fixed_xyz_ROI` mode                                                                                     |
 
 ## Assumptions / Known limits
 

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp

@@ -30,6 +30,8 @@
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/search/pcl_search.h>
 
+#include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace pointcloud_preprocessor
@@ -48,6 +50,12 @@ enum ReturnType : uint8_t {
   DUAL_ONLY,
 };
 
+std::unordered_map<std::string, uint8_t> roi_mode_map_ = {
+  {"No_ROI", 0},
+  {"Fixed_xyz_ROI", 1},
+  {"Fixed_azimuth_ROI", 2},
+};
+
 class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter
 {
 protected:
@@ -73,6 +81,17 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter
   double visibility_threshold_;
   int vertical_bins_;
   float max_azimuth_diff_;
+  std::string roi_mode_;
+  float x_max_;
+  float x_min_;
+  float y_max_;
+  float y_min_;
+  float z_max_;
+  float z_min_;
+
+  float min_azimuth_deg_;
+  float max_azimuth_deg_;
+  float max_distance_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp

@@ -34,6 +34,15 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent(
 {
   // set initial parameters
   {
+    x_max_ = static_cast<float>(declare_parameter("x_max", 18.0));
+    x_min_ = static_cast<float>(declare_parameter("x_min", -12.0));
+    y_max_ = static_cast<float>(declare_parameter("y_max", 2.0));
+    y_min_ = static_cast<float>(declare_parameter("y_min", -2.0));
+    z_max_ = static_cast<float>(declare_parameter("z_max", 10.0));
+    z_min_ = static_cast<float>(declare_parameter("z_min", 0.0));
+    min_azimuth_deg_ = static_cast<float>(declare_parameter("min_azimuth_deg", 135.0));
+    max_azimuth_deg_ = static_cast<float>(declare_parameter("max_azimuth_deg", 225.0));
+    max_distance_ = static_cast<float>(declare_parameter("max_distance", 12.0));
     vertical_bins_ = static_cast<int>(declare_parameter("vertical_bins", 128));
     max_azimuth_diff_ = static_cast<float>(declare_parameter("max_azimuth_diff", 50.0));
     weak_first_distance_ratio_ =
@@ -42,8 +51,9 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent(
       static_cast<double>(declare_parameter("general_distance_ratio", 1.03));
 
     weak_first_local_noise_threshold_ =
-      static_cast<int>(declare_parameter("weak_first_local_noise_threshold", 10));
+      static_cast<int>(declare_parameter("weak_first_local_noise_threshold", 2));
     visibility_threshold_ = static_cast<float>(declare_parameter("visibility_threshold", 0.5));
+    roi_mode_ = static_cast<std::string>(declare_parameter("roi_mode", "Fixed_xyz_ROI"));
   }
   updater_.setHardwareID("dual_return_outlier_filter");
   updater_.add(
@@ -96,6 +106,23 @@ void DualReturnOutlierFilterComponent::filter(
 
   uint32_t vertical_bins = vertical_bins_;
   uint32_t horizontal_bins = 36;
+  float max_azimuth = 36000.0f;
+  float min_azimuth = 0.0f;
+  switch (roi_mode_map_[roi_mode_]) {
+    case 2: {
+      max_azimuth = max_azimuth_deg_ * 100.0;
+      min_azimuth = min_azimuth_deg_ * 100.0;
+      break;
+    }
+
+    default: {
+      max_azimuth = 36000.0f;
+      min_azimuth = 0.0f;
+      break;
+    }
+  }
+
+  uint32_t horizontal_res = static_cast<uint32_t>((max_azimuth - min_azimuth) / horizontal_bins);
 
   pcl::PointCloud<return_type_cloud::PointXYZIRADT>::Ptr pcl_output(
     new pcl::PointCloud<return_type_cloud::PointXYZIRADT>);
@@ -127,7 +154,7 @@ void DualReturnOutlierFilterComponent::filter(
     if (weak_first_single_ring.points.size() < 2) {
       continue;
     }
-    std::vector<uint> deleted_azimuths;
+    std::vector<float> deleted_azimuths;
     std::vector<float> deleted_distances;
     pcl::PointCloud<return_type_cloud::PointXYZIRADT> temp_segment;
 
@@ -149,9 +176,35 @@ void DualReturnOutlierFilterComponent::filter(
         // Analyse segment points here
       } else {
         // Log the deleted azimuth and its distance for analysis
-        deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth);
-        deleted_distances.push_back(iter->distance);
-        noise_output->points.push_back(*iter);
+        switch (roi_mode_map_[roi_mode_]) {
+          case 1:  // base_link xyz-ROI
+          {
+            if (
+              iter->x > x_min_ && iter->x < x_max_ && iter->y > y_min_ && iter->y < y_max_ &&
+              iter->z > z_min_ && iter->z < z_max_) {
+              deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth);
+              deleted_distances.push_back(iter->distance);
+              noise_output->points.push_back(*iter);
+            }
+            break;
+          }
+          case 2: {
+            if (
+              iter->azimuth > min_azimuth && iter->azimuth < max_azimuth &&
+              iter->distance < max_distance_) {
+              deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth);
+              noise_output->points.push_back(*iter);
+              deleted_distances.push_back(iter->distance);
+            }
+            break;
+          }
+          default: {
+            deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth);
+            deleted_distances.push_back(iter->distance);
+            noise_output->points.push_back(*iter);
+            break;
+          }
+        }
       }
     }
     // Analyse last segment points here
@@ -163,27 +216,54 @@ void DualReturnOutlierFilterComponent::filter(
         continue;
       }
       while ((uint)deleted_azimuths[current_deleted_index] <
-               ((i + 1) * (36000 / horizontal_bins)) &&
+               ((i + static_cast<uint>(min_azimuth / horizontal_res) + 1) * horizontal_res) &&
              current_deleted_index < (deleted_azimuths.size() - 1)) {
         noise_frequency[i] = noise_frequency[i] + 1;
         current_deleted_index++;
       }
-      if (temp_segment.points.size() == 0) {
-        continue;
-      }
-      while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f
-                ? 0.f
-                : temp_segment.points[current_temp_segment_index].azimuth) <
-               ((i + 1) * (36000 / horizontal_bins)) &&
-             current_temp_segment_index < (temp_segment.points.size() - 1)) {
-        if (noise_frequency[i] < weak_first_local_noise_threshold_) {
-          pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]);
-        } else {
-          noise_frequency[i] = noise_frequency[i] + 1;
-          noise_output->points.push_back(temp_segment.points[current_temp_segment_index]);
+      if (temp_segment.points.size() > 0) {
+        while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f
+                  ? 0.f
+                  : temp_segment.points[current_temp_segment_index].azimuth) <
+                 ((i + 1 + static_cast<uint>(min_azimuth / horizontal_res)) * horizontal_res) &&
+               current_temp_segment_index < (temp_segment.points.size() - 1)) {
+          if (noise_frequency[i] < weak_first_local_noise_threshold_) {
+            pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]);
+          } else {
+            switch (roi_mode_map_[roi_mode_]) {
+              case 1: {
+                if (
+                  temp_segment.points[current_temp_segment_index].x < x_max_ &&
+                  temp_segment.points[current_temp_segment_index].x > x_min_ &&
+                  temp_segment.points[current_temp_segment_index].y > y_max_ &&
+                  temp_segment.points[current_temp_segment_index].y < y_min_ &&
+                  temp_segment.points[current_temp_segment_index].z < z_max_ &&
+                  temp_segment.points[current_temp_segment_index].z > z_min_) {
+                  noise_frequency[i] = noise_frequency[i] + 1;
+                  noise_output->points.push_back(temp_segment.points[current_temp_segment_index]);
+                }
+                break;
+              }
+              case 2: {
+                if (
+                  temp_segment.points[current_temp_segment_index].azimuth < max_azimuth &&
+                  temp_segment.points[current_temp_segment_index].azimuth > min_azimuth &&
+                  temp_segment.points[current_temp_segment_index].distance < max_distance_) {
+                  noise_frequency[i] = noise_frequency[i] + 1;
+                  noise_output->points.push_back(temp_segment.points[current_temp_segment_index]);
+                }
+                break;
+              }
+              default: {
+                noise_frequency[i] = noise_frequency[i] + 1;
+                noise_output->points.push_back(temp_segment.points[current_temp_segment_index]);
+                break;
+              }
+            }
+          }
+          current_temp_segment_index++;
+          frequency_image.at<uchar>(ring_id, i) = noise_frequency[i];
         }
-        current_temp_segment_index++;
-        frequency_image.at<uchar>(ring_id, i) = noise_frequency[i];
       }
     }
   }
@@ -220,6 +300,7 @@ void DualReturnOutlierFilterComponent::filter(
       pcl_output->points.push_back(tmp_p);
     }
   }
+
   // Threshold for diagnostics (tunable)
   cv::Mat binary_image;
   cv::inRange(frequency_image, weak_first_local_noise_threshold_, 255, binary_image);
@@ -233,7 +314,7 @@ void DualReturnOutlierFilterComponent::filter(
   cv::applyColorMap(frequency_image * 4, frequency_image_colorized, cv::COLORMAP_JET);
   sensor_msgs::msg::Image::SharedPtr frequency_image_msg =
     cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frequency_image_colorized).toImageMsg();
-
+  frequency_image_msg->header = input->header;
   // Publish histogram image
   image_pub_.publish(frequency_image_msg);
   tier4_debug_msgs::msg::Float32Stamped visibility_msg;