fix(ground_segmentation): add intensity field (#6791)

* fix(grond_segmentation): add intensity

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

* fix(ray_ground_filter): change to pointXYZI

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

* fix(ransac_ground_filter): change to pointXYZI

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

* fix(scan_ground_filter): intensity bug fix

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

* fix(ground_segmentation): add field select option

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

* Revert "fix(ground_segmentation): add field select option"

This reverts commit 36fc554.

* fix: copy input field to output

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

* fix(ray_ground_filter): copy input fields

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

* fix(ransac_ground_filter): copy input fields

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

* fix(ray_ground_filter): indices extract bug fix

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

* refactor: ray_ground_filter

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

* fix: indices max

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

* style(pre-commit): autofix

---------

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>

Author: badai-nguyen

perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp

@@ -186,15 +186,16 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
    * @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed
    */
   void ExtractPointsIndices(
-    const pcl::PointCloud<PointType_>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
-    pcl::PointCloud<PointType_>::Ptr out_only_indices_cloud_ptr,
-    pcl::PointCloud<PointType_>::Ptr out_removed_indices_cloud_ptr);
+    const PointCloud2::ConstSharedPtr in_cloud_ptr, pcl::PointIndices & in_indices,
+    PointCloud2::SharedPtr out_only_indices_cloud_ptr,
+    PointCloud2::SharedPtr out_removed_indices_cloud_ptr);
 
   boost::optional<float> calcPointVehicleIntersection(const Point & point);
 
   void setVehicleFootprint(
     const double min_x, const double max_x, const double min_y, const double max_y);
-
+  void initializePointCloud2(
+    const PointCloud2::ConstSharedPtr & in_cloud_ptr, PointCloud2::SharedPtr & out_cloud_msg_ptr);
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
 

perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp

@@ -284,19 +284,36 @@ void RANSACGroundFilterComponent::filter(
   const Eigen::Affine3d plane_affine = getPlaneAffine(*segment_ground_cloud_ptr, plane_normal);
   pcl::PointCloud<PointType>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<PointType>);
 
+  int x_offset = input->fields[pcl::getFieldIndex(*input, "x")].offset;
+  int y_offset = input->fields[pcl::getFieldIndex(*input, "y")].offset;
+  int z_offset = input->fields[pcl::getFieldIndex(*input, "z")].offset;
+  int point_step = input->point_step;
+
+  sensor_msgs::msg::PointCloud2::SharedPtr no_ground_cloud_msg_ptr(
+    new sensor_msgs::msg::PointCloud2);
+  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;
+  size_t output_size = 0;
   // use not downsampled pointcloud for extract pointcloud that higher than height threshold
-  for (const auto & p : current_sensor_cloud_ptr->points) {
-    const Eigen::Vector3d transformed_point =
-      plane_affine.inverse() * Eigen::Vector3d(p.x, p.y, p.z);
+  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]);
+    const Eigen::Vector3d transformed_point = plane_affine.inverse() * Eigen::Vector3d(x, y, z);
     if (std::abs(transformed_point.z()) > height_threshold_) {
-      no_ground_cloud_ptr->points.push_back(p);
+      std::memcpy(
+        &no_ground_cloud_msg_ptr->data[output_size], &input->data[global_size], point_step);
+      output_size += point_step;
     }
   }
+  no_ground_cloud_msg_ptr->data.resize(output_size);
+  no_ground_cloud_msg_ptr->height = input->height;
+  no_ground_cloud_msg_ptr->width = output_size / point_step / input->height;
+  no_ground_cloud_msg_ptr->row_step = output_size / input->height;
+  no_ground_cloud_msg_ptr->is_dense = input->is_dense;
+  no_ground_cloud_msg_ptr->is_bigendian = input->is_bigendian;
 
