feat(autoware_ground_segmentation): grid data structure revision for …

…efficiency improvement (#9297)

* fix: replace point index to data index

Signed-off-by: Taekjin LEE <[email protected]>

* feat: Use emplace_back instead of push_back for adding gnd_grids in node.cpp

Signed-off-by: Taekjin LEE <[email protected]>

* fix: prep for non-sorted grid process

Signed-off-by: Taekjin LEE <[email protected]>

* feat: Add Cell class and Grid class for grid-based segmentation

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Add Cell and Grid classes for grid-based segmentation

Signed-off-by: Taekjin LEE <[email protected]>

* feat: initialize new grid

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Update Grid class initialization to use radians for azimuth size

refactor: Update Grid class initialization to use radians for azimuth size

refactor: Update Grid class initialization to use radians for azimuth size
Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Fix calculation of azimuth index in Grid class

Signed-off-by: Taekjin LEE <[email protected]>

* feat: implement grid based segmentation, temporary logic

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: idx position convert methods

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Update Grid class initialization to use radians for azimuth size

Signed-off-by: Taekjin LEE <[email protected]>

* feat: reconnect grids filled

Signed-off-by: Taekjin LEE <[email protected]>

* feat: grid initialization

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Update Grid class initialization and reset methods, implement a segmentation logic

refactor: Update Grid class initialization and reset methods, implement a segmentation logic

refactor: replace original methods
Signed-off-by: Taekjin LEE <[email protected]>

* feat: add time_keeper

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: add time keeper in grid class

refactor: remove previous scan ground grid
Signed-off-by: Taekjin LEE <[email protected]>

* refactor: optimize grid boundary calculations and use squared values for radius comparisons

Signed-off-by: Taekjin LEE <[email protected]>

* fix: use pointer for prev cell

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: remove time keeper called too many times

Signed-off-by: Taekjin LEE <[email protected]>

* fix: radial idx estimation fix

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: optimize ground bin average calculation

fix: ground bin logic fix
Signed-off-by: Taekjin LEE <[email protected]>

* refactor: make grid ground filter separate

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: remove unused code

fix: azimuth grid index converter bug
Signed-off-by: Taekjin LEE <[email protected]>

* fix: segmentation logic determination fix

fix: cell connection bug fix
Signed-off-by: Taekjin LEE <[email protected]>

* refactor: optimize pseudoArcTan2 function

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: update grid radial calculation

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: contain input cloud ptr

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: separate ground initialization

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Remove unused code and optimize grid radial calculation

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Inline functions for improved performance

Signed-off-by: Taekjin LEE <[email protected]>

* feat: various azimuth interval per radial distance

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Fix bug in grid ground filter segmentation logic and cell connection

Remove unused code and optimize grid radial calculation

Signed-off-by: Taekjin LEE <[email protected]>

* fix: add missing offset calculation

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Improve grid ground filter segmentation logic and cell connection

Optimize grid radial calculation and remove unused code

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Remove debug print statements and optimize grid initialization

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Update grid radial limit to 200.0m

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Update grid size to 0.5m for improved ground segmentation

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Improve grid ground filter segmentation logic

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Optimize grid ground filter segmentation logic

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: Update logic order for fast segmentation

Signed-off-by: Taekjin LEE <[email protected]>

* fix: resolve cppcheck issue

Signed-off-by: Taekjin LEE <[email protected]>

* fix: pseudo atan2 fix for even distribution of azimuth

Signed-off-by: Taekjin LEE <[email protected]>

* fix: remove unused next_grid_idx_ update

Signed-off-by: Taekjin LEE <[email protected]>

* fix: introduce pseudo tangent to match result of pseudo arc tangent

Signed-off-by: Taekjin LEE <[email protected]>

* style(pre-commit): autofix

Signed-off-by: Taekjin LEE <[email protected]>

* fix: limit gradient

Signed-off-by: Taekjin LEE <[email protected]>

* fix: bring previous average when the ground bin is empty

Signed-off-by: Taekjin LEE <[email protected]>

* fix: back to constant azimuth interval grid

Signed-off-by: Taekjin LEE <[email protected]>

* perf: remove division for efficiency

Signed-off-by: Taekjin LEE <[email protected]>

* perf: remove division for efficiency

Signed-off-by: Taekjin LEE <[email protected]>

* perf: contain radius and height to avoid double calculation

Signed-off-by: Taekjin LEE <[email protected]>

* perf: optimize grid distance calculation for efficiency

Signed-off-by: Taekjin LEE <[email protected]>

* style(pre-commit): autofix

Signed-off-by: Taekjin LEE <[email protected]>

* perf: using isEmpty for efficiency

Signed-off-by: Taekjin LEE <[email protected]>

* chore: initialization fix

Signed-off-by: Taekjin LEE <[email protected]>

* perf:  initial ground cell is integrated into the classify method for efficiency

Signed-off-by: Taekjin LEE <[email protected]>

* perf: refactor grid initialization for efficiency

Signed-off-by: Taekjin LEE <[email protected]>

* perf: optimize grid cell linking for efficiency

Signed-off-by: Taekjin LEE <[email protected]>

* Revert "perf:  initial ground cell is integrated into the classify method for efficiency"

This reverts commit a4ab70b.

Signed-off-by: Taekjin LEE <[email protected]>

* fix: fix pseudo atan2 bug

Signed-off-by: Taekjin LEE <[email protected]>

* feat: various azimuth interval by range

Signed-off-by: Taekjin LEE <[email protected]>

* perf: optimize pseudoArcTan2 function for efficiency

Signed-off-by: Taekjin LEE <[email protected]>

* style(pre-commit): autofix

Signed-off-by: Taekjin LEE <[email protected]>

* fix: avoid zero division on the slope estimation

Signed-off-by: Taekjin LEE <[email protected]>

* fix: limit recursive search

Signed-off-by: Taekjin LEE <[email protected]>

refactor: improve efficiency of recursiveSearch function

Fix function parameter type in GridGroundFilter

* refactor: add comments about unclassified case

Signed-off-by: Taekjin LEE <[email protected]>

* chore: add comment to explain methods

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: remove unnecessary include statement

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: cast point_list size to int in getPointNum method

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: add index check in getCell method

Signed-off-by: Taekjin LEE <[email protected]>

---------

Signed-off-by: Taekjin LEE <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/autoware_ground_segmentation/CMakeLists.txt

@@ -25,6 +25,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/ray_ground_filter/node.cpp
   src/ransac_ground_filter/node.cpp
   src/scan_ground_filter/node.cpp
+  src/scan_ground_filter/grid_ground_filter.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml

@@ -6,7 +6,7 @@
     use_virtual_ground_point: True
     split_height_distance: 0.2
     non_ground_height_threshold: 0.20
-    grid_size_m: 0.1
+    grid_size_m: 0.5
     grid_mode_switch_radius: 20.0
     gnd_grid_buffer_size: 4
     detection_range_z_max: 2.5

perception/autoware_ground_segmentation/src/scan_ground_filter/data.hpp

@@ -0,0 +1,69 @@
+// 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__DATA_HPP_
+#define SCAN_GROUND_FILTER__DATA_HPP_
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+namespace autoware::ground_segmentation
+{
+using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
+
+class PclDataAccessor
+{
+public:
+  PclDataAccessor() = default;
+  ~PclDataAccessor() = default;
+
+  bool isInitialized() const { return data_offset_initialized_; }
+
+  void setField(const PointCloud2ConstPtr & input)
+  {
+    data_offset_x_ = input->fields[pcl::getFieldIndex(*input, "x")].offset;
+    data_offset_y_ = input->fields[pcl::getFieldIndex(*input, "y")].offset;
+    data_offset_z_ = input->fields[pcl::getFieldIndex(*input, "z")].offset;
+    int intensity_index = pcl::getFieldIndex(*input, "intensity");
+    if (intensity_index != -1) {
+      data_offset_intensity_ = input->fields[intensity_index].offset;
+      intensity_type_ = input->fields[intensity_index].datatype;
+    } else {
+      data_offset_intensity_ = -1;
+    }
+    data_offset_initialized_ = true;
+  }
+
+  inline void getPoint(
+    const PointCloud2ConstPtr & input, const size_t data_index, pcl::PointXYZ & point) const
+  {
+    point.x = *reinterpret_cast<const float *>(&input->data[data_index + data_offset_x_]);
+    point.y = *reinterpret_cast<const float *>(&input->data[data_index + data_offset_y_]);
+    point.z = *reinterpret_cast<const float *>(&input->data[data_index + data_offset_z_]);
+  }
+
+private:
+  // data field offsets
+  int data_offset_x_ = 0;
+  int data_offset_y_ = 0;
+  int data_offset_z_ = 0;
+  int data_offset_intensity_ = 0;
+  int intensity_type_ = 0;
+  bool data_offset_initialized_ = false;
+};
+
+}  // namespace autoware::ground_segmentation
+
+#endif  // SCAN_GROUND_FILTER__DATA_HPP_