Merge branch 'main' into refactor-to-fix-dangerous-data-access-in-autoware-downsample-filters-started-on-26nd-June-2026

Author: sasakisasaki

sensing/autoware_downsample_filters/src/voxel_grid_downsample_filter/faster_voxel_grid_downsample_filter.cpp

@@ -49,7 +49,7 @@ void FasterVoxelGridDownsampleFilter::set_voxel_size(
 }
 
 ValidationResult FasterVoxelGridDownsampleFilter::filter(
-  const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info)
+  const PointCloud2ConstPtr & input, PointCloud2 & output)
 {
   const int x_index = find_field_index(*input, "x");
   const int y_index = find_field_index(*input, "y");
@@ -67,7 +67,7 @@ ValidationResult FasterVoxelGridDownsampleFilter::filter(
     return {false, "The intensity field in the input point cloud is not of type UINT8."};
   }
 
-  // Compute the minimum and maximum voxel coordinates
+  // Compute the voxel-space bounds of all valid points.
   Eigen::Vector3i min_voxel, max_voxel;
   if (!get_min_max_voxel(input, min_voxel, max_voxel)) {
     output = *input;
@@ -77,10 +77,10 @@ ValidationResult FasterVoxelGridDownsampleFilter::filter(
     };
   }
 
-  // Storage for mapping voxel coordinates to centroids
+  // Accumulate one centroid per voxel.
   auto voxel_centroid_map = calc_centroids_each_voxel(input, max_voxel, min_voxel);
 
-  // Initialize the output
+  // Prepare output metadata and storage.
   output.row_step = voxel_centroid_map.size() * input->point_step;
   output.data.resize(output.row_step);
   output.width = voxel_centroid_map.size();
@@ -91,15 +91,15 @@ ValidationResult FasterVoxelGridDownsampleFilter::filter(
   output.point_step = input->point_step;
   output.header = input->header;
 
-  // Copy the centroids to the output
-  copy_centroids_to_output(voxel_centroid_map, output, transform_info);
+  // Serialize centroids into the output PointCloud2 buffer.
+  copy_centroids_to_output(voxel_centroid_map, output);
   return {true, ""};
 }
 
 bool FasterVoxelGridDownsampleFilter::get_min_max_voxel(
   const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel)
 {
-  // Compute the minimum and maximum point coordinates
+  // Scan all valid points and track XYZ min/max values.
   Eigen::Vector3f min_point, max_point;
   min_point.setConstant(FLT_MAX);
   max_point.setConstant(-FLT_MAX);
@@ -116,7 +116,7 @@ bool FasterVoxelGridDownsampleFilter::get_min_max_voxel(
     }
   }
 
-  // Check that the voxel size is not too small, given the size of the data
+  // Guard against integer overflow when flattening 3D voxel indices into a 1D id.
   if (
     ((static_cast<std::int64_t>((max_point[0] - min_point[0]) * inverse_voxel_size_[0]) + 1) *
      (static_cast<std::int64_t>((max_point[1] - min_point[1]) * inverse_voxel_size_[1]) + 1) *
@@ -125,7 +125,7 @@ bool FasterVoxelGridDownsampleFilter::get_min_max_voxel(
     return false;
   }
 
-  // Compute the minimum and maximum voxel coordinates
+  // Convert point-space bounds to voxel-space bounds.
   min_voxel[0] = static_cast<int>(std::floor(min_point[0] * inverse_voxel_size_[0]));
   min_voxel[1] = static_cast<int>(std::floor(min_point[1] * inverse_voxel_size_[1]));
   min_voxel[2] = static_cast<int>(std::floor(min_point[2] * inverse_voxel_size_[2]));
@@ -142,9 +142,10 @@ FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel(
   const Eigen::Vector3i & min_voxel)
 {
   std::unordered_map<uint32_t, Centroid> voxel_centroid_map;
-  // Compute the number of divisions needed along all axis
+  // Number of voxel bins along each axis within the bounding box.
   Eigen::Vector3i div_b = max_voxel - min_voxel + Eigen::Vector3i::Ones();
-  // Set up the division multiplier
+  // Strides for flattening voxel coordinates (i, j, k) into one key:
+  // id = i * 1 + j * div_b[0] + k * (div_b[0] * div_b[1]).
   Eigen::Vector3i div_b_mul(1, div_b[0], div_b[0] * div_b[1]);
 
   sensor_msgs::PointCloud2ConstIterator<float> input_x(*input, "x");
@@ -155,7 +156,7 @@ FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel(
     const Eigen::Vector4f point(
       *input_x, *input_y, *input_z, static_cast<float>(*input_intensity));
     if (std::isfinite(point[0]) && std::isfinite(point[1]) && std::isfinite(point[2])) {
-      // Calculate the voxel index to which the point belongs
+      // Compute voxel coordinates relative to the minimum voxel corner.
       int ijk0 = static_cast<int>(
         std::floor(point[0] * inverse_voxel_size_[0]) - static_cast<float>(min_voxel[0]));
       int ijk1 = static_cast<int>(
@@ -164,7 +165,7 @@ FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel(
         std::floor(point[2] * inverse_voxel_size_[2]) - static_cast<float>(min_voxel[2]));
       uint32_t voxel_id = ijk0 * div_b_mul[0] + ijk1 * div_b_mul[1] + ijk2 * div_b_mul[2];
 
-      // Add the point to the corresponding centroid
+      // Start or update the centroid accumulator for this voxel.
       if (voxel_centroid_map.find(voxel_id) == voxel_centroid_map.end()) {
         voxel_centroid_map[voxel_id] = Centroid(point[0], point[1], point[2], point[3]);
       } else {
@@ -177,8 +178,7 @@ FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel(
 }
 
 void FasterVoxelGridDownsampleFilter::copy_centroids_to_output(
-  const std::unordered_map<uint32_t, Centroid> & voxel_centroid_map, PointCloud2 & output,
-  const TransformInfo & transform_info) const
+  const std::unordered_map<uint32_t, Centroid> & voxel_centroid_map, PointCloud2 & output) const
 {
   sensor_msgs::PointCloud2Iterator<float> output_x(output, "x");
   sensor_msgs::PointCloud2Iterator<float> output_y(output, "y");

sensing/autoware_downsample_filters/src/voxel_grid_downsample_filter/faster_voxel_grid_downsample_filter.hpp

@@ -15,8 +15,6 @@
 #ifndef VOXEL_GRID_DOWNSAMPLE_FILTER__FASTER_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_
 #define VOXEL_GRID_DOWNSAMPLE_FILTER__FASTER_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_
 
-#include "transform_info.hpp"
-
 #include <pcl/filters/voxel_grid.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <sensor_msgs/msg/point_cloud2.h>
@@ -42,8 +40,7 @@ class FasterVoxelGridDownsampleFilter
 public:
   FasterVoxelGridDownsampleFilter();
   void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z);
-  ValidationResult filter(
-    const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info);
+  ValidationResult filter(const PointCloud2ConstPtr & input, PointCloud2 & output);
 
 private:
   struct Centroid
@@ -89,8 +86,7 @@ class FasterVoxelGridDownsampleFilter
     const Eigen::Vector3i & min_voxel);
 
   void copy_centroids_to_output(
-    const std::unordered_map<uint32_t, Centroid> & voxel_centroid_map, PointCloud2 & output,
-    const TransformInfo & transform_info) const;
+    const std::unordered_map<uint32_t, Centroid> & voxel_centroid_map, PointCloud2 & output) const;
 };
 
 }  // namespace autoware::downsample_filters

sensing/autoware_downsample_filters/src/voxel_grid_downsample_filter/transform_info.hpp

@@ -15,7 +15,6 @@
 #include "voxel_grid_downsample_filter.hpp"
 
 #include "memory.hpp"
-#include "transform_info.hpp"
 
 #include <sstream>
 #include <string>
@@ -99,7 +98,7 @@ tl::expected<PointCloud2, std::string> VoxelGridDownsampleFilter::filter(
   PointCloud2 output;
   faster_voxel_filter_.set_voxel_size(
     parameters_.voxel_size_x, parameters_.voxel_size_y, parameters_.voxel_size_z);
-  const auto filter_result = faster_voxel_filter_.filter(input, output, TransformInfo{});
+  const auto filter_result = faster_voxel_filter_.filter(input, output);
 
   if (!filter_result.is_valid) {
     return tl::unexpected(filter_result.reason);