[filters] VoxelGrid ignores PCLPointCloud2 indices

[Hugal31]

trafficstars

Describe the bug

The VoxelGrid filter doesn't use the provided indices when working with PCLPointCloud2.

Context

I am using VoxelGrid on a large PCLPointCloud2. The point cloud was cropped using multiple operations on indices. When running the VoxelGrid filter on the point cloud, it ignores the indices and

1. Most of the time emit a warning because the point cloud is too big.
2. Run on all the points even if they are not in the indices vector.

Expected behavior

The VoxelGrid filter should behave the same way with regular PointCloud and with PCLPointCloud2 and use the indices vector as all the other filters.

Current Behavior

The VoxelGrid<PCLPointCloud2> ignores the indices vector.

To Reproduce

Run the VoxelGrid on a PCLPointCloud2 and give it indices ignoring at least one point. Make the point far away from the other and have a low leaf size to trigger the warning.

Code snippets

#include <cassert>
#include <iostream>

#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

int main()
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud =
      pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
  cloud->push_back(pcl::PointXYZ{-20, -20, -5});
  cloud->push_back(pcl::PointXYZ{15, 15, 4});
  cloud->push_back(pcl::PointXYZ{20, 20, 5});

  // Indices, ignoring the first point.
  pcl::IndicesPtr indices = pcl::make_shared<pcl::Indices>();
  indices->push_back(1);
  indices->push_back(2);

  // PointXYZ workflow
  pcl::VoxelGrid<pcl::PointXYZ> voxelGrid;
  voxelGrid.setLeafSize(0.01, 0.01, 0.01);
  voxelGrid.setInputCloud(cloud);
  voxelGrid.setIndices(indices);
  pcl::PointCloud<pcl::PointXYZ> result;
  voxelGrid.filter(result);
  std::cout << "Filtered down to " << result.size() << " points with PointXYZ" << std::endl;
  assert(result.size() == 2);

  // PCLPointCloud2 workflow
  pcl::PCLPointCloud2::Ptr cloud2 = pcl::make_shared<pcl::PCLPointCloud2>();
  pcl::toPCLPointCloud2(*cloud, *cloud2);

  pcl::VoxelGrid<pcl::PCLPointCloud2> voxelGrid2;
  voxelGrid2.setLeafSize(0.01, 0.01, 0.01);
  voxelGrid2.setInputCloud(cloud2);
  voxelGrid2.setIndices(indices);
  pcl::PCLPointCloud2 result2;
  voxelGrid2.filter(result2);

  std::cout << "Filtered down to " << result2.width * result2.height << " points with PointCloud2" << std::endl;
  assert(result2.width * result2.height == 2); // Abort here
}

Your Environment (please complete the following information):

• OS: Ubuntu 20.04
• Compiler: GCC 9.4
• PCL Version 1.10. Should still be the case on HEAD.

Possible Solution

Use indices in getMinMax3D and when iterating over the point cloud.

[KStew8111]

I have started looking at this issue, and want to to try to tackle it. Where would be a good place to start?

[mvieth]

@KStew8111 That is great! The first step would be adding two new functions:

  PCL_EXPORTS void
  getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, const Indices &indices, int x_idx, int y_idx, int z_idx,
               Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
  PCL_EXPORTS void
  getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, const Indices &indices, int x_idx, int y_idx, int z_idx,
               const std::string &distance_field_name, float min_distance, float max_distance,
               Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);

Notice the new parameter indices. Instead of iterating over all points, they should iterate over indices. The next step is adapting pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter in voxel_grid.cpp (compare to pcl::VoxelGrid<PointT>::applyFilter in voxel_grid.hpp)

[mvieth]

Fixed by https://github.com/PointCloudLibrary/pcl/pull/5979