Skip to content

Commit

Permalink
feat: add grid elevation ground filter (autowarefoundation#154)
Browse files Browse the repository at this point in the history 
* fix(ground_filter): remove base_frame and fix ray_ground_filter  (autowarefoundation#1614)

* fix(ray_ground_filter): cannot remove ground pcl

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: remove base_frame

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* docs: update docs

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chores: remove unnecessary calculation

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chores: remove unnecessary calculation

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* docs: update parameters

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* docs: update parameters

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix(ground segmentation): add elevation grid ground filter (autowarefoundation#1899)

* fix: add grid elevation scan ground filter

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: merge with scan ground filter

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* ci(pre-commit): autofix

* chore: update docs

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: remove debug variables

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: add switchable param for grid scan mode

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* ci(pre-commit): autofix

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* docs: update docs

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* ci(pre-commit): autofix

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chores: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* docs: update

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@tkimura4 @badai-nguyen @pre-commit-ci
3 people authored Oct 17, 2022
1 parent 9676275 commit ba1bfca
Show file tree
Hide file tree
Showing 10 changed files with 404 additions and 135 deletions.
12 changes: 10 additions & 2 deletions ...on_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,21 @@
max_x: 100.0
min_y: -50.0
max_y: 50.0
max_z: 10.7 # recommended 2.5 for non elevation_grid_mode
min_z: -8.7 # recommended 0.0 for non elevation_grid_mode
negative: False

common_ground_filter:
plugin: "ground_segmentation::ScanGroundFilterComponent"
parameters:
global_slope_max_angle_deg: 10.0
local_slope_max_angle_deg: 30.0
local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode
split_points_distance_tolerance: 0.2
split_height_distance: 0.3
use_virtual_ground_point: True
split_height_distance: 0.2
non_ground_height_threshold: 0.20
grid_size_m: 0.1
grid_mode_switch_radius: 20.0
gnd_grid_buffer_size: 4
detection_range_z_max: 2.5
elevation_grid_mode: true
8 changes: 2 additions & 6 deletions ...ion_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,6 @@ def get_vehicle_info(self):
p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"]
p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"])
p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"]
p["min_height_offset"] = 0.0
p["max_height_offset"] = p["vehicle_height"]
return p

def get_vehicle_mirror_info(self):
Expand All @@ -82,8 +80,6 @@ def create_additional_pipeline(self, lidar_name):
{
"input_frame": LaunchConfiguration("base_frame"),
"output_frame": LaunchConfiguration("base_frame"),
"min_z": self.vehicle_info["min_height_offset"],
"max_z": self.vehicle_info["max_height_offset"],
},
self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"],
],
Expand Down Expand Up @@ -216,8 +212,6 @@ def create_common_pipeline(self, input_topic, output_topic):
{
"input_frame": LaunchConfiguration("base_frame"),
"output_frame": LaunchConfiguration("base_frame"),
"min_z": self.vehicle_info["min_height_offset"],
"max_z": self.vehicle_info["max_height_offset"],
},
self.ground_segmentation_param["common_crop_box_filter"]["parameters"],
],
Expand All @@ -239,6 +233,8 @@ def create_common_pipeline(self, input_topic, output_topic):
parameters=[
self.ground_segmentation_param["common_ground_filter"]["parameters"],
self.vehicle_info,
{"input_frame": "base_link"},
{"output_frame": "base_link"},
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
Expand Down
1 change: 1 addition & 0 deletions launch/tier4_perception_launch/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
<version>0.1.0</version>
<description>The tier4_perception_launch package</description>
<maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
<maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
Expand Down
1 change: 1 addition & 0 deletions perception/ground_segmentation/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ rclcpp_components_register_node(ground_segmentation
PLUGIN "ground_segmentation::ScanGroundFilterComponent"
EXECUTABLE scan_ground_filter_node)


ament_auto_package(INSTALL_TO_SHARE
launch
)
5 changes: 4 additions & 1 deletion perception/ground_segmentation/docs/ray-ground-filter.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref

| Name | Type | Description |
| ----------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| `base_frame` | string | The target frame of input points |
| `input_frame` | string | frame id of input pointcloud |
| `output_frame` | string | frame id of output pointcloud |
| `general_max_slope` | double | The triangle created by `general_max_slope` is called the global cone. If the point is outside the global cone, it is judged to be a point that is no on the ground |
| `initial_max_slope` | double | Generally, the point where the object first hits is far from ego-vehicle because of sensor blind spot, so resolution is different from that point and thereafter, so this parameter exists to set a separate `local_max_slope` |
| `local_max_slope` | double | The triangle created by `local_max_slope` is called the local cone. This parameter for classification based on the continuity of points |
Expand All @@ -41,6 +42,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref

## Assumptions / Known limits

The input_frame is set as parameter but it must be fixed as base_link for the current algorithm.

## (Optional) Error detection and handling

## (Optional) Performance characterization
Expand Down
44 changes: 29 additions & 15 deletions perception/ground_segmentation/docs/scan-ground-filter.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,13 @@ 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. Check the distance and vertical angle of the point one by one.
3. Set a center of the ground contact point of the rear or front wheels as the initial point.
4. Check vertical angle between the points. If the angle from the initial point is larger than "global_slope_max", the point is classified as "no ground".
5. If the angle from the previous point is larger than "local_max_slope", the point is classified as "no ground".
6. Otherwise the point is labeled as "ground point".
7. If the distance from the last checked point is close, ignore any vertical angle and set current point attribute to the same as the last point.
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

## Inputs / Outputs

Expand All @@ -28,24 +29,37 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref

#### Core Parameters

| Name | Type | Default Value | Description |
| --------------------------------- | ------ | ------------- | ----------------------------------------------------------------------------- |
| `base_frame` | string | "base_link" | base_link frame |
| `global_slope_max` | double | 8.0 | The global angle to classify as the ground or object [deg] |
| `local_max_slope` | double | 6.0 | The local angle to classify as the ground or object [deg] |
| `radial_divider_angle` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] |
| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to to distinguishing far and near [m] |
| `split_height_distance` | double | 0.2 | The height threshold to distinguishing far and near [m] |
| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. |
| Name | Type | Default Value | Description |
| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------- |
| `input_frame` | string | "base_link" | frame id of input pointcloud |
| `output_frame` | string | "base_link" | frame id of output pointcloud |
| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg] |
| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] |
| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] |
| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to to distinguishing far and near [m] |
| `split_height_distance` | double | 0.2 | The height threshold to distinguishing far and near [m] |
| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. |
| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode |
| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m], <br /> recommended to use only for additional LiDARs in elevation_grid_mode |
| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m], applied only for elevation_grid_mode |
| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],<br /> applied only for elevation_grid_mode |
| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode |
| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,<br /> applied only for elevation_grid_mode |
| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] |
| `elevation_grid_mode` | bool | true | Elevation grid scan mode option |

