fix(euclidean_cluster): add intensity field (#6818)

* fix(euclidean_cluster): add intensity field voxel_grid_based_euclidean_cluster

Signed-off-by: badai-nguyen <[email protected]>

* chore(euclidean_cluster): refactor

Signed-off-by: badai-nguyen <[email protected]>

* chore(euclidean_cluster): refactor

Signed-off-by: badai-nguyen <[email protected]>

* fix(euclidean_cluster): limit max_cluster_size

Signed-off-by: badai-nguyen <[email protected]>

* chore: add TODO

Signed-off-by: badai-nguyen <[email protected]>

* fix: typo

Signed-off-by: badai-nguyen <[email protected]>

---------

Signed-off-by: badai-nguyen <[email protected]>

perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp

@@ -32,6 +32,10 @@ class EuclideanCluster : public EuclideanClusterInterface
   bool cluster(
     const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
     std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) override;
+
+  bool cluster(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud,
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override;
   void setTolerance(float tolerance) { tolerance_ = tolerance; }
 
 private:

perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp

@@ -16,6 +16,9 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
@@ -41,6 +44,10 @@ class EuclideanClusterInterface
     const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
     std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) = 0;
 
+  virtual bool cluster(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_msg,
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature & output_clusters) = 0;
+
 protected:
   bool use_height_ = true;
   int min_cluster_size_;

perception/euclidean_cluster/include/euclidean_cluster/utils.hpp

@@ -31,6 +31,9 @@ void convertPointCloudClusters2Msg(
   const std_msgs::msg::Header & header,
   const std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters,
   tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg);
+void convertPointCloudClusters2Msg(
+  const std_msgs::msg::Header & header, const std::vector<sensor_msgs::msg::PointCloud2> & clusters,
+  tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg);
 void convertObjectMsg2SensorMsg(
   const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input,
   sensor_msgs::msg::PointCloud2 & output);

perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp

@@ -35,6 +35,9 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
   bool cluster(
     const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
     std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) override;
+  bool cluster(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud,
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature & 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)

perception/euclidean_cluster/lib/euclidean_cluster.cpp

@@ -33,6 +33,15 @@ EuclideanCluster::EuclideanCluster(
 : EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size), tolerance_(tolerance)
 {
 }
+// TODO(badai-nguyen): implement input field copy for euclidean_cluster.cpp
+bool EuclideanCluster::cluster(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg,
+  tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters)
+{
+  (void)pointcloud_msg;
+  (void)clusters;
+  return false;
+}
 
 bool EuclideanCluster::cluster(
   const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,

perception/euclidean_cluster/lib/utils.cpp

@@ -62,6 +62,25 @@ void convertPointCloudClusters2Msg(
     msg.feature_objects.push_back(feature_object);
   }
 }
+
+void convertPointCloudClusters2Msg(
+  const std_msgs::msg::Header & header, const std::vector<sensor_msgs::msg::PointCloud2> & clusters,
+  tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg)
+{
+  msg.header = header;
+  for (const auto & ros_pointcloud : clusters) {
+    tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
+    feature_object.feature.cluster = ros_pointcloud;
+    feature_object.object.kinematics.pose_with_covariance.pose.position =
+      getCentroid(ros_pointcloud);
+    autoware_auto_perception_msgs::msg::ObjectClassification classification;
+    classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+    classification.probability = 1.0f;
+    feature_object.object.classification.emplace_back(classification);
+    msg.feature_objects.push_back(feature_object);
+  }
+}
+
 void convertObjectMsg2SensorMsg(
   const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input,
   sensor_msgs::msg::PointCloud2 & output)

perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp

@@ -40,14 +40,27 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster(
   min_points_number_per_voxel_(min_points_number_per_voxel)
 {
 }
-
+// TODO(badai-nguyen): remove this function when field copying also implemented for
+// euclidean_cluster.cpp
 bool VoxelGridBasedEuclideanCluster::cluster(
   const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
   std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters)
+{
+  (void)pointcloud;
+  (void)clusters;
+  return false;
+}
+
+bool VoxelGridBasedEuclideanCluster::cluster(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg,
+  tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects)
 {
   // TODO(Saito) implement use_height is false version
 
   // create voxel
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
+  int point_step = pointcloud_msg->point_step;
+  pcl::fromROSMsg(*pointcloud_msg, *pointcloud);
   pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_map_ptr(new pcl::PointCloud<pcl::PointXYZ>);
   voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0);
   voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_);
@@ -81,36 +94,69 @@ bool VoxelGridBasedEuclideanCluster::cluster(
 
   // create map to search cluster index from voxel grid index
   std::unordered_map</* voxel grid index */ int, /* cluster index */ int> map;
+  std::vector<sensor_msgs::msg::PointCloud2> temporary_clusters;  // no check about cluster size
+  std::vector<size_t> clusters_data_size;
+  temporary_clusters.resize(cluster_indices.size());
   for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) {
     const auto & cluster = cluster_indices.at(cluster_idx);
+    auto & temporary_cluster = temporary_clusters.at(cluster_idx);
     for (const auto & point_idx : cluster.indices) {
       map[point_idx] = cluster_idx;
     }
+    temporary_cluster.height = pointcloud_msg->height;
+    temporary_cluster.fields = pointcloud_msg->fields;
+    temporary_cluster.point_step = point_step;
+    temporary_cluster.data.resize(max_cluster_size_ * point_step);
+    clusters_data_size.push_back(0);
   }
 
   // create vector of point cloud cluster. vector index is voxel grid index.
-  std::vector<pcl::PointCloud<pcl::PointXYZ>> temporary_clusters;  // no check about cluster size
-  temporary_clusters.resize(cluster_indices.size());
-  for (const auto & point : pointcloud->points) {
+  for (size_t i = 0; i < pointcloud->points.size(); ++i) {
+    const auto & point = pointcloud->points.at(i);
     const int index =
       voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z));
     if (map.find(index) != map.end()) {
-      temporary_clusters.at(map[index]).points.push_back(point);
+      auto & cluster_data_size = clusters_data_size.at(map[index]);
+      if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) {
+        continue;
+      }
+      std::memcpy(
+        &temporary_clusters.at(map[index]).data[cluster_data_size],
+        &pointcloud_msg->data[i * point_step], point_step);
+      cluster_data_size += point_step;
     }
   }
 
   // build output and check cluster size
   {
-    for (const auto & cluster : temporary_clusters) {
-      if (!(min_cluster_size_ <= static_cast<int>(cluster.points.size()) &&
-            static_cast<int>(cluster.points.size()) <= max_cluster_size_)) {
+    for (size_t i = 0; i < temporary_clusters.size(); ++i) {
+      auto & i_cluster_data_size = clusters_data_size.at(i);
+      if (!(min_cluster_size_ <= static_cast<int>(i_cluster_data_size / point_step) &&
+            static_cast<int>(i_cluster_data_size / point_step) <= max_cluster_size_)) {
         continue;
       }
-      clusters.push_back(cluster);
-      clusters.back().width = cluster.points.size();
-      clusters.back().height = 1;
-      clusters.back().is_dense = false;
+      const auto & cluster = temporary_clusters.at(i);
+      tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
+      feature_object.feature.cluster = cluster;
+      feature_object.feature.cluster.data.resize(i_cluster_data_size);
+      feature_object.feature.cluster.header = pointcloud_msg->header;
+      feature_object.feature.cluster.is_bigendian = pointcloud_msg->is_bigendian;
+      feature_object.feature.cluster.is_dense = pointcloud_msg->is_dense;
+      feature_object.feature.cluster.point_step = point_step;
+      feature_object.feature.cluster.row_step = i_cluster_data_size / pointcloud_msg->height;
+      feature_object.feature.cluster.width =
+        i_cluster_data_size / point_step / pointcloud_msg->height;
+
+      feature_object.object.kinematics.pose_with_covariance.pose.position =
+        getCentroid(feature_object.feature.cluster);
+      autoware_auto_perception_msgs::msg::ObjectClassification classification;
+      classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+      classification.probability = 1.0f;
+      feature_object.object.classification.emplace_back(classification);
+
+      objects.feature_objects.push_back(feature_object);
     }
+    objects.header = pointcloud_msg->header;
   }
 
   return true;

perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp

@@ -55,24 +55,15 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud(
   stop_watch_ptr_->toc("processing_time", true);
 
   // convert ros to pcl
-  pcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
   if (input_msg->data.empty()) {
     // NOTE: prevent pcl log spam
     RCLCPP_WARN_STREAM_THROTTLE(
       this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
-  } else {
-    pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);
   }
-
-  // clustering
-  std::vector<pcl::PointCloud<pcl::PointXYZ>> clusters;
-  if (!raw_pointcloud_ptr->empty()) {
-    cluster_->cluster(raw_pointcloud_ptr, clusters);
-  }
-
-  // build output msg
+  // cluster and build output msg
   tier4_perception_msgs::msg::DetectedObjectsWithFeature output;
-  convertPointCloudClusters2Msg(input_msg->header, clusters, output);
+
+  cluster_->cluster(input_msg, output);
   cluster_pub_->publish(output);
 
   // build debug msg