A small test that can efficiently Fixes #3072 Voxel grid filter "Leaf size is too small for the input dataset"

Hi,

since Voxel grid filter combine the X, Y, Z index to a single int type index...

int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
      int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
      int ijk2 = static_cast<int> (floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));

      // Compute the centroid leaf index
      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];

it is suffering from always report "Leaf size is too small for the input dataset"

why not use X, Y, Z index separately rather than combine them into a single index ?
and the only increasing cost is on sorting, only add more if-else...

Original sorting compare that is used by V oxel grid filter:

struct cloud_point_index_idx 
{
  unsigned int idx;
  unsigned int cloud_point_index;

  cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
  bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }
};

My modified sorting compare that use X, Y, Z index separately

struct CloudPointIndexIdx
	{
		std::size_t idx;
		std::size_t idy;
		std::size_t idz;
		std::size_t cloudPointIndex;

		CloudPointIndexIdx(std::size_t idx, std::size_t idy, std::size_t idz, std::size_t cloudPointIndex) 
			: idx(idx), idy(idy), idz(idz), cloudPointIndex(cloudPointIndex) {}
		bool operator < (const CloudPointIndexIdx& p) const 
		{ 
			if (idx < p.idx)
				return true;
			else if(idx > p.idx)
				return false;
			else
			{
				if (idy < p.idy)
					return true;
				else if (idy > p.idy)
					return false;
				else
				{
					if (idz < p.idz)
						return true;
					else 
						return false;
				}
			}
		}

		bool operator == (const CloudPointIndexIdx& p) const
		{
			return (idx == p.idx) && (idy == p.idy) && (idz == p.idz);
		}
	};

Here is a small test code that can efficiently fix the problem

Header:

#pragma once

#include <pcl/filters/boost.h>
#include <pcl/filters/filter.h>
#include <map>

#include "Common.h"

namespace pcl
{
	template <typename PointT>
	class VoxelGridFilter : public pcl::Filter<PointT>
	{
	protected:
		using pcl::Filter<PointT>::filter_name_;
		using pcl::Filter<PointT>::input_;
		using pcl::Filter<PointT>::indices_;

		using PointCloud = pcl::Filter<PointT>::PointCloud;

	public:

		VoxelGridFilter(const Eigen::Vector3d& leafSize, const Eigen::Vector3d& minAABB, const Eigen::Vector3d& maxAABB) :
			leafSize(leafSize),
			minAABB(minAABB),
			maxAABB(maxAABB),
			minPointsPerVoxel(0)
		{
			filter_name_ = "VoxelGridFilter";

			if (!(leafSize.x() > 0.0))
				THROW_EXCEPTION("!(leafSize.x() > 0.0)");
			if (!(leafSize.y() > 0.0))
				THROW_EXCEPTION("!(leafSize.y() > 0.0)");
			if (!(leafSize.z() > 0.0))
				THROW_EXCEPTION("!(leafSize.z() > 0.0)");
			invLeafSize = Eigen::Array3d::Ones() / leafSize.array();

			if (std::floor((maxAABB.x() - minAABB.x()) * invLeafSize[0]) > static_cast<double>(std::numeric_limits<std::size_t>::max()))
				THROW_EXCEPTION("X dimention overflow");
			if (std::floor((maxAABB.y() - minAABB.y()) * invLeafSize[1]) > static_cast<double>(std::numeric_limits<std::size_t>::max()))
				THROW_EXCEPTION("Y dimention overflow");
			if (std::floor((maxAABB.z() - minAABB.z()) * invLeafSize[2]) > static_cast<double>(std::numeric_limits<std::size_t>::max()))
				THROW_EXCEPTION("Z dimention overflow");
		}

	protected:
		Eigen::Vector3d leafSize;
		Eigen::Array3d invLeafSize;
		Eigen::Vector3d minAABB, maxAABB;

		/** \brief Minimum number of points per voxel for the centroid to be computed */
		std::size_t minPointsPerVoxel;

