perf(voxel_grid_downsample_filter): performance tuning

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp

@@ -66,10 +66,44 @@ class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filte
   void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
 
+  // TODO(atsushi421): Temporary Implementation: Remove this interface when all the filter nodes
+  // conform to new API
+  virtual void faster_filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
+    const TransformInfo & transform_info);
+
 private:
-  double voxel_size_x_;
-  double voxel_size_y_;
-  double voxel_size_z_;
+  struct Centroid
+  {
+    float x;
+    float y;
+    float z;
+    uint32_t point_count_;
+
+    Centroid() : x(0), y(0), z(0) {}
+    Centroid(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { this->point_count_ = 1; }
+
+    void add_point(float _x, float _y, float _z)
+    {
+      this->x += _x;
+      this->y += _y;
+      this->z += _z;
+      this->point_count_++;
+    }
+
+    void calc_centroid()
+    {
+      this->x /= this->point_count_;
+      this->y /= this->point_count_;
+      this->z /= this->point_count_;
+    }
+  };
+
+  float voxel_size_x_;
+  float voxel_size_y_;
+  float voxel_size_z_;
+  Eigen::Vector3f inverse_voxel_size_;
+  std::vector<pcl::PCLPointField> xyz_fields_;
 
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;

sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp

@@ -64,31 +64,144 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent(
 {
   // set initial parameters
   {
-    voxel_size_x_ = static_cast<double>(declare_parameter("voxel_size_x", 0.3));
-    voxel_size_y_ = static_cast<double>(declare_parameter("voxel_size_y", 0.3));
-    voxel_size_z_ = static_cast<double>(declare_parameter("voxel_size_z", 0.1));
+    voxel_size_x_ = static_cast<float>(declare_parameter("voxel_size_x", 0.3));
+    voxel_size_y_ = static_cast<float>(declare_parameter("voxel_size_y", 0.3));
+    voxel_size_z_ = static_cast<float>(declare_parameter("voxel_size_z", 0.1));
   }
 
+  inverse_voxel_size_[0] = 1.0f / voxel_size_x_;
+  inverse_voxel_size_[1] = 1.0f / voxel_size_y_;
+  inverse_voxel_size_[2] = 1.0f / voxel_size_z_;
+  pcl::for_each_type<typename pcl::traits::fieldList<pcl::PointXYZ>::type>(
+    pcl::detail::FieldAdder<pcl::PointXYZ>(xyz_fields_));
+
   using std::placeholders::_1;
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&VoxelGridDownsampleFilterComponent::paramCallback, this, _1));
 }
 
+// TODO(atsushi421): Temporary Implementation: Delete this function definition when all the filter
+// nodes conform to new API.
 void VoxelGridDownsampleFilterComponent::filter(
   const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output)
