From c1b3040bb0d1be919a7a2fb2f802a02732b09786 Mon Sep 17 00:00:00 2001 From: "daf@blue-ocean-robotics.com" Date: Fri, 3 Apr 2020 12:34:25 +0200 Subject: [PATCH 1/8] Optimize VoxelGrid VoxelGrid spends a lot of time sorting. Radix sort implemted in boost seems to be faster than std::sort --- filters/include/pcl/filters/impl/voxel_grid.hpp | 14 +++++++++++--- filters/src/voxel_grid.cpp | 6 +++++- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp index 6ba0bc6c9d8..048b1652d39 100644 --- a/filters/include/pcl/filters/impl/voxel_grid.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid.hpp @@ -42,6 +42,7 @@ #include #include #include +#include /////////////////////////////////////////////////////////////////////////////////////////// template void @@ -202,13 +203,18 @@ pcl::getMinMax3D (const typename pcl::PointCloud::ConstPtr &cloud, struct cloud_point_index_idx { - unsigned int idx; + 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_) {} + cloud_point_index_idx(){} + cloud_point_index_idx (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); } }; +struct cloud_point_index_rightshift { + inline int operator()(const cloud_point_index_idx &p, const unsigned offset) { return p.idx >> offset; } +}; + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::VoxelGrid::applyFilter (PointCloud &output) @@ -339,7 +345,9 @@ pcl::VoxelGrid::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 ()); + //std::sort (index_vector.begin (), index_vector.end (), std::less ()); + boost::sort::spreadsort::integer_sort(index_vector.begin (), index_vector.end (), + cloud_point_index_rightshift (), std::less ()); // Third pass: count output cells // we need to skip all the same, adjacent idx values diff --git a/filters/src/voxel_grid.cpp b/filters/src/voxel_grid.cpp index 68e44dba0ed..d6f4fe2c6b5 100644 --- a/filters/src/voxel_grid.cpp +++ b/filters/src/voxel_grid.cpp @@ -41,6 +41,7 @@ #include #include #include +#include using Array4size_t = Eigen::Array; @@ -377,7 +378,10 @@ pcl::VoxelGrid::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 ()); + //std::sort (index_vector.begin (), index_vector.end (), std::less ()); + + boost::sort::spreadsort::integer_sort(index_vector.begin (), index_vector.end (), + cloud_point_index_rightshift (), std::less ()); // Third pass: count output cells // we need to skip all the same, adjacenent idx values From c0dcad0c09f0a171cebbad035b3700e131fca4c0 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 Apr 2020 16:56:03 +0200 Subject: [PATCH 2/8] change reverted (should have no effect on results) --- filters/include/pcl/filters/impl/voxel_grid.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp index 048b1652d39..f585a1822d9 100644 --- a/filters/include/pcl/filters/impl/voxel_grid.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid.hpp @@ -203,7 +203,7 @@ pcl::getMinMax3D (const typename pcl::PointCloud::ConstPtr &cloud, struct cloud_point_index_idx { - int idx; + unsigned int idx; unsigned int cloud_point_index; cloud_point_index_idx(){} From 6f99de8707b5589fc5e2b33d3f5cb8b16cbb0c3b Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 Apr 2020 21:21:57 +0200 Subject: [PATCH 3/8] Applying suggested changes --- filters/include/pcl/filters/impl/voxel_grid.hpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp index f585a1822d9..81978d25262 100644 --- a/filters/include/pcl/filters/impl/voxel_grid.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid.hpp @@ -206,8 +206,8 @@ struct cloud_point_index_idx unsigned int idx; unsigned int cloud_point_index; - cloud_point_index_idx(){} - cloud_point_index_idx (int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (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); } }; @@ -345,10 +345,9 @@ pcl::VoxelGrid::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 ()); - boost::sort::spreadsort::integer_sort(index_vector.begin (), index_vector.end (), - cloud_point_index_rightshift (), std::less ()); - + 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); + // Third pass: count output cells // we need to skip all the same, adjacent idx values unsigned int total = 0; From 8bd2e15467565c3a3ad5ce193ef3de0ffe925838 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 Apr 2020 21:22:41 +0200 Subject: [PATCH 4/8] applying suggested changes --- filters/src/voxel_grid.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/filters/src/voxel_grid.cpp b/filters/src/voxel_grid.cpp index d6f4fe2c6b5..cbbe7a6d665 100644 --- a/filters/src/voxel_grid.cpp +++ b/filters/src/voxel_grid.cpp @@ -379,9 +379,8 @@ pcl::VoxelGrid::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 ()); - - boost::sort::spreadsort::integer_sort(index_vector.begin (), index_vector.end (), - cloud_point_index_rightshift (), std::less ()); + 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); // Third pass: count output cells // we need to skip all the same, adjacenent idx values From d47d1747c572c80bcb62a44858dbd9d3bd519573 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 Apr 2020 21:47:45 +0200 Subject: [PATCH 5/8] cleaunp --- filters/include/pcl/filters/impl/voxel_grid.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp index 81978d25262..b1783fe9efc 100644 --- a/filters/include/pcl/filters/impl/voxel_grid.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid.hpp @@ -211,10 +211,6 @@ struct cloud_point_index_idx bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); } }; -struct cloud_point_index_rightshift { - inline int operator()(const cloud_point_index_idx &p, const unsigned offset) { return p.idx >> offset; } -}; - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::VoxelGrid::applyFilter (PointCloud &output) From 17e9cc435678ec1a95f3247efe944dbcfb6f18f1 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 Apr 2020 21:48:26 +0200 Subject: [PATCH 6/8] Update voxel_grid.hpp --- filters/include/pcl/filters/impl/voxel_grid.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp index b1783fe9efc..669b5c6fe64 100644 --- a/filters/include/pcl/filters/impl/voxel_grid.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid.hpp @@ -342,7 +342,7 @@ pcl::VoxelGrid::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 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); + 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, adjacent idx values From 802536bd9c8f813622d978071969f240992e0015 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 Apr 2020 21:48:46 +0200 Subject: [PATCH 7/8] Update voxel_grid.cpp --- filters/src/voxel_grid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/filters/src/voxel_grid.cpp b/filters/src/voxel_grid.cpp index cbbe7a6d665..11be01efd9a 100644 --- a/filters/src/voxel_grid.cpp +++ b/filters/src/voxel_grid.cpp @@ -380,7 +380,7 @@ pcl::VoxelGrid::applyFilter (PCLPointCloud2 &output) // 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 ()); 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); + 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 From 270b6f3eb3ad3bfe1d90c5f6d06b06b2417d2dcd Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Mon, 27 Apr 2020 16:49:56 +0900 Subject: [PATCH 8/8] Remove commented out code --- filters/src/voxel_grid.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/filters/src/voxel_grid.cpp b/filters/src/voxel_grid.cpp index 11be01efd9a..92e5f71dd25 100644 --- a/filters/src/voxel_grid.cpp +++ b/filters/src/voxel_grid.cpp @@ -378,7 +378,6 @@ pcl::VoxelGrid::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 ()); 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); @@ -552,4 +551,3 @@ PCL_INSTANTIATE(getMinMax3D, PCL_XYZ_POINT_TYPES) PCL_INSTANTIATE(VoxelGrid, PCL_XYZ_POINT_TYPES) #endif // PCL_NO_PRECOMPILE -