## Assumptions / Known limits

The input_frame is set as parameter but it must be fixed as base_link for the current algorithm.

## (Optional) Error detection and handling

## (Optional) Performance characterization

## (Optional) References/External links

The elevation grid idea is referred from "Shen Z, Liang H, Lin L, Wang Z, Huang W, Yu J. Fast Ground Segmentation for 3D LiDAR Point Cloud Based on Jump-Convolution-Process. Remote Sensing. 2021; 13(16):3239. <https://doi.org/10.3390/rs13163239>"

## (Optional) Future extensions / Unimplemented parts

- Horizontal check for classification is not implemented yet.
Expand Down
4 changes: 0 additions & 4 deletions perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,6 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;

private:
std::string base_frame_ = "base_link";
double general_max_slope_; // degrees
double local_max_slope_; // degrees
double initial_max_slope_; // degrees
Expand Down Expand Up @@ -150,9 +149,6 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
* @retval true transform succeeded
* @retval false transform failed
*/
bool TransformPointCloud(
const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr,
const PointCloud2::SharedPtr & out_cloud_ptr);

/*!
*
Expand Down
56 changes: 49 additions & 7 deletions perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,12 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
POINT_FOLLOW,
UNKNOWN,
VIRTUAL_GROUND,
OUT_OF_RANGE
};
struct PointRef
{
float grid_size; // radius of grid
uint16_t grid_id; // id of grid in vertical
float radius; // cylindrical coords on XY Plane
float theta; // angle deg on XY plane
size_t radial_div; // index of the radial division to which this point belongs to
Expand All @@ -67,13 +70,23 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
};
using PointCloudRefVector = std::vector<PointRef>;

struct GridCenter
{
float radius;
float avg_height;
float max_height;
uint16_t grid_id;
};

struct PointsCentroid
{
float radius_sum;
float height_sum;
float radius_avg;
float height_avg;
float height_max;
uint32_t point_num;
uint16_t grid_id;

PointsCentroid()
: radius_sum(0.0f), height_sum(0.0f), radius_avg(0.0f), height_avg(0.0f), point_num(0)
Expand All @@ -86,7 +99,9 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
height_sum = 0.0f;
radius_avg = 0.0f;
height_avg = 0.0f;
height_max = 0.0f;
point_num = 0;
grid_id = 0;
}

void addPoint(const float radius, const float height)
Expand All @@ -96,13 +111,18 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
++point_num;
radius_avg = radius_sum / point_num;
height_avg = height_sum / point_num;
height_max = height_max < height ? height : height_max;
}

float getAverageSlope() { return std::atan2(height_avg, radius_avg); }

float getAverageHeight() { return height_avg; }

float getAverageRadius() { return radius_avg; }

float getMaxHeight() { return height_max; }

uint16_t getGridId() { return grid_id; }
};

void filter(
Expand All @@ -111,8 +131,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
tf2_ros::Buffer tf_buffer_{get_clock()};
tf2_ros::TransformListener tf_listener_{tf_buffer_};

std::string base_frame_;
std::string sensor_frame_;
const uint16_t gnd_grid_continual_thresh_ = 3;
bool elevation_grid_mode_;
float non_ground_height_threshold_;
float grid_size_rad_;
float grid_size_m_;
float low_priority_region_x_;
uint16_t gnd_grid_buffer_size_;
float grid_mode_switch_grid_id_;
float grid_mode_switch_angle_rad_;
float virtual_lidar_z_;
float detection_range_z_max_;
float center_pcl_shift_; // virtual center of pcl to center mass
float grid_mode_switch_radius_; // non linear grid size switching distance
double global_slope_max_angle_rad_; // radians
double local_slope_max_angle_rad_; // radians
double radial_divider_angle_rad_; // distance in rads between dividers
Expand All @@ -131,9 +162,6 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
* @retval true transform succeeded
* @retval false transform failed
*/
bool transformPointCloud(
const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr,
const PointCloud2::SharedPtr & out_cloud_ptr);

/*!
* Convert pcl::PointCloud to sorted PointCloudRefVector
Expand All @@ -144,7 +172,9 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
void convertPointcloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);

void convertPointcloudGridScan(
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
/*!
* Output ground center of front wheels as the virtual ground point
* @param[out] point Virtual ground origin point
Expand All @@ -158,10 +188,22 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
* @param out_no_ground_indices Returns the indices of the points
* classified as not ground in the original PointCloud
*/

void initializeFirstGndGrids(
const float h, const float r, const uint16_t id, std::vector<GridCenter> & gnd_grids);

void checkContinuousGndGrid(
PointRef & p, const std::vector<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster);
void checkDiscontinuousGndGrid(
PointRef & p, const std::vector<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster);
void checkBreakGndGrid(
PointRef & p, const std::vector<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster);
void classifyPointCloud(
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
pcl::PointIndices & out_no_ground_indices);

void classifyPointCloudGridScan(
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
pcl::PointIndices & out_no_ground_indices);
/*!
* Returns the resulting complementary PointCloud, one with the points kept
* and the other removed as indicated in the indices
Expand Down
Loading

0 comments on commit ba1bfca

Please sign in to comment.