+{
+  (void)input;
+  (void)output;
+}
+
+// TODO(atsushi421): Temporary Implementation: Rename this function to `filter()` when all the
+// filter nodes conform to new API. Then delete the old `filter()` defined above.
+void VoxelGridDownsampleFilterComponent::faster_filter(
+  const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
+  PointCloud2 & output, const TransformInfo & transform_info)
 {
   std::scoped_lock lock(mutex_);
-  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
-  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
-  pcl::fromROSMsg(*input, *pcl_input);
-  pcl_output->points.reserve(pcl_input->points.size());
-  pcl::VoxelGrid<pcl::PointXYZ> filter;
-  filter.setInputCloud(pcl_input);
-  // filter.setSaveLeafLayout(true);
-  filter.setLeafSize(voxel_size_x_, voxel_size_y_, voxel_size_z_);
-  filter.filter(*pcl_output);
-
-  pcl::toROSMsg(*pcl_output, output);
+
+  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 intensity_offset = input->fields[pcl::getFieldIndex(*input, "intensity")].offset;
+
+  // Get the minimum and maximum dimensions
+  Eigen::Vector3f min_p, max_p;
+  min_p.setConstant(FLT_MAX);
+  max_p.setConstant(-FLT_MAX);
+  for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size();
+       global_offset += input->point_step) {
+    Eigen::Vector3f point(
+      *reinterpret_cast<const float *>(&input->data[global_offset + x_offset]),
+      *reinterpret_cast<const float *>(&input->data[global_offset + y_offset]),
+      *reinterpret_cast<const float *>(&input->data[global_offset + z_offset]));
+
+    if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) {
+      min_p = min_p.cwiseMin(point);
+      max_p = max_p.cwiseMax(point);
+    }
+  }
+
+  // Check that the voxel size is not too small, given the size of the data
+  if (
+    ((static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverse_voxel_size_[0]) + 1) *
+     (static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverse_voxel_size_[1]) + 1) *
+     (static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverse_voxel_size_[2]) + 1)) >
+    static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max())) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Voxel size is too small for the input dataset. "
+      "Integer indices would overflow.");
+    output = *input;
+    return;
+  }
+
+  // Compute the minimum and maximum bounding box values
+  Eigen::Vector3f min_b, max_b;
+  min_b[0] = static_cast<int>(std::floor(min_p[0] * inverse_voxel_size_[0]));
+  max_b[0] = static_cast<int>(std::floor(max_p[0] * inverse_voxel_size_[0]));
+  min_b[1] = static_cast<int>(std::floor(min_p[1] * inverse_voxel_size_[1]));
+  max_b[1] = static_cast<int>(std::floor(max_p[1] * inverse_voxel_size_[1]));
+  min_b[2] = static_cast<int>(std::floor(min_p[2] * inverse_voxel_size_[2]));
+  max_b[2] = static_cast<int>(std::floor(max_p[2] * inverse_voxel_size_[2]));
+
+  // Compute the number of divisions needed along all axis
+  Eigen::Vector3f div_b = max_b - min_b + Eigen::Vector3f::Ones();
+
+  // Set up the division multiplier
+  Eigen::Vector3i div_b_mul(1, div_b[0], div_b[0] * div_b[1]);
+
+  // Storage for mapping voxel coordinates to centroids
+  std::unordered_map<uint32_t, Centroid> voxel_centroid_map;
+  for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size();
+       global_offset += input->point_step) {
+    Eigen::Vector3f point(
+      *reinterpret_cast<const float *>(&input->data[global_offset + x_offset]),
+      *reinterpret_cast<const float *>(&input->data[global_offset + y_offset]),
+      *reinterpret_cast<const float *>(&input->data[global_offset + z_offset]));
+    if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) {
+      int ijk0 = static_cast<int>(
+        std::floor(point[0] * inverse_voxel_size_[0]) - static_cast<float>(min_b[0]));
+      int ijk1 = static_cast<int>(
+        std::floor(point[1] * inverse_voxel_size_[1]) - static_cast<float>(min_b[1]));
+      int ijk2 = static_cast<int>(
+        std::floor(point[2] * inverse_voxel_size_[2]) - static_cast<float>(min_b[2]));
+      uint32_t voxel_id = ijk0 * div_b_mul[0] + ijk1 * div_b_mul[1] + ijk2 * div_b_mul[2];
+
+      if (voxel_centroid_map.find(voxel_id) == voxel_centroid_map.end()) {
+        voxel_centroid_map[voxel_id] = Centroid(point[0], point[1], point[2]);
+      } else {
+        voxel_centroid_map[voxel_id].add_point(point[0], point[1], point[2]);
+      }
+    }
+  }
+
+  // Copy the centroids to the output
+  output.row_step = voxel_centroid_map.size() * input->point_step;
+  output.data.resize(output.row_step);
+  size_t output_data_size = 0;
+  for (auto & pair : voxel_centroid_map) {
+    pair.second.calc_centroid();
+    if (transform_info.need_transform) {
+      Eigen::Array4f point = transform_info.eigen_transform *
+                             Eigen::Vector4f(pair.second.x, pair.second.y, pair.second.z, 1);
+      *reinterpret_cast<float *>(&output.data[output_data_size + x_offset]) = point[0];
+      *reinterpret_cast<float *>(&output.data[output_data_size + y_offset]) = point[1];
+      *reinterpret_cast<float *>(&output.data[output_data_size + z_offset]) = point[2];
+    } else {
+      *reinterpret_cast<float *>(&output.data[output_data_size + x_offset]) = pair.second.x;
+      *reinterpret_cast<float *>(&output.data[output_data_size + y_offset]) = pair.second.y;
+      *reinterpret_cast<float *>(&output.data[output_data_size + z_offset]) = pair.second.z;
+    }
+
+    *reinterpret_cast<float *>(&output.data[output_data_size + intensity_offset]) = 1.0f;
+    output_data_size += input->point_step;
+  }
+
+  // Post processing
+  pcl_conversions::fromPCL(xyz_fields_, output.fields);
+  output.width = voxel_centroid_map.size();
+  output.is_dense = true;  // we filter out invalid points
+  output.height = input->height;
+  output.is_bigendian = input->is_bigendian;
+  output.point_step = input->point_step;
   output.header = input->header;
 }
 
@@ -107,6 +220,10 @@ rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::par
     RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_z_);
   }
 
+  inverse_voxel_size_[0] = 1.0f / voxel_size_x_;
+  inverse_voxel_size_[1] = 1.0f / voxel_size_y_;
+  inverse_voxel_size_[2] = 1.0f / voxel_size_z_;
+
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
   result.reason = "success";

sensing/pointcloud_preprocessor/src/filter.cpp

@@ -124,7 +124,8 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name)
   // TODO(sykwer): Change the corresponding node to subscribe to `faster_input_indices_callback`
   // each time a child class supports the faster version.
   // When all the child classes support the faster version, this workaround is deleted.
-  std::set<std::string> supported_nodes = {"CropBoxFilter", "RingOutlierFilter"};
+  std::set<std::string> supported_nodes = {
+    "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"};
   auto callback = supported_nodes.find(filter_name) != supported_nodes.end()
                     ? &Filter::faster_input_indices_callback
                     : &Filter::input_indices_callback;