Revert "fix(ground_segmentation): add intensity field (autowarefoundation#6791)"

This reverts commit 729eec0.

Author: 1222-takeshi

perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp

@@ -186,16 +186,15 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
    * @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed
    */
   void ExtractPointsIndices(
-    const PointCloud2::ConstSharedPtr in_cloud_ptr, pcl::PointIndices & in_indices,
-    PointCloud2::SharedPtr out_only_indices_cloud_ptr,
-    PointCloud2::SharedPtr out_removed_indices_cloud_ptr);
+    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);
 
   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,36 +284,19 @@ 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 (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);
+  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);
     if (std::abs(transformed_point.z()) > height_threshold_) {
-      std::memcpy(
-        &no_ground_cloud_msg_ptr->data[output_size], &input->data[global_size], point_step);
-      output_size += point_step;
+      no_ground_cloud_ptr->points.push_back(p);
     }
   }
-  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,53 +257,20 @@ void RayGroundFilterComponent::ClassifyPointCloud(
 //   return (true);
 // }
 
-void RayGroundFilterComponent::initializePointCloud2(
-  const PointCloud2::ConstSharedPtr & in_cloud_ptr, PointCloud2::SharedPtr & out_cloud_msg_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());
-}
-
 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)
+  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)
 {
-  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;
+  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);
+
+  extract_ground.setNegative(true);  // true removes the indices, false leaves only the indices
+  extract_ground.filter(*out_removed_indices_cloud_ptr);
 }
 
 void RayGroundFilterComponent::filter(
@@ -332,11 +299,14 @@ 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);
-  sensor_msgs::msg::PointCloud2::SharedPtr 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;
 
-  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,6 +575,8 @@ 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;
   }
 }
@@ -604,7 +606,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 = input->fields;
+  output.fields.assign(input->fields.begin(), input->fields.begin() + 3);
   output.is_dense = true;
   output.height = input->height;
   output.is_bigendian = input->is_bigendian;