Merge normal and OMP classes as well cleanup redundant code

Author: shrijitsingh99

features/include/pcl/features/fpfh.h

@@ -93,9 +93,10 @@ namespace pcl
       using PointCloudOut = typename Feature<PointInT, PointOutT>::PointCloudOut;
 
       /** \brief Empty constructor. */
-      FPFHEstimation () : 
+      FPFHEstimation (unsigned int nr_threads = -1) :
         nr_bins_f1_ (11), nr_bins_f2_ (11), nr_bins_f3_ (11), 
-        d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI)))
+        d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI))),
+        threads_(nr_threads)
       {
         feature_name_ = "FPFHEstimation";
       };
@@ -177,6 +178,21 @@ namespace pcl
         nr_bins_f3 = nr_bins_f3_;
       }
 
+    /** \brief Initialize the scheduler and set the number of threads to use.
+      * \param[in] nr_threads the number of hardware threads to use (0 sets the value back to automatic)
+      */
+    void
+    setNumberOfThreads (unsigned int nr_threads = 0) {
+      if (nr_threads == 0)
+        #ifdef _OPENMP
+        threads_ = omp_get_num_procs();
+        #else
+        threads_ = 1;
+        #endif
+      else
+        threads_ = nr_threads;
+    }
+
     protected:
 
       /** \brief Estimate the set of all SPFH (Simple Point Feature Histograms) signatures for the input cloud
@@ -213,8 +229,15 @@ namespace pcl
       Eigen::VectorXf fpfh_histogram_;
 
       /** \brief Float constant = 1.0 / (2.0 * M_PI) */
-      float d_pi_; 
+      float d_pi_;
+
+      /** \brief The number of threads the scheduler should use. */
+      unsigned int threads_;
   };
+
+template <typename PointInT, typename PointNT, typename PointOutT>
+using FPFHEstimationOMP PCL_DEPRECATED(1, 12, "use FPFHEstimation instead") = FPFHEstimation<PointInT, PointNT, PointOutT>;
+
 }
 
 #ifdef PCL_NO_PRECOMPILE

features/include/pcl/features/fpfh_omp.h

@@ -40,91 +40,8 @@
 
 #pragma once
 
-#include <pcl/features/feature.h>
-#include <pcl/features/fpfh.h>
-
-namespace pcl
-{
-  /** \brief FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud
-    * dataset containing points and normals, in parallel, using the OpenMP standard.
-    *
-    * \note If you use this code in any academic work, please cite:
-    *
-    *   - R.B. Rusu, N. Blodow, M. Beetz.
-    *     Fast Point Feature Histograms (FPFH) for 3D Registration.
-    *     In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
-    *     Kobe, Japan, May 12-17 2009.
-    *   - R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
-    *     Fast Geometric Point Labeling using Conditional Random Fields.
-    *     In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
-    *     St. Louis, MO, USA, October 11-15 2009.
-    *
-    * \attention
-    * The convention for FPFH features is:
-    *   - if a query point's nearest neighbors cannot be estimated, the FPFH feature will be set to NaN
-    *     (not a number)
-    *   - it is impossible to estimate a FPFH descriptor for a point that
-    *     doesn't have finite 3D coordinates. Therefore, any point that contains
-    *     NaN data on x, y, or z, will have its FPFH feature property set to NaN.
-    *
-    * \author Radu B. Rusu
-    * \ingroup features
-    */
-  template <typename PointInT, typename PointNT, typename PointOutT>
-  class FPFHEstimationOMP : public FPFHEstimation<PointInT, PointNT, PointOutT>
-  {
-    public:
-      using Ptr = shared_ptr<FPFHEstimationOMP<PointInT, PointNT, PointOutT> >;
-      using ConstPtr = shared_ptr<const FPFHEstimationOMP<PointInT, PointNT, PointOutT> >;
-      using Feature<PointInT, PointOutT>::feature_name_;
-      using Feature<PointInT, PointOutT>::getClassName;
-      using Feature<PointInT, PointOutT>::indices_;
-      using Feature<PointInT, PointOutT>::k_;
-      using Feature<PointInT, PointOutT>::search_parameter_;
-      using Feature<PointInT, PointOutT>::input_;
-      using Feature<PointInT, PointOutT>::surface_;
-      using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
-      using FPFHEstimation<PointInT, PointNT, PointOutT>::hist_f1_;
-      using FPFHEstimation<PointInT, PointNT, PointOutT>::hist_f2_;
-      using FPFHEstimation<PointInT, PointNT, PointOutT>::hist_f3_;
-      using FPFHEstimation<PointInT, PointNT, PointOutT>::weightPointSPFHSignature;
-
-      using PointCloudOut = typename Feature<PointInT, PointOutT>::PointCloudOut;
-
-      /** \brief Initialize the scheduler and set the number of threads to use.
-        * \param[in] nr_threads the number of hardware threads to use (0 sets the value back to automatic)
-        */
-      FPFHEstimationOMP (unsigned int nr_threads = 0) : nr_bins_f1_ (11), nr_bins_f2_ (11), nr_bins_f3_ (11)
-      {
-        feature_name_ = "FPFHEstimationOMP";
+PCL_DEPRECATED_HEADER(1, 13, "Use <pcl/features/fpfh.h> instead.")
 
-        setNumberOfThreads(nr_threads);
-      }
-
-      /** \brief Initialize the scheduler and set the number of threads to use.
-        * \param[in] nr_threads the number of hardware threads to use (0 sets the value back to automatic)
-        */
-      void
-      setNumberOfThreads (unsigned int nr_threads = 0);
-
-    private:
-      /** \brief Estimate the Fast Point Feature Histograms (FPFH) descriptors at a set of points given by
-        * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in
-        * setSearchMethod ()
-        * \param[out] output the resultant point cloud model dataset that contains the FPFH feature estimates
-        */
-      void
-      computeFeature (PointCloudOut &output) override;
+#include <pcl/features/fpfh.h>
 
-    public:
-      /** \brief The number of subdivisions for each angular feature interval. */
-      int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_;
-    private:
-      /** \brief The number of threads the scheduler should use. */
-      unsigned int threads_;
-  };
-}
 
