Apply https://github.com/PointCloudLibrary/pcl/pull/3853 from Davide Faconti to improve Voxelgrid runtime by 20% to 1.8.1 tag (backport)

0001-Apply-https-github.com-PointCloudLibrary-pcl-pull-38.patch

From 466e0f57b15e11ab19af900341f98a726ad1bf73 Mon Sep 17 00:00:00 2001
From: Sammy Pfeiffer <sam.pfeiffer@conigital.com>
Date: Wed, 8 Sep 2021 16:06:44 +1000
Subject: [PATCH] Apply https://github.com/PointCloudLibrary/pcl/pull/3853 from
 Davide Faconti to improve Voxelgrid runtime by 20% to 1.8.1 tag (backport)

---
 filters/include/pcl/filters/impl/voxel_grid.hpp | 7 +++++--
 filters/src/voxel_grid.cpp                      | 6 ++++--
 2 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp
index 9f2db7a..4982ae5 100644
--- a/filters/include/pcl/filters/impl/voxel_grid.hpp
+++ b/filters/include/pcl/filters/impl/voxel_grid.hpp
@@ -42,6 +42,7 @@
 #include <pcl/common/common.h>
 #include <pcl/common/io.h>
 #include <pcl/filters/voxel_grid.h>
+#include  <boost/sort/spreadsort/integer_sort.hpp>
 
 ///////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
@@ -205,6 +206,7 @@ struct cloud_point_index_idx
   unsigned int idx;
   unsigned int cloud_point_index;
 
+  cloud_point_index_idx() = default;
   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); }
 };
@@ -339,8 +341,9 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
 
   // Second pass: sort the index_vector vector using value representing target cell as index
   // in effect all points belonging to the same output cell will be next to each other
-  std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
-
+  auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
+  boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
+  
   // Third pass: count output cells
   // we need to skip all the same, adjacenent idx values
   unsigned int total = 0;
diff --git a/filters/src/voxel_grid.cpp b/filters/src/voxel_grid.cpp
index 62552a5..187ff3d 100644
--- a/filters/src/voxel_grid.cpp
+++ b/filters/src/voxel_grid.cpp
@@ -41,6 +41,7 @@
 #include <iostream>
 #include <pcl/common/io.h>
 #include <pcl/filters/impl/voxel_grid.hpp>
+#include <boost/sort/spreadsort/integer_sort.hpp>
 
 typedef Eigen::Array<size_t, 4, 1> Array4size_t;
 
@@ -377,8 +378,9 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 
   // Second pass: sort the index_vector vector using value representing target cell as index
   // in effect all points belonging to the same output cell will be next to each other
-  std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
-
+  auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
+  boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
+  
   // Third pass: count output cells
   // we need to skip all the same, adjacenent idx values
   size_t total = 0;
-- 
2.17.1