Skip to content

Commit 47d2ba7

Browse files
ktro2828badai-nguyenamadeuszszmanatopre-commit-ci-lite[bot]
authored
feat(perception): apply new perception pipeline (tier4/autoware_universe#3008)
* fix: update ptv3 filter classes Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * feat: add label-based clustering algorithm Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp> * feat: update cluster Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp> * feat: update cluster Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp> * feat(object_merger): add a new merger (tier4/autoware_universe#2906) * fix: missign mapped_label Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: move shape policy to launch Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: label_based_EU node name Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: using voxel_based EC Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * feat(autoware_ptv3): source cloud reconstruction & entropy (tier4/autoware_universe#12547) * feat(autoware_ptv3): source cloud reconstruction & entropy Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * refactor(ptv3): clarify reconstruction feature handling Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * refactor(autoware_ptv3): remove unnecessary buffers clear Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * refactor(ptv3): simplify source reconstruction buffers Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * fix(autoware_ptv3): remove redundant memory clear Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * feat(autoware_ptv3): add __restrict__ Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> Co-authored-by: Manato Hirabayashi <3022416+manato@users.noreply.github.com> * style(pre-commit): autofix * feat(autoware_ptv3): add restrict Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * feat(autoware_ptv3): parallelizaiton Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> --------- Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> Co-authored-by: Manato Hirabayashi <3022416+manato@users.noreply.github.com> Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> * change to partial reconstruction Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: imu scale estimation covariance limit for stability Signed-off-by: Sergio Reyes Sanchez <sergioreyes.sanchez@tier4.jp> * chore: fix pre-commit Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp> Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> Signed-off-by: Sergio Reyes Sanchez <sergioreyes.sanchez@tier4.jp> Co-authored-by: badai-nguyen <dai.nguyen@tier4.jp> Co-authored-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> Co-authored-by: Manato Hirabayashi <3022416+manato@users.noreply.github.com> Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Co-authored-by: Sergio Reyes Sanchez <sergioreyes.sanchez@tier4.jp>
1 parent 4ae75e9 commit 47d2ba7

32 files changed

Lines changed: 3609 additions & 41 deletions

‎perception/autoware_euclidean_cluster/CMakeLists.txt‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ project(autoware_euclidean_cluster)
44
find_package(autoware_cmake REQUIRED)
55
autoware_package()
66

7+
find_package(autoware_shape_estimation REQUIRED)
78
find_package(PCL REQUIRED)
89
find_package(autoware_agnocast_wrapper REQUIRED)
910

@@ -62,6 +63,26 @@ autoware_agnocast_wrapper_register_node(${PROJECT_NAME}_voxel_grid_node_core
6263
ROS2_EXECUTOR SingleThreadedExecutor
6364
)
6465

66+
ament_auto_add_library(${PROJECT_NAME}_label_based_node_core SHARED
67+
src/label_based_euclidean_cluster_node.cpp
68+
)
69+
target_link_libraries(${PROJECT_NAME}_label_based_node_core
70+
${PCL_LIBRARIES}
71+
${PROJECT_NAME}_lib
72+
)
73+
ament_target_dependencies(${PROJECT_NAME}_label_based_node_core
74+
autoware_agnocast_wrapper
75+
autoware_shape_estimation
76+
)
77+
autoware_agnocast_wrapper_setup(${PROJECT_NAME}_label_based_node_core)
78+
79+
autoware_agnocast_wrapper_register_node(${PROJECT_NAME}_label_based_node_core
80+
PLUGIN "autoware::euclidean_cluster::LabelBasedEuclideanClusterNode"
81+
EXECUTABLE label_based_euclidean_cluster_node
82+
AGNOCAST_EXECUTOR CallbackIsolatedAgnocastExecutor
83+
ROS2_EXECUTOR SingleThreadedExecutor
84+
)
85+
6586
if(BUILD_TESTING)
6687
find_package(ament_cmake_gtest REQUIRED)
6788
ament_auto_add_gtest(test_voxel_grid_based_euclidean_cluster_fusion

‎perception/autoware_euclidean_cluster/README.md‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,20 @@ This package has two clustering methods: `euclidean_cluster` and `voxel_grid_bas
4545

4646
{{ json_to_markdown("perception/autoware_euclidean_cluster/schema/voxel_grid_based_euclidean_cluster.schema.json") }}
4747

48+
#### label_based_euclidean_cluster
49+
50+
| Name | Type | Description |
51+
| ----------------------------------- | ------ | --------------------------------------------------------------------------------------------- |
52+
| `use_height` | bool | use point.z for clustering |
53+
| `min_cluster_size` | int | minimum number of points required to keep a cluster |
54+
| `max_cluster_size` | int | maximum number of points allowed in a cluster |
55+
| `tolerance` | float | Euclidean clustering tolerance |
56+
| `min_probability` | float | minimum point probability to keep a point when the input has a `probability` field |
57+
| `class_names.<original_class_name>` | string | mapped label keyed by original class name; YAML declaration order is used as input `class_id` |
58+
| `use_shape_estimation_corrector` | bool | pass clusters through the standard shape estimation corrector |
59+
| `use_shape_estimation_filter` | bool | pass estimated boxes through the standard shape estimation filter |
60+
| `use_boost_bbox_optimizer` | bool | use the boost optimizer in the shape estimation backend |
61+
4862
## Assumptions / Known limits
4963

5064
<!-- Write assumptions and limitations of your implementation.
@@ -88,3 +102,9 @@ Example:
88102
## (Optional) Future extensions / Unimplemented parts
89103

90104
The `use_height` option of `voxel_grid_based_euclidean_cluster` isn't implemented yet.
105+
106+
`label_based_euclidean_cluster` reads `class_names.<original_class_name>` in YAML declaration order and uses that order as `class_id`. Classes mapped to `car`, `bus`, `truck`, `motorcycle`, `bicycle`, or `pedestrian` are kept, and classes mapped to `ignore` are skipped.
107+
108+
If the input pointcloud has no `probability` field, the node skips probability filtering and treats each point as probability `1.0`.
109+
110+
If the input pointcloud has no `class_id` field, the node clusters all points together as `UNKNOWN` and runs shape estimation with the `UNKNOWN` label.
Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
/**:
2+
ros__parameters:
3+
# Euclidean clustering
4+
use_height: false
5+
tolerance: 0.7
6+
min_cluster_size: 10
7+
max_cluster_size: 3000
8+
voxel_leaf_size: 0.2
9+
min_points_number_per_voxel: 1
10+
min_voxel_cluster_size_for_filtering: 10
11+
max_points_per_voxel_in_large_cluster: 100
12+
max_voxel_cluster_for_output: 1000
13+
14+
# Point filtering by semantic confidence
15+
min_probability: 0.3
16+
17+
# Semantic label selection and mapping
18+
# YAML declaration order is treated as the input class_id order.
19+
class_names:
20+
drivable_surface: ignore
21+
other_flat_surface: ignore
22+
sidewalk: ignore
23+
manmade: ignore
24+
vegetation: ignore
25+
car: car
26+
bus: bus
27+
emergency_vehicle: car
28+
train: unknown
29+
truck: truck
30+
tractor_unit: trailer
31+
semi_trailer: trailer
32+
construction_vehicle: truck
33+
forklift: car
34+
kart: car
35+
motorcycle: motorcycle
36+
bicycle: bicycle
37+
pedestrian: pedestrian
38+
personal_mobility: pedestrian
39+
animal: animal
40+
pushable_pullable: unknown
41+
traffic_cone: unknown
42+
debris: unknown
43+
stroller: pedestrian
44+
other_stuff: ignore
45+
noise+ghost_point: ignore
46+
47+
# Shape estimation behavior
48+
use_shape_estimation_corrector: false
49+
use_shape_estimation_filter: false
50+
use_boost_bbox_optimizer: false
51+
# 0: ALL_POLYGON, 1: LABEL_DEPEND
52+
# shape_policy: 0
Lines changed: 93 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
# Copyright 2026 TIER IV, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import launch
16+
from launch.actions import DeclareLaunchArgument
17+
from launch.actions import OpaqueFunction
18+
from launch.conditions import IfCondition
19+
from launch.conditions import UnlessCondition
20+
from launch.substitutions import LaunchConfiguration
21+
from launch_ros.actions import ComposableNodeContainer
22+
from launch_ros.actions import LoadComposableNodes
23+
from launch_ros.descriptions import ComposableNode
24+
from launch_ros.substitutions import FindPackageShare
25+
import yaml
26+
27+
28+
def launch_setup(context, *args, **kwargs):
29+
def load_composable_node_param(param_path):
30+
with open(LaunchConfiguration(param_path).perform(context), "r") as f:
31+
return yaml.safe_load(f)["/**"]["ros__parameters"]
32+
33+
ns = ""
34+
component = ComposableNode(
35+
package="autoware_euclidean_cluster",
36+
namespace=ns,
37+
plugin="autoware::euclidean_cluster::LabelBasedEuclideanClusterNode",
38+
name="label_based_euclidean_cluster",
39+
remappings=[
40+
("input", LaunchConfiguration("input_pointcloud")),
41+
("output", LaunchConfiguration("output_objects")),
42+
],
43+
parameters=[
44+
load_composable_node_param("param_path"),
45+
{"shape_policy": LaunchConfiguration("shape_policy")},
46+
],
47+
)
48+
49+
container = ComposableNodeContainer(
50+
name="label_based_euclidean_cluster_container",
51+
namespace=ns,
52+
package="rclcpp_components",
53+
executable="component_container",
54+
composable_node_descriptions=[],
55+
output="screen",
56+
condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
57+
)
58+
59+
target_container = (
60+
LaunchConfiguration("pointcloud_container_name")
61+
if IfCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
62+
else container
63+
)
64+
65+
loader = LoadComposableNodes(
66+
composable_node_descriptions=[component],
67+
target_container=target_container,
68+
)
69+
70+
return [container, loader]
71+
72+
73+
def generate_launch_description():
74+
def add_launch_arg(name: str, default_value=None):
75+
return DeclareLaunchArgument(name, default_value=default_value)
76+
77+
return launch.LaunchDescription(
78+
[
79+
add_launch_arg("input_pointcloud", "/perception/ptv3/segmented/pointcloud"),
80+
add_launch_arg("output_objects", "objects"),
81+
add_launch_arg("use_pointcloud_container", "false"),
82+
add_launch_arg("pointcloud_container_name", "pointcloud_container"),
83+
add_launch_arg("shape_policy"),
84+
add_launch_arg(
85+
"param_path",
86+
[
87+
FindPackageShare("autoware_euclidean_cluster"),
88+
"/config/label_based_euclidean_cluster.param.yaml",
89+
],
90+
),
91+
OpaqueFunction(function=launch_setup),
92+
]
93+
)
Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
<launch>
2+
<arg name="input_pointcloud" default="/perception/ptv3/segmented/pointcloud"/>
3+
<arg name="output_objects" default="objects"/>
4+
<arg name="use_pointcloud_container" default="false"/>
5+
<arg name="pointcloud_container_name" default="pointcloud_container"/>
6+
<arg name="param_path" default="$(find-pkg-share autoware_euclidean_cluster)/config/label_based_euclidean_cluster.param.yaml"/>
7+
<arg name="shape_policy" default="0" description="Shape policy for clustering 0: ALL_POLYGON, 1: LABEL_DEPEND"/>
8+
<include file="$(find-pkg-share autoware_euclidean_cluster)/launch/label_based_euclidean_cluster.launch.py">
9+
<arg name="input_pointcloud" value="$(var input_pointcloud)"/>
10+
<arg name="output_objects" value="$(var output_objects)"/>
11+
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
12+
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
13+
<arg name="param_path" value="$(var param_path)"/>
14+
<arg name="shape_policy" value="$(var shape_policy)"/>
15+
</include>
16+
</launch>

‎perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp‎

Lines changed: 93 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -56,9 +56,99 @@ bool VoxelGridBasedEuclideanCluster::cluster(
5656
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
5757
std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters)
5858
{
59-
(void)pointcloud;
60-
(void)clusters;
61-
return false;
59+
// 1) Voxel grid filtering
60+
pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_map_ptr(new pcl::PointCloud<pcl::PointXYZ>);
61+
constexpr float Z_AXIS_VOXEL_SIZE = 100000.0f;
62+
voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, Z_AXIS_VOXEL_SIZE);
63+
voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_);
64+
voxel_grid_.setInputCloud(pointcloud);
65+
voxel_grid_.setSaveLeafLayout(true);
66+
voxel_grid_.filter(*voxel_map_ptr);
67+
68+
// 2) Build 2D centroid cloud
69+
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_2d_ptr(new pcl::PointCloud<pcl::PointXYZ>);
70+
for (const auto & point : voxel_map_ptr->points) {
71+
pcl::PointXYZ point2d;
72+
point2d.x = point.x;
73+
point2d.y = point.y;
74+
point2d.z = 0.0; // Set z to 0.0 for 2D clustering
75+
pointcloud_2d_ptr->push_back(point2d);
76+
}
77+
78+
// 3) KD-tree + clustering
79+
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
80+
tree->setInputCloud(pointcloud_2d_ptr);
81+
std::vector<pcl::PointIndices> cluster_indices;
82+
pcl::EuclideanClusterExtraction<pcl::PointXYZ> pcl_euclidean_cluster;
83+
pcl_euclidean_cluster.setClusterTolerance(tolerance_);
84+
pcl_euclidean_cluster.setMinClusterSize(1);
85+
pcl_euclidean_cluster.setMaxClusterSize(max_cluster_size_);
86+
pcl_euclidean_cluster.setSearchMethod(tree);
87+
pcl_euclidean_cluster.setInputCloud(pointcloud_2d_ptr);
88+
pcl_euclidean_cluster.extract(cluster_indices);
89+
90+
// 4) Map voxel index to cluster index
91+
std::unordered_map<int, int> voxel_to_cluster_map;
92+
for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) {
93+
const auto & cluster = cluster_indices.at(cluster_idx);
94+
for (const auto & point_idx : cluster.indices) {
95+
voxel_to_cluster_map[point_idx] = cluster_idx;
96+
}
97+
}
98+
99+
// Precompute which clusters are large enough based on the voxel threshold.
100+
std::vector<bool> is_large_cluster(cluster_indices.size(), false);
101+
std::vector<bool> is_extreme_large_cluster(cluster_indices.size(), false);
102+
for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) {
103+
const int cluster_size = static_cast<int>(cluster_indices[cluster_idx].indices.size());
104+
is_large_cluster[cluster_idx] = cluster_size > min_voxel_cluster_size_for_filtering_;
105+
is_extreme_large_cluster[cluster_idx] = cluster_size > max_voxel_cluster_for_output_;
106+
}
107+
108+
// 5) Prepare output clusters
109+
clusters.clear();
110+
clusters.resize(cluster_indices.size());
111+
// Track how many points each voxel has per cluster
112+
std::unordered_map<int, std::unordered_map<int, int>> point_counts_per_voxel_per_cluster;
113+
std::vector<size_t> random_indices(pointcloud->points.size());
114+
static std::default_random_engine rng(42);
115+
std::iota(random_indices.begin(), random_indices.end(), 0);
116+
std::shuffle(random_indices.begin(), random_indices.end(), rng);
117+
for (size_t i = 0; i < random_indices.size(); ++i) {
118+
const size_t random_index = random_indices[i];
119+
const auto & point = pointcloud->points.at(random_index);
120+
const Eigen::Vector3i grid_coord = voxel_grid_.getGridCoordinates(point.x, point.y, point.z);
121+
#pragma GCC diagnostic push
122+
#pragma GCC diagnostic ignored "-Warray-bounds"
123+
const int voxel_index = voxel_grid_.getCentroidIndexAt(grid_coord);
124+
#pragma GCC diagnostic pop
125+
auto voxel_to_cluster_map_it = voxel_to_cluster_map.find(voxel_index);
126+
if (voxel_to_cluster_map_it != voxel_to_cluster_map.end()) {
127+
int cluster_idx = voxel_to_cluster_map_it->second;
128+
if (is_extreme_large_cluster[cluster_idx]) {
129+
continue;
130+
}
131+
if (is_large_cluster[cluster_idx]) {
132+
int & voxel_point_count = point_counts_per_voxel_per_cluster[cluster_idx][voxel_index];
133+
if (voxel_point_count >= max_points_per_voxel_in_large_cluster_) {
134+
continue;
135+
}
136+
voxel_point_count++;
137+
}
138+
clusters.at(cluster_idx).push_back(point);
139+
}
140+
}
141+
142+
// Remove clusters that are too small
143+
clusters.erase(
144+
std::remove_if(
145+
clusters.begin(), clusters.end(),
146+
[this](const pcl::PointCloud<pcl::PointXYZ> & cluster) {
147+
return static_cast<int>(cluster.size()) < min_cluster_size_;
148+
}),
149+
clusters.end());
150+
151+
return true;
62152
}
63153

64154
bool VoxelGridBasedEuclideanCluster::cluster(

‎perception/autoware_euclidean_cluster/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
<depend>autoware_agnocast_wrapper</depend>
1717
<depend>autoware_perception_msgs</depend>
1818
<depend>autoware_point_types</depend>
19+
<depend>autoware_shape_estimation</depend>
1920
<depend>autoware_utils</depend>
2021
<depend>geometry_msgs</depend>
2122
<depend>libpcl-all-dev</depend>

0 commit comments

Comments
 (0)