feat: add euclidean cluster

perception/euclidean_cluster/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 3.5)
+project(euclidean_cluster)
+
+if(NOT CMAKE_CXX_STANDARD)
+	set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(PCL REQUIRED QUIET COMPONENTS common search filters segmentation)
+ament_auto_find_build_dependencies()
+
+
+include_directories(
+  include
+  ${PCL_COMMON_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
+
+ament_auto_add_library(cluster_lib SHARED
+  lib/utils.cpp
+  lib/euclidean_cluster.cpp
+  lib/voxel_grid_based_euclidean_cluster.cpp
+)
+
+target_link_libraries(cluster_lib
+  ${PCL_LIBRARIES}
+)
+
+target_include_directories(cluster_lib
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_auto_add_library(euclidean_cluster_node_core SHARED
+  src/euclidean_cluster_node.cpp
+)
+target_link_libraries(euclidean_cluster_node_core
+  ${PCL_LIBRARIES}
+  cluster_lib
+)
+
+rclcpp_components_register_node(euclidean_cluster_node_core
+  PLUGIN "euclidean_cluster::EuclideanClusterNode"
+  EXECUTABLE euclidean_cluster_node
+)
+
+ament_auto_add_library(voxel_grid_based_euclidean_cluster_node_core SHARED
+  src/voxel_grid_based_euclidean_cluster_node.cpp
+)
+target_link_libraries(voxel_grid_based_euclidean_cluster_node_core
+  ${PCL_LIBRARIES}
+  cluster_lib
+)
+
+rclcpp_components_register_node(voxel_grid_based_euclidean_cluster_node_core
+  PLUGIN "euclidean_cluster::VoxelGridBasedEuclideanClusterNode"
+  EXECUTABLE voxel_grid_based_euclidean_cluster_node
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+    launch
+    config
+)

perception/euclidean_cluster/README.md

@@ -0,0 +1,102 @@
+# euclidean_cluster
+
+## Purpose
+
+euclidean_cluster is a package for clustering points into smaller parts to classify objects.
+
+This package has two clustering methods: `euclidean_cluster` and `voxel_grid_based_euclidean_cluster`.
+
+## Inner-workings / Algorithms
+
+### euclidean_cluster
+
+`pcl::EuclideanClusterExtraction` is applied to points. See [official document](https://pcl.readthedocs.io/en/latest/cluster_extraction.html) for details.
+
+### voxel_grid_based_euclidean_cluster
+
+1. A centroid in each voxel is calculated by `pcl::VoxelGrid`.
+2. The centroids are clustered by `pcl::EuclideanClusterExtraction`.
+3. The input points are clustered based on the clustered centroids.
+
+## Inputs / Outputs
+
+### Input
+
+| Name    | Type                            | Description      |
+| ------- | ------------------------------- | ---------------- |
+| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud |
+
+### Output
+
+| Name             | Type                                                        | Description                                  |
+| ---------------- | ----------------------------------------------------------- | -------------------------------------------- |
+| `output`         | `autoware_perception_msgs::msg::DetectedObjectsWithFeature` | cluster pointcloud                           |
+| `debug/clusters` | `sensor_msgs::msg::PointCloud2`                             | colored cluster pointcloud for visualization |
+
+## Parameters
+
+### Core Parameters
+
+#### euclidean_cluster
+
+| Name               | Type  | Description                                                                                  |
+| ------------------ | ----- | -------------------------------------------------------------------------------------------- |
+| `use_height`       | bool  | use point.z for clustering                                                                   |
+| `min_cluster_size` | int   | the minimum number of points that a cluster needs to contain in order to be considered valid |
+| `max_cluster_size` | int   | the maximum number of points that a cluster needs to contain in order to be considered valid |
+| `tolerance`        | float | the spatial cluster tolerance as a measure in the L2 Euclidean space                         |
+
+#### voxel_grid_based_euclidean_cluster
+
+| Name                          | Type  | Description                                                                                  |
+| ----------------------------- | ----- | -------------------------------------------------------------------------------------------- |
+| `use_height`                  | bool  | use point.z for clustering                                                                   |
+| `min_cluster_size`            | int   | the minimum number of points that a cluster needs to contain in order to be considered valid |
+| `max_cluster_size`            | int   | the maximum number of points that a cluster needs to contain in order to be considered valid |
+| `tolerance`                   | float | the spatial cluster tolerance as a measure in the L2 Euclidean space                         |
+| `voxel_leaf_size`             | float | the voxel leaf size of x and y                                                               |
+| `min_points_number_per_voxel` | int   | the minimum number of points for a voxel                                                     |
+
+## Assumptions / Known limits
+
+<!-- Write assumptions and limitations of your implementation.
+
+Example:
+  This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
+  Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
+-->
+
+## (Optional) Error detection and handling
+
+<!-- Write how to detect errors and how to recover from them.
+
+Example:
+  This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
+-->
+
+## (Optional) Performance characterization
+
+<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
+
+Example:
+  ### Complexity
+
+  This algorithm is O(N).
+
+  ### Processing time
+
+  ...
+-->
+
+## (Optional) References/External links
+
+<!-- Write links you referred to when you implemented.
+
+Example:
+  [1] {link_to_a_thesis}
+  [2] {link_to_an_issue}
+-->
+
+## (Optional) Future extensions / Unimplemented parts
+
+The `use_height` option of `voxel_grid_based_euclidean_cluster` isn't implemented yet.

perception/euclidean_cluster/config/compare_map.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    distance_threshold: 0.5

perception/euclidean_cluster/config/euclidean_cluster.param.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    max_cluster_size: 1000
+    min_cluster_size: 10
+    tolerance: 0.7
+    use_height: false

perception/euclidean_cluster/config/outlier.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    input_frame: base_link
+    output_frame: base_link
+    voxel_size_x: 0.3
+    voxel_size_y: 0.3
+    voxel_size_z: 100.0
+    voxel_points_threshold: 3

perception/euclidean_cluster/config/voxel_grid.param.yaml

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    input_frame: base_link
+    output_frame: base_link
+    voxel_size_x: 0.15
+    voxel_size_y: 0.15
+    voxel_size_z: 0.15

perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    tolerance: 0.7
+    voxel_leaf_size: 0.3
+    min_points_number_per_voxel: 1
+    min_cluster_size: 10
+    max_cluster_size: 3000
+    use_height: false

perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp

@@ -0,0 +1,41 @@
+// Copyright 2020 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.
+
+#pragma once
+
+#include "euclidean_cluster/euclidean_cluster_interface.hpp"
+#include "euclidean_cluster/utils.hpp"
+
+#include <pcl/point_types.h>
+
+#include <vector>
+
+namespace euclidean_cluster
+{
+class EuclideanCluster : public EuclideanClusterInterface
+{
+public:
+  EuclideanCluster();
+  EuclideanCluster(bool use_height, int min_cluster_size, int max_cluster_size);
+  EuclideanCluster(bool use_height, int min_cluster_size, int max_cluster_size, float tolerance);
+  bool cluster(
+    const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) override;
+  void setTolerance(float tolerance) { tolerance_ = tolerance; }
+
+private:
+  float tolerance_;
+};
+
+}  // namespace euclidean_cluster

perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp

@@ -0,0 +1,50 @@
+// Copyright 2021 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.
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+
+namespace euclidean_cluster
+{
+class EuclideanClusterInterface
+{
+public:
+  EuclideanClusterInterface() = default;
+  EuclideanClusterInterface(bool use_height, int min_cluster_size, int max_cluster_size)
+  : use_height_(use_height),
+    min_cluster_size_(min_cluster_size),
+    max_cluster_size_(max_cluster_size)
+  {
+  }
+  virtual ~EuclideanClusterInterface() = default;
+  void setUseHeight(bool use_height) { use_height_ = use_height; }
+  void setMinClusterSize(int size) { min_cluster_size_ = size; }
+  void setMaxClusterSize(int size) { max_cluster_size_ = size; }
+  virtual bool cluster(
+    const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) = 0;
+
+protected:
+  bool use_height_ = true;
+  int min_cluster_size_;
+  int max_cluster_size_;
+};
+
+}  // namespace euclidean_cluster

perception/euclidean_cluster/include/euclidean_cluster/utils.hpp

@@ -0,0 +1,37 @@
+// Copyright 2021 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.
+
+#pragma once
+
+#include <autoware_perception_msgs/msg/detected_objects_with_feature.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <vector>
+
+namespace euclidean_cluster
+{
+geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud);
+void convertPointCloudClusters2Msg(
+  const std_msgs::msg::Header & header,
+  const std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters,
+  autoware_perception_msgs::msg::DetectedObjectsWithFeature & msg);
+void convertObjectMsg2SensorMsg(
+  const autoware_perception_msgs::msg::DetectedObjectsWithFeature & input,
+  sensor_msgs::msg::PointCloud2 & output);
+}  // namespace euclidean_cluster

perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp

@@ -0,0 +1,52 @@
+// Copyright 2021 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.
+
+#pragma once
+
+#include "euclidean_cluster/euclidean_cluster_interface.hpp"
+#include "euclidean_cluster/utils.hpp"
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+
+namespace euclidean_cluster
+{
+class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
+{
+public:
+  VoxelGridBasedEuclideanCluster();
+  VoxelGridBasedEuclideanCluster(bool use_height, int min_cluster_size, int max_cluster_size);
+  VoxelGridBasedEuclideanCluster(
+    bool use_height, int min_cluster_size, int max_cluster_size, float tolerance,
+    float voxel_leaf_size, int min_points_number_per_voxel);
+  bool cluster(
+    const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) override;
+  void setVoxelLeafSize(float voxel_leaf_size) { voxel_leaf_size_ = voxel_leaf_size; }
+  void setTolerance(float tolerance) { tolerance_ = tolerance; }
+  void setMinPointsNumberPerVoxel(int min_points_number_per_voxel)
+  {
+    min_points_number_per_voxel_ = min_points_number_per_voxel;
+  }
+
+private:
+  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
+  float tolerance_;
+  float voxel_leaf_size_;
+  int min_points_number_per_voxel_;
+};
+
+}  // namespace euclidean_cluster