fix(euclidean_cluster, ground_segmentation): cherry-pick bug fix PRs (#…

…1378) * fix(euclidean_cluster): fix euclidean cluster params (autowarefoundation#7662) * fix(euclidean_cluster): fix max and min cluster size Signed-off-by: beginningfan <[email protected]> * fix(gnss_poser): fix a typo Signed-off-by: beginningfan <[email protected]> * fix(euclidean_cluster): fix min cluster size Signed-off-by: beginningfan <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: beginningfan <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(euclidean_cluster): fix max_cluster_size bug (autowarefoundation#7734) Signed-off-by: badai-nguyen <[email protected]> * fix(ground_segmentation): fix bug (autowarefoundation#7771) --------- Signed-off-by: beginningfan <[email protected]> Signed-off-by: badai-nguyen <[email protected]> Co-authored-by: beginningfan <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <[email protected]>
tier4 · Jul 4, 2024 · a28bbd2 · a28bbd2
1 parent 4cc01c3
commit a28bbd2
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 5 deletions.
diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml
@@ -19,6 +19,7 @@
   <exec_depend>detected_object_feature_remover</exec_depend>
   <exec_depend>detected_object_validation</exec_depend>
   <exec_depend>detection_by_tracker</exec_depend>
+  <exec_depend>elevation_map_loader</exec_depend>
   <exec_depend>euclidean_cluster</exec_depend>
   <exec_depend>ground_segmentation</exec_depend>
   <exec_depend>image_projection_based_fusion</exec_depend>

diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
@@ -106,7 +106,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
     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);
+    temporary_cluster.data.resize(cluster.indices.size() * point_step);
     clusters_data_size.push_back(0);
   }
 
@@ -117,13 +117,17 @@ bool VoxelGridBasedEuclideanCluster::cluster(
       voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z));
     if (map.find(index) != map.end()) {
       auto & cluster_data_size = clusters_data_size.at(map[index]);
-      if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) {
+      if (cluster_data_size > 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;
+      if (cluster_data_size == temporary_clusters.at(map[index]).data.size()) {
+        temporary_clusters.at(map[index])
+          .data.resize(temporary_clusters.at(map[index]).data.size() * 2);
+      }
     }
   }
 

diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp
@@ -292,12 +292,13 @@ void RANSACGroundFilterComponent::filter(
   no_ground_cloud_msg_ptr->header = input->header;
   no_ground_cloud_msg_ptr->fields = input->fields;
   no_ground_cloud_msg_ptr->point_step = point_step;
+  no_ground_cloud_msg_ptr->data.resize(input->data.size());
   size_t output_size = 0;
   // use not downsampled pointcloud for extract pointcloud that higher than height threshold
   for (size_t global_size = 0; global_size < input->data.size(); global_size += point_step) {
-    float x = *reinterpret_cast<float *>(input->data[global_size + x_offset]);
-    float y = *reinterpret_cast<float *>(input->data[global_size + y_offset]);
-    float z = *reinterpret_cast<float *>(input->data[global_size + z_offset]);
+    float x = *reinterpret_cast<const float *>(&input->data[global_size + x_offset]);
+    float y = *reinterpret_cast<const float *>(&input->data[global_size + y_offset]);
+    float z = *reinterpret_cast<const float *>(&input->data[global_size + z_offset]);
     const Eigen::Vector3d transformed_point = plane_affine.inverse() * Eigen::Vector3d(x, y, z);
     if (std::abs(transformed_point.z()) > height_threshold_) {
       std::memcpy(