-  sensor_msgs::msg::PointCloud2::SharedPtr no_ground_cloud_msg_ptr(
-    new sensor_msgs::msg::PointCloud2);
-  pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr);
-  no_ground_cloud_msg_ptr->header = input->header;
   sensor_msgs::msg::PointCloud2::SharedPtr no_ground_cloud_transformed_msg_ptr(
     new sensor_msgs::msg::PointCloud2);
   if (!transformPointCloud(

perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp

@@ -257,20 +257,53 @@ void RayGroundFilterComponent::ClassifyPointCloud(
 //   return (true);
 // }
 
-void RayGroundFilterComponent::ExtractPointsIndices(
-  const pcl::PointCloud<PointType_>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
-  pcl::PointCloud<PointType_>::Ptr out_only_indices_cloud_ptr,
-  pcl::PointCloud<PointType_>::Ptr out_removed_indices_cloud_ptr)
+void RayGroundFilterComponent::initializePointCloud2(
+  const PointCloud2::ConstSharedPtr & in_cloud_ptr, PointCloud2::SharedPtr & out_cloud_msg_ptr)
 {
-  pcl::ExtractIndices<PointType_> extract_ground;
-  extract_ground.setInputCloud(in_cloud_ptr);
-  extract_ground.setIndices(pcl::make_shared<pcl::PointIndices>(in_indices));
-
-  extract_ground.setNegative(false);  // true removes the indices, false leaves only the indices
-  extract_ground.filter(*out_only_indices_cloud_ptr);
+  out_cloud_msg_ptr->header = in_cloud_ptr->header;
+  out_cloud_msg_ptr->height = in_cloud_ptr->height;
+  out_cloud_msg_ptr->fields = in_cloud_ptr->fields;
+  out_cloud_msg_ptr->is_bigendian = in_cloud_ptr->is_bigendian;
+  out_cloud_msg_ptr->point_step = in_cloud_ptr->point_step;
+  out_cloud_msg_ptr->is_dense = in_cloud_ptr->is_dense;
+  out_cloud_msg_ptr->data.resize(in_cloud_ptr->data.size());
+}
 
-  extract_ground.setNegative(true);  // true removes the indices, false leaves only the indices
-  extract_ground.filter(*out_removed_indices_cloud_ptr);
+void RayGroundFilterComponent::ExtractPointsIndices(
+  const PointCloud2::ConstSharedPtr in_cloud_ptr, pcl::PointIndices & in_indices,
+  PointCloud2::SharedPtr ground_cloud_msg_ptr, PointCloud2::SharedPtr no_ground_cloud_msg_ptr)
+{
+  initializePointCloud2(in_cloud_ptr, ground_cloud_msg_ptr);
+  initializePointCloud2(in_cloud_ptr, no_ground_cloud_msg_ptr);
+  int point_step = in_cloud_ptr->point_step;
+  size_t ground_count = 0;
+  size_t no_ground_count = 0;
+  std::vector<bool> is_ground_idx(in_cloud_ptr->width, false);
+  for (const auto & idx : in_indices.indices) {
+    if (std::size_t(idx) >= is_ground_idx.size()) {
+      continue;
+    }
+    is_ground_idx[idx] = true;
+  }
+  for (size_t i = 0; i < is_ground_idx.size(); ++i) {
+    if (is_ground_idx[i]) {
+      std::memcpy(
+        &ground_cloud_msg_ptr->data[ground_count * point_step], &in_cloud_ptr->data[i * point_step],
+        point_step);
+      ground_count++;
+    } else {
+      std::memcpy(
+        &no_ground_cloud_msg_ptr->data[no_ground_count * point_step],
+        &in_cloud_ptr->data[i * point_step], point_step);
+      no_ground_count++;
+    }
+  }
+  ground_cloud_msg_ptr->data.resize(ground_count * point_step);
+  no_ground_cloud_msg_ptr->data.resize(no_ground_count * point_step);
+  ground_cloud_msg_ptr->width = ground_count;
+  no_ground_cloud_msg_ptr->width = no_ground_count;
+  ground_cloud_msg_ptr->row_step = ground_count * point_step;
+  no_ground_cloud_msg_ptr->row_step = no_ground_count * point_step;
 }
 
 void RayGroundFilterComponent::filter(
@@ -299,14 +332,11 @@ void RayGroundFilterComponent::filter(
   pcl::PointCloud<PointType_>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<PointType_>);
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr radials_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
 
-  ExtractPointsIndices(
-    current_sensor_cloud_ptr, ground_indices, ground_cloud_ptr, no_ground_cloud_ptr);
-
   sensor_msgs::msg::PointCloud2::SharedPtr no_ground_cloud_msg_ptr(
     new sensor_msgs::msg::PointCloud2);
-  pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr);
-  no_ground_cloud_msg_ptr->header = input->header;
+  sensor_msgs::msg::PointCloud2::SharedPtr ground_cloud_msg_ptr(new sensor_msgs::msg::PointCloud2);
 
+  ExtractPointsIndices(input, ground_indices, ground_cloud_msg_ptr, no_ground_cloud_msg_ptr);
   output = *no_ground_cloud_msg_ptr;
 }
 

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -575,8 +575,6 @@ void ScanGroundFilterComponent::extractObjectPoints(
     std::memcpy(
       &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step],
       in_cloud_ptr->point_step * sizeof(uint8_t));
-    *reinterpret_cast<float *>(&out_object_cloud.data[output_data_size + intensity_offset_]) =
-      1;  // set intensity to 1
     output_data_size += in_cloud_ptr->point_step;
   }
 }
@@ -606,7 +604,7 @@ void ScanGroundFilterComponent::faster_filter(
   output.row_step = no_ground_indices.indices.size() * input->point_step;
   output.data.resize(output.row_step);
   output.width = no_ground_indices.indices.size();
-  output.fields.assign(input->fields.begin(), input->fields.begin() + 3);
+  output.fields = input->fields;
   output.is_dense = true;
   output.height = input->height;
   output.is_bigendian = input->is_bigendian;