intensity filter, outlier filter

Signed-off-by: amc-nu <[email protected]>
MasatoSaeki · Nov 2, 2023 · b9872f3 · b9872f3
1 parent 314e356
commit b9872f3
Show file tree

Hide file tree

Showing 5 changed files with 140 additions and 30 deletions.
diff --git a/include/ros2_ground_segmentation/ray/ray_ground_params.h b/include/ros2_ground_segmentation/ray/ray_ground_params.h
@@ -33,7 +33,8 @@ struct RayGroundParams {
   : general_max_slope(3.0), local_max_slope(5.0), sensor_height(0.0),
   radial_divider_angle(0.1), concentric_divider_distance(0.01), min_height_threshold(0.05),
   clipping_height(0.2), pointcloud_min_z(-0.1), min_point_distance(0.5),
-  reclass_distance_threshold(0.2), outlier_filter(false){
+  reclass_distance_threshold(0.2), outlier_filter(false), min_outlier_filter_neighbors(5),
+  min_outlier_filter_radius(0.5), intensity_filter(false), min_intensity(4), max_intensity_distance(20.0) {
   }
   double general_max_slope;
   double local_max_slope;
@@ -46,6 +47,11 @@ struct RayGroundParams {
   double min_point_distance;
   double reclass_distance_threshold;
   bool outlier_filter;
+  int min_outlier_filter_neighbors;
+  double min_outlier_filter_radius;
+  bool intensity_filter;
+  int min_intensity;
+  double max_intensity_distance;
 };
 
 #endif //ROS2_RAYGROUND_PARAMS_H
diff --git a/include/ros2_ground_segmentation/ray/ray_ground_seg.h b/include/ros2_ground_segmentation/ray/ray_ground_seg.h
@@ -96,6 +96,17 @@ class RayGroundSeg {
                         double in_min_distance,
                         pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr);
 
+  /*!
+ * Removes points up to a certain distance in the XY Plane
+ * @param in_cloud_ptr Input PointCloud
+ * @param in_min_distance Minimum valid distance, points closer than this will be removed.
+ * @param out_filtered_cloud_ptr Resulting PointCloud with the invalid points removed.
+ */
+  static void FilterByIntensity(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
+                               int min_intensity,
+                               double max_distance,
+                               pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr);
+
   /*!
  * Returns the resulting complementary PointCloud, one with the points kept and the other removed as indicated
  * in the indices

diff --git a/launch/ray_ground_segmentation.launch.xml b/launch/ray_ground_segmentation.launch.xml
@@ -7,8 +7,8 @@
     <arg name="sensor_height" default="0.0"/>
     <arg name="output_frame" default="base_link"/>
 
-    <arg name="general_max_slope" default="3.0"/>
-    <arg name="local_max_slope" default="4.0"/>
+    <arg name="general_max_slope" default="10.0"/>
+    <arg name="local_max_slope" default="10.0"/>
     <arg name="radial_divider_angle" default="0.1"/>
     <arg name="concentric_divider_distance" default="0.01"/>
     <arg name="min_height_threshold" default="0.1"/>
@@ -18,6 +18,9 @@
     <arg name="reclass_distance_threshold" default="0.2"/>
     <arg name="outlier_filter" default="false"/>
 
+    <arg name="intensity_filter" default="false"/>
+    <arg name="min_intensity" default="25"/>
+    <arg name="max_intensity_distance" default="20.0"/>
 
     <node
         pkg="tf2_ros"
@@ -26,22 +29,46 @@
         args="0.0 0.0 0.5 1.5714 0.01 0.005 base_link pandar_xt32_front_center"
     />
 
-    <node pkg="ros2_ground_segmentation" exec="ray_ground_segmentation_node" name="ray_ground_segmentation_node" output="screen">
-        <remap from="input/topic" to="$(var input/topic)" />
-        <remap from="output/topic" to="$(var output/topic)" />
-        <param name="sensor_height" value="$(var sensor_height)"/>
-        <param name="output_frame" value="$(var output_frame)"/>
-        <param name="general_max_slope" value="$(var general_max_slope)"/>
-        <param name="local_max_slope" value="$(var local_max_slope)"/>
-        <param name="radial_divider_angle" value="$(var radial_divider_angle)"/>
-        <param name="concentric_divider_distance" value="$(var concentric_divider_distance)"/>
-        <param name="min_height_threshold" value="$(var min_height_threshold)"/>
-        <param name="clipping_height" value="$(var clipping_height)"/>
-        <param name="pointcloud_min_z" value="$(var pointcloud_min_z)"/>
-        <param name="min_point_distance" value="$(var min_point_distance)"/>
-        <param name="reclass_distance_threshold" value="$(var reclass_distance_threshold)"/>
-        <param name="outlier_filter" value="$(var outlier_filter)"/>
-    </node>
+    <group>
+        <node_container pkg="rclcpp_components" exec="component_container_mt" name="pointcloud_container" namespace="/">
+            <composable_node pkg="ros2_ground_segmentation"
+                             plugin="RayGroundSegmentation"
+                             name="ray_ground_filter_node"
+                             namespace="">
+                <remap from="input/topic" to="$(var input/topic)" />
+                <remap from="output/topic" to="points_processed" />
+
+                <param name="sensor_height" value="$(var sensor_height)"/>
+                <param name="output_frame" value="$(var output_frame)"/>
+                <param name="general_max_slope" value="$(var general_max_slope)"/>
+                <param name="local_max_slope" value="$(var local_max_slope)"/>
+                <param name="radial_divider_angle" value="$(var radial_divider_angle)"/>
+                <param name="concentric_divider_distance" value="$(var concentric_divider_distance)"/>
+                <param name="min_height_threshold" value="$(var min_height_threshold)"/>
+                <param name="clipping_height" value="$(var clipping_height)"/>
+                <param name="pointcloud_min_z" value="$(var pointcloud_min_z)"/>
+                <param name="min_point_distance" value="$(var min_point_distance)"/>
+                <param name="reclass_distance_threshold" value="$(var reclass_distance_threshold)"/>
+                <param name="outlier_filter" value="$(var outlier_filter)"/>
+                <param name="intensity_filter" value="$(var intensity_filter)"/>
+                <param name="min_intensity" value="$(var min_intensity)"/>
+                <param name="max_intensity_distance" value="$(var max_intensity_distance)"/>
+
+                <extra_arg name="use_intra_process_comms" value="true" />
+            </composable_node>
+
+            <composable_node pkg="euclidean_cluster"
+                             plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode"
+                             name="voxel_grid_based_euclidean_cluster_node"
+                             namespace="">
+                <remap from="input" to="points_processed" />
+                <remap from="output" to="euclidean_clustered/cluster" />
+                <param from="$(find-pkg-share euclidean_cluster)/config/voxel_grid_based_euclidean_cluster.param.yaml" />
+                <extra_arg name="use_intra_process_comms" value="true" />
+            </composable_node>
+        </node_container>
+    </group>
+
     <node pkg="rviz2" exec="rviz2" />
     <node pkg="rqt_gui" exec="rqt_gui" />
 

diff --git a/src/ray/ray_ground_seg.cpp b/src/ray/ray_ground_seg.cpp
@@ -50,14 +50,27 @@ void RayGroundSeg::segmentGround(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr
                    params_.min_point_distance,
                    filtered_cloud_ptr);
 
-    PointCloudXYZIRTColor organized_points;
+  //filter by intensity if enabled
+  pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_filtered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  if (params_.intensity_filter) {
+    FilterByIntensity(filtered_cloud_ptr,
+                      params_.min_intensity,
+                      params_.max_intensity_distance,
+                      intensity_filtered_cloud_ptr);
+    std::cout << "Intensity B: " << filtered_cloud_ptr->points.size() << " | A: " << intensity_filtered_cloud_ptr->points.size() << "\n";
+  }
+  else {
+    intensity_filtered_cloud_ptr = filtered_cloud_ptr;
+  }
+
+  PointCloudXYZIRTColor organized_points;
   std::vector<pcl::PointIndices> radial_division_indices;
   std::vector<pcl::PointIndices> closest_indices;
   std::vector<PointCloudXYZIRTColor> radial_ordered_clouds;
 
   radial_dividers_num_ = ceil(360 / params_.radial_divider_angle);
 
-  ConvertXYZIToRTZColor(filtered_cloud_ptr,
+  ConvertXYZIToRTZColor(intensity_filtered_cloud_ptr,
                         organized_points,
                         radial_division_indices,
                         radial_ordered_clouds);
@@ -66,7 +79,7 @@ void RayGroundSeg::segmentGround(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr
 
   ClassifyPointCloud(radial_ordered_clouds, ground_indices, no_ground_indices);
 
-  ExtractPointsIndices(filtered_cloud_ptr,
+  ExtractPointsIndices(intensity_filtered_cloud_ptr,
                        ground_indices,
                        out_ground_points,
                        out_groundless_points);
@@ -78,11 +91,10 @@ void RayGroundSeg::segmentGround(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr
     pcl::RadiusOutlierRemoval<pcl::PointXYZI> outlier_rem;
 
     outlier_rem.setInputCloud(out_groundless_points);
-    outlier_rem.setRadiusSearch(0.5);
-    outlier_rem.setMinNeighborsInRadius (1);
-    outlier_rem.filter (*no_ground_clean_cloud_ptr);
-
-    out_groundless_points = no_ground_clean_cloud_ptr;
+    outlier_rem.setRadiusSearch(params_.min_outlier_filter_radius);
+    outlier_rem.setMinNeighborsInRadius (params_.min_outlier_filter_neighbors);
+    outlier_rem.filter (*out_groundless_points);
+    std::cout << "Radius B: " << out_groundless_points->points.size()<< " | A: " << out_groundless_points->points.size()<<"\n";
   }
 }
 
@@ -152,6 +164,32 @@ void RayGroundSeg::RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Ptr i
   extractor.filter(*out_filtered_cloud_ptr);
 }
 
+void RayGroundSeg::FilterByIntensity(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
+                                     int min_intensity,
+                                     double max_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 ( in_cloud_ptr->points[i].intensity < min_intensity
+    && std::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)
+      < max_distance
+    )
+    {
+      indices.indices.push_back(i);
+    }
+  }
+  extractor.setIndices(std::make_shared<pcl::PointIndices>(indices));
+  extractor.setNegative(true);//true removes the indices, false leaves only the indices
+  extractor.filter(*out_filtered_cloud_ptr);
+}
+
 /*!
  *
  * @param[in] in_cloud Input Point Cloud to be organized in radial segments

diff --git a/src/ray_ground_segmentation.cpp b/src/ray_ground_segmentation.cpp
@@ -95,6 +95,26 @@ rcl_interfaces::msg::SetParametersResult RayGroundSegmentation::ReconfigureCallb
     RCLCPP_INFO_STREAM(get_logger(), "Setting outlier_filter to: " << rayground_params_.outlier_filter);
     update = true;
   }
+  if (get_param(p, "min_outlier_filter_neighbors", rayground_params_.min_outlier_filter_neighbors)) {
+    RCLCPP_INFO_STREAM(get_logger(), "Setting min_outlier_filter_neighbors to: " << rayground_params_.min_outlier_filter_neighbors);
+    update = true;
+  }
+  if (get_param(p, "min_outlier_filter_radius", rayground_params_.min_outlier_filter_radius)) {
+    RCLCPP_INFO_STREAM(get_logger(), "Setting min_outlier_filter_radius to: " << rayground_params_.min_outlier_filter_radius);
+    update = true;
+  }
+  if (get_param(p, "intensity_filter", rayground_params_.intensity_filter)) {
+    RCLCPP_INFO_STREAM(get_logger(), "Setting intensity_filter to: " << rayground_params_.intensity_filter);
+    update = true;
+  }
+  if (get_param(p, "min_intensity", rayground_params_.min_intensity)) {
+    RCLCPP_INFO_STREAM(get_logger(), "Setting min_intensity to: " << rayground_params_.min_intensity);
+    update = true;
+  }
+  if (get_param(p, "max_intensity_distance", rayground_params_.max_intensity_distance)) {
+    RCLCPP_INFO_STREAM(get_logger(), "Setting max_intensity_distance to: " << rayground_params_.max_intensity_distance);
+    update = true;
+  }
 
   if (update) {
     rayground_ptr_->setParameters(rayground_params_);
@@ -136,6 +156,17 @@ RayGroundParams RayGroundSegmentation::GetParameters() {
 
   this->declare_parameter<bool>("outlier_filter", false, descriptor);
   rayground_params.outlier_filter = this->get_parameter("outlier_filter").as_bool();
+  this->declare_parameter<int>("min_outlier_filter_neighbors", 5, descriptor);
+  rayground_params.min_outlier_filter_neighbors = this->get_parameter("min_outlier_filter_neighbors").as_int();
+  this->declare_parameter<double>("min_outlier_filter_radius", 0.5, descriptor);
+  rayground_params.min_outlier_filter_radius = this->get_parameter("min_outlier_filter_radius").as_double();
+
+  this->declare_parameter<bool>("intensity_filter", false, descriptor);
+  rayground_params.intensity_filter = this->get_parameter("intensity_filter").as_bool();
+  this->declare_parameter<int>("min_intensity", 5, descriptor);
+  rayground_params.min_intensity = this->get_parameter("min_intensity").as_int();
+  this->declare_parameter<double>("max_intensity_distance", 20.0, descriptor);
+  rayground_params.max_intensity_distance = this->get_parameter("max_intensity_distance").as_double();
 
   this->declare_parameter<std::string>("output_frame", "base_link");
   output_frame_ = this->get_parameter("output_frame").as_string();
@@ -192,9 +223,6 @@ void RayGroundSegmentation::CloudCallback(const sensor_msgs::msg::PointCloud2::C
     rayground_ptr_->segmentGround(raw_pointcloud_ptr,
                                   no_ground_pointcloud_ptr,
                                   ground_pointcloud_ptr);
-    RCLCPP_INFO_STREAM(get_logger(), "points:" << raw_pointcloud_ptr->points.size()
-    <<" no_g: " << no_ground_pointcloud_ptr->points.size()
-    <<" gr: " << ground_pointcloud_ptr->points.size());
 
     // convert pcl to ros
     auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();