Parallelize selectWithinDistance

Approach: use error_sqr_dists_ as buffer, for each point (in parallel) store the distance to the model (if inlier), or -1 (if outlier). In a second loop, fill inliers vector and move distance entries to final position.

- results are exactly the same as before (same order)
- does not need more space than before

Benchmarks for plane and sphere model:
       | Before | Now (OpenMP disabled) | OpenMP 1 Thread | 2 Threads | 3 Threads | 4 Threads | 5 Threads | 6 Threads
Plane  | 96.0   | 93.9                  | 91.7            | 49.1      | 34.9      | 28.0      | 23.9      | 20.6
Sphere | 16.2   | 16.2                  | 16.2            | 8.56      | 6.07      | 4.72      | 4.00      | 3.40

sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp

@@ -174,15 +174,21 @@ pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
     inliers.clear ();
     return;
   }
+
+#ifdef _OPENMP
+  if (threads_ == 0)
+    threads_ = omp_get_num_procs();
+#endif // _OPENMP
+
   inliers.clear ();
-  error_sqr_dists_.clear ();
   inliers.reserve (indices_->size ());
-  error_sqr_dists_.reserve (indices_->size ());
+  error_sqr_dists_.resize (indices_->size ());
 
   const float sqr_inner_radius = (model_coefficients[2] <= threshold ? 0.0f : (model_coefficients[2] - threshold) * (model_coefficients[2] - threshold));
   const float sqr_outer_radius = (model_coefficients[2] + threshold) * (model_coefficients[2] + threshold);
   // Iterate through the 3d points and calculate the distances from them to the circle
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+#pragma omp parallel for num_threads(threads_) default(none) shared(model_coefficients, sqr_inner_radius, sqr_outer_radius)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size ()); ++i)
   {
     // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold).
     // Valid if point is in larger circle, but not in smaller circle.
@@ -191,13 +197,18 @@ pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
                            ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
                            ( (*input_)[(*indices_)[i]].y - model_coefficients[1] );
     if ((sqr_dist <= sqr_outer_radius) && (sqr_dist >= sqr_inner_radius))
-    {
+      // Only compute exact distance if necessary (if point is inlier)
+      error_sqr_dists_[i] = static_cast<double> (std::abs (std::sqrt (sqr_dist) - model_coefficients[2]));
+    else
+      error_sqr_dists_[i] = -1.0;
+  }
+  for (std::size_t i = 0, j = 0; i < indices_->size (); ++i)
+    if (error_sqr_dists_[i] >= 0.0) {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
-      // Only compute exact distance if necessary (if point is inlier)
-      error_sqr_dists_.push_back (static_cast<double> (std::abs (std::sqrt (sqr_dist) - model_coefficients[2])));
+      error_sqr_dists_[j++] = error_sqr_dists_[i];
     }
-  }
+  error_sqr_dists_.resize(inliers.size());
 }
 
 //////////////////////////////////////////////////////////////////////////

sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp

@@ -214,9 +214,8 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
   }
 
   inliers.clear ();
-  error_sqr_dists_.clear ();
   inliers.reserve (indices_->size ());
-  error_sqr_dists_.reserve (indices_->size ());
+  error_sqr_dists_.resize (indices_->size ());
 
   Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
   Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
@@ -227,7 +226,8 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
   float apexdotdir = apex.dot (axis_dir);
   float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
   // Iterate through the 3d points and calculate the distances from them to the cone
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+#pragma omp parallel for num_threads(threads_) default(none) shared(model_coefficients, threshold, apex, axis_dir, apexdotdir, dirdotdir, sin_opening_angle, cos_opening_angle, tan_opening_angle)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size ()); ++i)
   {
     Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
 
@@ -242,8 +242,10 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
     // Approximate the distance from the point to the cone as the difference between
     // dist(point,cone_axis) and actual cone radius
     const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
-    if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
+    if (weighted_euclid_dist > threshold) { // Early termination: cannot be an inlier
+      error_sqr_dists_[i] = -1.0;
       continue;
+    }
 
     // Calculate the direction of the point from center
     Eigen::Vector4f pp_pt_dir = pt - pt_proj;
@@ -261,12 +263,17 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
     double distance = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
 
     if (distance < threshold)
-    {
+      error_sqr_dists_[i] = distance;
+    else
+      error_sqr_dists_[i] = -1.0;
+  }
+  for (std::size_t i = 0, j = 0; i < indices_->size (); ++i)
+    if (error_sqr_dists_[i] >= 0.0) {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
-      error_sqr_dists_.push_back (distance);
+      error_sqr_dists_[j++] = error_sqr_dists_[i];
     }
-  }
+  error_sqr_dists_.resize(inliers.size());
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////

sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp

@@ -142,29 +142,34 @@ pcl::SampleConsensusModelLine<PointT>::selectWithinDistance (
   double sqr_threshold = threshold * threshold;
 
   inliers.clear ();
-  error_sqr_dists_.clear ();
   inliers.reserve (indices_->size ());
-  error_sqr_dists_.reserve (indices_->size ());
+  error_sqr_dists_.resize (indices_->size ());
 
   // Obtain the line point and direction
   Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
   Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
   line_dir.normalize ();
 
   // Iterate through the 3d points and calculate the distances from them to the line
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+#pragma omp parallel for num_threads(threads_) default(none) shared(line_pt, line_dir, sqr_threshold)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size ()); ++i)
   {
     // Calculate the distance from the point to the line
     // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
     double sqr_distance = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
 
     if (sqr_distance < sqr_threshold)
-    {
+      error_sqr_dists_[i] = sqr_distance;
+    else
+      error_sqr_dists_[i] = -1.0;
+  }
+  for (std::size_t i = 0, j = 0; i < indices_->size (); ++i)
+    if (error_sqr_dists_[i] >= 0.0) {
       // Returns the indices of the points whose squared distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
-      error_sqr_dists_.push_back (sqr_distance);
+      error_sqr_dists_[j++] = error_sqr_dists_[i];
     }
-  }
+  error_sqr_dists_.resize(inliers.size());
 }
 
 //////////////////////////////////////////////////////////////////////////

sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp

@@ -63,17 +63,22 @@ pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::selectWithinDistance (
     return;
   }
 
+#ifdef _OPENMP
+  if (threads_ == 0)
+    threads_ = omp_get_num_procs();
+#endif // _OPENMP
+
   // Obtain the plane normal
   Eigen::Vector4f coeff = model_coefficients;
   coeff[3] = 0.0f;
 
   inliers.clear ();
-  error_sqr_dists_.clear ();
   inliers.reserve (indices_->size ());