-#ifdef PCL_NO_PRECOMPILE
-#include <pcl/features/impl/fpfh_omp.hpp>
-#endif

features/include/pcl/features/impl/fpfh.hpp

@@ -186,7 +186,8 @@ pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures (std::v
   std::vector<int> nn_indices (k_);
   std::vector<float> nn_dists (k_);
 
-  std::set<int> spfh_indices;
+  std::set<int> spfh_indices_set;
+  std::vector<int> spfh_indices_vec;
   spfh_hist_lookup.resize (surface_->size ());
 
   // Build a list of (unique) indices for which we will need to compute SPFH signatures
@@ -199,26 +200,37 @@ pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures (std::v
       if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
         continue;
 
-      spfh_indices.insert (nn_indices.begin (), nn_indices.end ());
+      spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
     }
+    spfh_indices_vec.resize (spfh_indices_set.size ());
+    std::copy (spfh_indices_set.cbegin (), spfh_indices_set.cend (), spfh_indices_vec.begin ());
   }
   else
   {
     // Special case: When a feature must be computed at every point, there is no need for a neighborhood search
-    for (std::size_t idx = 0; idx < indices_->size (); ++idx)
-      spfh_indices.insert (static_cast<int> (idx));
+    spfh_indices_vec.resize (indices_->size ());
+    std::iota(spfh_indices_vec.begin (), spfh_indices_vec.end (),
+              static_cast<decltype(spfh_indices_vec)::value_type>(0));
   }
 
   // Initialize the arrays that will store the SPFH signatures
-  std::size_t data_size = spfh_indices.size ();
+  const auto data_size = spfh_indices_vec.size ();
   hist_f1.setZero (data_size, nr_bins_f1_);
   hist_f2.setZero (data_size, nr_bins_f2_);
   hist_f3.setZero (data_size, nr_bins_f3_);
 
   // Compute SPFH signatures for every point that needs them
-  std::size_t i = 0;
-  for (const auto& p_idx: spfh_indices)
+#pragma omp parallel for \
+  default(none) \
+  shared(spfh_hist_lookup, spfh_indices_vec, hist_f1, hist_f2, hist_f3) \
+  firstprivate(nn_indices, nn_dists) \
+  num_threads(threads_) \
+  if (threads_ != -1)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t> (spfh_indices_vec.size ()); ++i)
   {
+    // Get the next point index
+    int p_idx = spfh_indices_vec[i];
+
     // Find the neighborhood around p_idx
     if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
       continue;
@@ -228,7 +240,6 @@ pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures (std::v
 
     // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
     spfh_hist_lookup[p_idx] = i;
-    i++;
   }
 }
 
@@ -245,59 +256,32 @@ pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   computeSPFHSignatures (spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_);
 
   output.is_dense = true;
-  // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
-  if (input_->is_dense)
-  {
-    // Iterate over the entire index vector
-    for (std::size_t idx = 0; idx < indices_->size (); ++idx)
-    {
-      if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
-      {
-        for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d)
-          output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
-
-        output.is_dense = false;
-        continue;
-      }
-
-      // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
-      // instead of indices into surface_->points
-      for (auto &nn_index : nn_indices)
-        nn_index = spfh_hist_lookup[nn_index];
-
-      // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
-      weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
 
-      // ...and copy it into the output cloud
-      std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output[idx].histogram);
-    }
-  }
-  else
+  // Iterate over the entire index vector
+  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
   {
-    // Iterate over the entire index vector
-    for (std::size_t idx = 0; idx < indices_->size (); ++idx)
-    {
-      if (!isFinite ((*input_)[(*indices_)[idx]]) ||
-          this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
-      {
-        for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d)
-          output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
-
-        output.is_dense = false;
+    if (input_->is_dense || isFinite ((*input_)[(*indices_)[idx]])) {
+      if (this->searchForNeighbors(
+              (*indices_)[idx], search_parameter_, nn_indices, nn_dists)) {
+        // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
+        // instead of indices into surface_->points
+        for (auto &nn_index : nn_indices)
+          nn_index = spfh_hist_lookup[nn_index];
+
+        // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
+        weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
+
+        // ...and copy it into the output cloud
+        std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output[idx].histogram);
         continue;
       }
+    }
 
-      // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
-      // instead of indices into surface_->points
-      for (auto &nn_index : nn_indices)
-        nn_index = spfh_hist_lookup[nn_index];
-
-      // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
-      weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
+    for (Eigen::Index d = 0; d < fpfh_histogram_.size(); ++d)
+      output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN();
 
-      // ...and copy it into the output cloud
-      std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output[idx].histogram);
-    }
+    if (output.is_dense)
+      output.is_dense = false;
   }
 }