chore(autoware_ground_segmentation): scan ground filter refactoring (a…

…utowarefoundation#9061) * chore: Add comment classification logic for point cloud grid scan Signed-off-by: Taekjin LEE <[email protected]> * chore: renamed horizontal angle to azimuth angle Signed-off-by: Taekjin LEE <[email protected]> * chore: rename offset to data_index Signed-off-by: Taekjin LEE <[email protected]> * chore: rename ground_cluster to centroid_bin chore: Refactor recheckGroundCluster function in scan_ground_filter Signed-off-by: Taekjin LEE <[email protected]> * chore: rename too short variables Signed-off-by: Taekjin LEE <[email protected]> * refactor: set input to be const Signed-off-by: Taekjin LEE <[email protected]> * refactor: update functions to be const Signed-off-by: Taekjin LEE <[email protected]> * chore: reorder params Signed-off-by: Taekjin LEE <[email protected]> * refactor: Add ScanGroundGrid class for managing grid data Signed-off-by: Taekjin LEE <[email protected]> * refactor: Update grid parameters and calculations in ScanGroundGrid class Signed-off-by: Taekjin LEE <[email protected]> * refactor: remove unused methods Signed-off-by: Taekjin LEE <[email protected]> * refactor: classification description Signed-off-by: Taekjin LEE <[email protected]> * refactor: initialize members in ScanGroundGrid class Signed-off-by: Taekjin LEE <[email protected]> * refactor: remove unused value Signed-off-by: Taekjin LEE <[email protected]> * chore: reduce scope Signed-off-by: Taekjin LEE <[email protected]> * refactor: align structure between convertPointcloud and convertPointcloudGridScan Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]>
technolojin · Oct 10, 2024 · f49e7a6 · f49e7a6
1 parent 41e08dc
commit f49e7a6
Show file tree

Hide file tree

Showing 4 changed files with 432 additions and 307 deletions.
diff --git a/perception/autoware_ground_segmentation/docs/scan-ground-filter.md b/perception/autoware_ground_segmentation/docs/scan-ground-filter.md
@@ -8,14 +8,16 @@ The purpose of this node is that remove the ground points from the input pointcl
 
 This algorithm works by following steps,
 
-1. Divide whole pointclouds into groups by horizontal angle and sort by xy-distance.
-2. Divide sorted pointclouds of each ray into grids
-3. Check the xy distance to previous pointcloud, if the distance is large and previous pointcloud is "no ground" and the height level of current point greater than previous point, the current pointcloud is classified as no ground.
-4. Check vertical angle of the point compared with previous ground grid
-5. Check the height of the point compared with predicted ground level
-6. If vertical angle is greater than local_slope_max and related height to predicted ground level is greater than "non ground height threshold", the point is classified as "non ground"
-7. If the vertical angle is in range of [-local_slope_max, local_slope_max] or related height to predicted ground level is smaller than non_ground_height_threshold, the point is classified as "ground"
-8. If the vertical angle is lower than -local_slope_max or the related height to ground level is greater than detection_range_z_max, the point will be classified as out of range
+1. Divide whole pointclouds into groups by azimuth angle (so-called ray)
+2. Sort points by radial distance (xy-distance), on each ray.
+3. Divide pointcloud into grids, on each ray.
+4. Classify the point
+   1. Check radial distance to previous pointcloud, if the distance is large and previous pointcloud is "no ground" and the height level of current point greater than previous point, the current pointcloud is classified as no ground.
+   2. Check vertical angle of the point compared with previous ground grid
+   3. Check the height of the point compared with predicted ground level
+   4. If vertical angle is greater than local_slope_max and related height to predicted ground level is greater than "non ground height threshold", the point is classified as "non ground"
+   5. If the vertical angle is in range of [-local_slope_max, local_slope_max] or related height to predicted ground level is smaller than non_ground_height_threshold, the point is classified as "ground"
+   6. If the vertical angle is lower than -local_slope_max or the related height to ground level is greater than detection_range_z_max, the point will be classified as out of range
 
 ## Inputs / Outputs
 

diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp
@@ -0,0 +1,111 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SCAN_GROUND_FILTER__GRID_HPP_
+#define SCAN_GROUND_FILTER__GRID_HPP_
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware/universe_utils/math/normalization.hpp>
+
+#include <cmath>
+
+namespace autoware::ground_segmentation
+{
+
+class ScanGroundGrid
+{
+public:
+  ScanGroundGrid() = default;
+  ~ScanGroundGrid() = default;
+
+  void initialize(
+    const float grid_size_m, const float grid_mode_switch_radius, const float virtual_lidar_z)
+  {
+    grid_size_m_ = grid_size_m;
+    mode_switch_radius_ = grid_mode_switch_radius;
+    virtual_lidar_z_ = virtual_lidar_z;
+
+    // calculate parameters
+    inv_grid_size_m_ = 1.0f / grid_size_m_;
+    mode_switch_grid_id_ = mode_switch_radius_ * inv_grid_size_m_;
+    mode_switch_angle_rad_ = std::atan2(mode_switch_radius_, virtual_lidar_z_);
+
+    grid_size_rad_ = universe_utils::normalizeRadian(
+                       std::atan2(mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) -
+                     universe_utils::normalizeRadian(mode_switch_angle_rad_);
+    inv_grid_size_rad_ = 1.0f / grid_size_rad_;
+    tan_grid_size_rad_ = std::tan(grid_size_rad_);
+    grid_id_offset_ = mode_switch_grid_id_ - mode_switch_angle_rad_ * inv_grid_size_rad_;
+
+    is_initialized_ = true;
+  }
+
+  float getGridSize(const float radius, const size_t grid_id) const
+  {
+    // check if initialized
+    if (!is_initialized_) {
+      throw std::runtime_error("ScanGroundGrid is not initialized.");
+    }
+
+    float grid_size = grid_size_m_;
+    constexpr uint16_t back_steps_num = 1;
+
+    if (radius > mode_switch_radius_ && grid_id > mode_switch_grid_id_ + back_steps_num) {
+      // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) *
+      // virtual_lidar_z_
+      // where gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)
+      grid_size = radius - (radius - tan_grid_size_rad_ * virtual_lidar_z_) /
+                             (1 + radius * tan_grid_size_rad_ / virtual_lidar_z_);
+    }
+    return grid_size;
+  }
+
+  uint16_t getGridId(const float radius) const
+  {
+    // check if initialized
+    if (!is_initialized_) {
+      throw std::runtime_error("ScanGroundGrid is not initialized.");
+    }
+
+    uint16_t grid_id = 0;
+    if (radius <= mode_switch_radius_) {
+      grid_id = static_cast<uint16_t>(radius * inv_grid_size_m_);
+    } else {
+      auto gamma{universe_utils::normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)};
+      grid_id = grid_id_offset_ + gamma * inv_grid_size_rad_;
+    }
+    return grid_id;
+  }
+
+private:
+  bool is_initialized_ = false;
+
+  // configured parameters
+  float grid_size_m_ = 0.0f;
+  float mode_switch_radius_ = 0.0f;
+  float virtual_lidar_z_ = 0.0f;
+
+  // calculated parameters
+  float inv_grid_size_m_ = 0.0f;
+  float grid_size_rad_ = 0.0f;
+  float inv_grid_size_rad_ = 0.0f;
+  float tan_grid_size_rad_ = 0.0f;
+  float mode_switch_grid_id_ = 0.0f;
+  float mode_switch_angle_rad_ = 0.0f;
+  float grid_id_offset_ = 0.0f;
+};
+
+}  // namespace autoware::ground_segmentation
+
+#endif  // SCAN_GROUND_FILTER__GRID_HPP_