registration: parallelize SAC-IA error metric with OpenMP (#6431)

* registration: parallelize SAC-IA error metric with OpenMP

* registration: add OpenMP thread control to SAC-IA

Author: zeel2104

registration/include/pcl/registration/ia_ransac.h

@@ -150,6 +150,7 @@ class SampleConsensusInitialAlignment : public Registration<PointSource, PointTa
     corr_dist_threshold_ = 100.0f;
     transformation_estimation_.reset(
         new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
+    setNumberOfThreads();
   };
 
   /** \brief Provide a shared pointer to the source point cloud's feature descriptors
@@ -230,6 +231,13 @@ class SampleConsensusInitialAlignment : public Registration<PointSource, PointTa
     return (k_correspondences_);
   }
 
+  /** \brief Initialize the scheduler and set the number of threads to use.
+   * \param nr_threads the number of hardware threads to use (0 sets the value back to
+   * automatic)
+   */
+  void
+  setNumberOfThreads(unsigned int nr_threads = 0);
+
   /** \brief Specify the error function to minimize
    * \note This call is optional.  TruncatedError will be used by default
    * \param[in] error_functor a shared pointer to a subclass of
@@ -316,6 +324,9 @@ class SampleConsensusInitialAlignment : public Registration<PointSource, PointTa
    * correspondence. */
   int k_correspondences_{10};
 
+  /** \brief The number of threads the scheduler should use. */
+  unsigned int threads_{1};
+
   /** \brief The KdTree used to compare feature descriptors. */
   FeatureKdTreePtr feature_tree_;
 

registration/include/pcl/registration/impl/ia_ransac.hpp

@@ -46,6 +46,28 @@
 
 namespace pcl {
 
+template <typename PointSource, typename PointTarget, typename FeatureT>
+void
+SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setNumberOfThreads(
+    unsigned int nr_threads)
+{
+#ifdef _OPENMP
+  if (nr_threads == 0)
+    threads_ = omp_get_num_procs();
+  else
+    threads_ = nr_threads;
+  PCL_DEBUG("[pcl::%s::setNumberOfThreads] Setting number of threads to %u.\n",
+            getClassName().c_str(),
+            threads_);
+#else
+  threads_ = 1;
+  if (nr_threads != 1)
+    PCL_WARN("[pcl::%s::setNumberOfThreads] Parallelization is requested, but OpenMP "
+             "is not available! Continuing without parallelization.\n",
+             getClassName().c_str());
+#endif // _OPENMP
+}
+
 template <typename PointSource, typename PointTarget, typename FeatureT>
 void
 SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setSourceFeatures(
@@ -172,12 +194,16 @@ SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErro
   std::vector<float> nn_distance(1);
 
   const ErrorFunctor& compute_error = *error_functor_;
+  const auto& tree = tree_;
   float error = 0;
 
-  for (const auto& point : cloud) {
+#pragma omp parallel for default(none) shared(cloud, tree, compute_error)              \
+    firstprivate(nn_index, nn_distance) reduction(+ : error) num_threads(threads_)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(cloud.size()); ++i) {
+    const auto& point = cloud[static_cast<std::size_t>(i)];
     // Find the distance between point and its nearest neighbor in the target point
     // cloud
-    tree_->nearestKSearch(point, 1, nn_index, nn_distance);
+    tree->nearestKSearch(point, 1, nn_index, nn_distance);
 
     // Compute the error
     error += compute_error(nn_distance[0]);