From cdaf0dfbfacbc31d0792d3888d3619872d655eed Mon Sep 17 00:00:00 2001
From: Markus Vieth <mvieth@techfak.uni-bielefeld.de>
Date: Wed, 30 Oct 2024 09:45:32 +0100
Subject: [PATCH] Faster organized radius search By checking the finite-ness of
 all points once, before doing any searches. This avoids calling isFinite
 repeatedly on the same points in radiusSearch. Benchmark: NormalEstimation
 with OrganizedNeighbor and radius search now takes 89 percent of the time it
 took before (so 11 percent faster).

---
 search/include/pcl/search/impl/organized.hpp |  2 +-
 search/include/pcl/search/organized.h        | 15 +++++++++++----
 2 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/search/include/pcl/search/impl/organized.hpp b/search/include/pcl/search/impl/organized.hpp
index 12e6da32962..df089620fc3 100644
--- a/search/include/pcl/search/impl/organized.hpp
+++ b/search/include/pcl/search/impl/organized.hpp
@@ -82,7 +82,7 @@ pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const               PointT
   {
     for (; idx < xEnd; ++idx)
     {
-      if (!mask_[idx] || !isFinite ((*input_)[idx]))
+      if (!mask_[idx])
         continue;
 
       float dist_x = (*input_)[idx].x - query.x;
diff --git a/search/include/pcl/search/organized.h b/search/include/pcl/search/organized.h
index ea6fea672c8..20f23841bbb 100644
--- a/search/include/pcl/search/organized.h
+++ b/search/include/pcl/search/organized.h
@@ -42,6 +42,7 @@
 #include <pcl/memory.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_cloud.h>
+#include <pcl/common/point_tests.h> // for pcl::isFinite
 #include <pcl/search/search.h>
 #include <pcl/common/eigen.h>
 
@@ -138,10 +139,16 @@ namespace pcl
           {
             mask_.assign (input_->size (), 0);
             for (const auto& idx : *indices_)
-              mask_[idx] = 1;
+              if (pcl::isFinite((*input_)[idx]))
+                mask_[idx] = 1;
           }
           else
-            mask_.assign (input_->size (), 1);
+          {
+            mask_.assign (input_->size (), 0);
+            for (std::size_t idx=0; idx<input_->size(); ++idx)
+              if (pcl::isFinite((*input_)[idx]))
+                mask_[idx] = 1;
+          }
 
           return estimateProjectionMatrix () && isValid ();
         }
@@ -216,7 +223,7 @@ namespace pcl
         testPoint (const PointT& query, unsigned k, std::vector<Entry>& queue, index_t index) const
         {
           const PointT& point = input_->points [index];
-          if (mask_ [index] && std::isfinite (point.x))
+          if (mask_ [index])
           {
             //float squared_distance = (point.getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
             float dist_x = point.x - query.x;
@@ -278,7 +285,7 @@ namespace pcl
         /** \brief using only a subsample of points to calculate the projection matrix. pyramid_level_ = use down sampled cloud given by pyramid_level_*/
         const unsigned pyramid_level_;
         
-        /** \brief mask, indicating whether the point was in the indices list or not.*/
+        /** \brief mask, indicating whether the point was in the indices list or not, and whether it is finite.*/
         std::vector<unsigned char> mask_;
       public:
         PCL_MAKE_ALIGNED_OPERATOR_NEW