		void applyFilter(PointCloud& output);
	};
}

#include "VoxelGridFilter.hpp"

Implement:

#pragma once

#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/common/io.h>

#include "VoxelGridFilter.h"

namespace pcl
{
	struct CloudPointIndexIdx
	{
		std::size_t idx;
		std::size_t idy;
		std::size_t idz;
		std::size_t cloudPointIndex;

		CloudPointIndexIdx(std::size_t idx, std::size_t idy, std::size_t idz, std::size_t cloudPointIndex) 
			: idx(idx), idy(idy), idz(idz), cloudPointIndex(cloudPointIndex) {}
		bool operator < (const CloudPointIndexIdx& p) const 
		{ 
			if (idx < p.idx)
				return true;
			else if(idx > p.idx)
				return false;
			else
			{
				if (idy < p.idy)
					return true;
				else if (idy > p.idy)
					return false;
				else
				{
					if (idz < p.idz)
						return true;
					else 
						return false;
				}
			}
		}

		bool operator == (const CloudPointIndexIdx& p) const
		{
			return (idx == p.idx) && (idy == p.idy) && (idz == p.idz);
		}
	};

	template <typename PointT> 
	void VoxelGridFilter<PointT>::applyFilter(PointCloud& output)
	{
		std::vector<CloudPointIndexIdx> indices;
		indices.reserve(indices_->size());

		for (std::vector<int>::const_iterator it = indices_->begin(); it != indices_->end(); ++it)
		{
			if ((*it) >= 0)
			{
				const PointT& p = (*input_)[(*it)];

				if (pcl_isfinite(p.x) &&
					pcl_isfinite(p.y) &&
					pcl_isfinite(p.z))
				{
					if ((p.x > minAABB.x()) &&
						(p.y > minAABB.y()) &&
						(p.z > minAABB.z()) &&
						(p.x < maxAABB.x()) &&
						(p.y < maxAABB.y()) &&
						(p.z < maxAABB.z()))
					{
						std::size_t idx = static_cast<std::size_t> (std::floor((p.x - minAABB.x()) * invLeafSize[0]));
						std::size_t idy = static_cast<std::size_t> (std::floor((p.y - minAABB.y()) * invLeafSize[1]));
						std::size_t idz = static_cast<std::size_t> (std::floor((p.z - minAABB.z()) * invLeafSize[2]));
						indices.push_back(CloudPointIndexIdx(idx, idy, idz, *it));
					}
				}
			}
		}
		std::sort(indices.begin(), indices.end(), std::less<CloudPointIndexIdx>());

		std::size_t total = 0;
		std::size_t index = 0;
		std::vector<std::pair<std::size_t, std::size_t>> firstAndLastIndices;
		firstAndLastIndices.reserve(indices.size());
		while (index < indices.size())
		{
			std::size_t i = index + 1;
			while ((i < indices.size()) && (indices[i] == indices[index]))
				++i;
			if ((i - index) >= minPointsPerVoxel)
			{
				++total;
				firstAndLastIndices.push_back(std::pair<std::size_t, std::size_t>(index, i));
			}
			index = i;
		}
		output.resize(total);

		//
		index = 0;
		for (std::vector<std::pair<std::size_t, std::size_t>>::const_iterator it = firstAndLastIndices.begin(); it != firstAndLastIndices.end(); ++it)
		{
			pcl::CentroidPoint<PointT> centroid;
			for (std::size_t li = it->first; li < it->second; ++li)
				centroid.add(input_->points[indices[li].cloudPointIndex]);
			centroid.get(output.points[index]);
			
			++index;
		}
	}
}

Start with the VoxelGrid filter implementation and its applyFilter entry point, using the VoxelGridFilter.h and VoxelGridFilter.hpp code shown in the issue as context. Review how the current filter combines X, Y, and Z indices and reproduce the reported leaf-size error with the supplied test setup. Done should include coverage for large input dimensions and confirmation that voxel grouping remains correct; no repository test path is named.