-  error_sqr_dists_.reserve (indices_->size ());
+  error_sqr_dists_.resize (indices_->size ());
 
   // Iterate through the 3d points and calculate the distances from them to the plane
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+#pragma omp parallel for num_threads(threads_) default(none) shared(coeff, threshold)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size()); ++i)
   {
     const PointT  &pt = (*input_)[(*indices_)[i]];
     const PointNT &nt = (*normals_)[(*indices_)[i]];
@@ -92,12 +97,19 @@ pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::selectWithinDistance (
 
     double distance = std::abs (weight * d_normal + (1.0 - weight) * d_euclid); 
     if (distance < threshold)
-    {
+      error_sqr_dists_[i] = distance;
+    else
+      error_sqr_dists_[i] = -1.0;
+  }
+  for (std::size_t i = 0, j = 0; i < indices_->size (); ++i)
+  {
+    if (error_sqr_dists_[i] >= 0.0) {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
-      error_sqr_dists_.push_back (distance);
+      error_sqr_dists_[j++] = error_sqr_dists_[i];
     }
   }
+  error_sqr_dists_.resize(inliers.size());
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////

sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp

@@ -68,12 +68,12 @@ pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::selectWithinDistance (
   center[3] = 0.0f;
 
   inliers.clear ();
-  error_sqr_dists_.clear ();
   inliers.reserve (indices_->size ());
-  error_sqr_dists_.reserve (indices_->size ());
+  error_sqr_dists_.resize (indices_->size ());
 
   // Iterate through the 3d points and calculate the distances from them to the sphere
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+#pragma omp parallel for num_threads(threads_) default(none) shared(center, model_coefficients, threshold)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size ()); ++i)
   {
     // Calculate the distance from the point to the sphere center as the difference between
     // dist(point,sphere_origin) and sphere_radius
@@ -84,8 +84,10 @@ pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::selectWithinDistance (
 
     Eigen::Vector4f n_dir = p - center;
     const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (n_dir.norm () - model_coefficients[3]);
-    if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
+    if (weighted_euclid_dist > threshold) { // Early termination: cannot be an inlier
+      error_sqr_dists_[i] = -1.0;
       continue;
+    }
 
     // Calculate the angular distance between the point normal and the sphere normal
     Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], 
@@ -97,12 +99,17 @@ pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::selectWithinDistance (
 
     double distance = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
     if (distance < threshold)
-    {
+      error_sqr_dists_[i] = static_cast<double> (distance);
+    else
+      error_sqr_dists_[i] = -1.0;
+  }
+  for (std::size_t i = 0, j = 0; i < indices_->size (); ++i)
+    if (error_sqr_dists_[i] >= 0.0) {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
-      error_sqr_dists_.push_back (static_cast<double> (distance));
+      error_sqr_dists_[j++] = error_sqr_dists_[i];
     }
-  }
+  error_sqr_dists_.resize(inliers.size());
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////

sample_consensus/include/pcl/sample_consensus/impl/sac_model_torus.hpp

@@ -303,12 +303,16 @@ pcl::SampleConsensusModelTorus<PointT, PointNT>::selectWithinDistance(
     inliers.clear();
     return;
   }
+#ifdef _OPENMP
+  if (threads_ == 0)
+    threads_ = omp_get_num_procs();
+#endif // _OPENMP
   inliers.clear();
-  error_sqr_dists_.clear();
   inliers.reserve(indices_->size());
-  error_sqr_dists_.reserve(indices_->size());
+  error_sqr_dists_.resize(indices_->size());
 
-  for (std::size_t i = 0; i < indices_->size(); ++i) {
+#pragma omp parallel for num_threads(threads_) default(none) shared(model_coefficients, threshold)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size()); ++i) {
     const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
     const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
 
@@ -317,13 +321,19 @@ pcl::SampleConsensusModelTorus<PointT, PointNT>::selectWithinDistance(
 
     const float distance = (torus_closest - pt).norm();
 
-    if (distance < threshold) {
+    if (distance < threshold)
+      error_sqr_dists_[i] = distance;
+    else
+      error_sqr_dists_[i] = -1.0;
+  }
+  for (std::size_t i = 0, j = 0; i < indices_->size (); ++i)
+    if (error_sqr_dists_[i] >= 0.0) {
       // Returns the indices of the points whose distances are smaller than the
       // threshold
-      inliers.push_back((*indices_)[i]);
-      error_sqr_dists_.push_back(distance);
+      inliers.push_back ((*indices_)[i]);
+      error_sqr_dists_[j++] = error_sqr_dists_[i];
     }
-  }
+  error_sqr_dists_.resize(inliers.size());
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////

sample_consensus/include/pcl/sample_consensus/sac_model.h

@@ -458,6 +458,12 @@ namespace pcl
         return (computeVariance (error_sqr_dists_));
       }
 
+      /** \brief Set how many threads the model should use.
+        * \param nr_threads the number of threads to use (if 0, the number is determined automatically)
+        */
+      inline void
+      setNumberOfThreads (unsigned int nr_threads) { threads_ = nr_threads; } // TODO automatic setting
+
     protected:
 
       /** \brief Fills a sample array with random samples from the indices_ vector
@@ -600,6 +606,9 @@ namespace pcl
 
       /** \brief A user defined function that takes model coefficients and returns whether the model is acceptable or not. */
       std::function<bool(const Eigen::VectorXf &)> custom_model_constraints_;
+
+      /** \brief The number of threads the scheduler should use. */
+      unsigned int threads_{0}; // TODO int or unsigned int?
     public:
       PCL_MAKE_ALIGNED_OPERATOR_NEW
  };

sample_consensus/include/pcl/sample_consensus/sac_model_circle.h

@@ -207,6 +207,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** \brief Check whether a model is valid given the user constraints.
         * \param[in] model_coefficients the set of model coefficients

sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h

@@ -202,6 +202,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** \brief Check whether a model is valid given the user constraints.
         * \param[in] model_coefficients the set of model coefficients

sample_consensus/include/pcl/sample_consensus/sac_model_cone.h

@@ -273,6 +273,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** \brief Get the distance from a point to a line (represented by a point and a direction)
         * \param[in] pt a point

sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h

@@ -245,6 +245,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** \brief Get the distance from a point to a line (represented by a point and a direction)
         * \param[in] pt a point

sample_consensus/include/pcl/sample_consensus/sac_model_ellipse3d.h

@@ -182,6 +182,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** \brief Check whether a model is valid given the user constraints.
         * \param[in] model_coefficients the set of model coefficients

sample_consensus/include/pcl/sample_consensus/sac_model_line.h

@@ -182,6 +182,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** \brief Check if a sample of indices results in a good sample of points
         * indices.

sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h

@@ -164,6 +164,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** This implementation uses no SIMD instructions. It is not intended for normal use.
         * See countWithinDistance which automatically uses the fastest implementation.

sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h

@@ -158,6 +158,7 @@ namespace pcl
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
       using SampleConsensusModelSphere<PointT>::isModelValid;
+      using SampleConsensusModel<PointT>::threads_;
   };
 }
 

sample_consensus/include/pcl/sample_consensus/sac_model_plane.h

@@ -263,6 +263,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** This implementation uses no SIMD instructions. It is not intended for normal use.
         * See countWithinDistance which automatically uses the fastest implementation.

sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h

@@ -214,6 +214,7 @@ namespace pcl
     protected:
       using SampleConsensusModel<PointT>::sample_size_;
       using SampleConsensusModel<PointT>::model_size_;
+      using SampleConsensusModel<PointT>::threads_;
 
       /** \brief Check whether a model is valid given the user constraints.
         * \param[in] model_coefficients the set of model coefficients

sample_consensus/include/pcl/sample_consensus/sac_model_torus.h

@@ -220,6 +220,7 @@ class SampleConsensusModelTorus
 protected:
   using SampleConsensusModel<PointT>::sample_size_;
   using SampleConsensusModel<PointT>::model_size_;
+  using SampleConsensusModel<PointT>::threads_;
 
   /** \brief Project a point onto a torus given by its model coefficients (radii,
    * torus_center_point, torus_normal)