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_circle3d.hpp

@@ -193,14 +193,19 @@ pcl::SampleConsensusModelCircle3D<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 auto squared_threshold = threshold * threshold;
   // 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(model_coefficients, squared_threshold)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size ()); ++i)
   {
     // what i have:
     // P : Sample Point
@@ -225,12 +230,17 @@ pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
 
     const double sqr_dist = distanceVector.squaredNorm ();
     if (sqr_dist < squared_threshold)
-    {
+      error_sqr_dists_[i] = sqr_dist;
+    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 (sqr_dist);
+      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

@@ -213,10 +213,14 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
     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 ());
 
   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 +231,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 +247,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 +268,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_cylinder.hpp

@@ -195,24 +195,31 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance (
     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 ());
 
   Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
   Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
   float ptdotdir = line_pt.dot (line_dir);
   float dirdotdir = 1.0f / line_dir.dot (line_dir);
   // 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(line_pt, line_dir, ptdotdir, dirdotdir, model_coefficients, threshold)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size ()); ++i)
   {
     // Approximate the distance from the point to the cylinder as the difference between
     // dist(point,cylinder_axis) and cylinder radius
     Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
     const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
-    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 point's projection on the cylinder axis
     float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
@@ -227,12 +234,17 @@ pcl::SampleConsensusModelCylinder<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_ellipse3d.hpp

@@ -312,14 +312,17 @@ pcl::SampleConsensusModelEllipse3D<PointT>::selectWithinDistance (
     Indices &inliers)
 {
   inliers.clear();
-  error_sqr_dists_.clear();
   // Check if the model is valid given the user constraints
   if (!isModelValid (model_coefficients))
   {
     return;
   }
+#ifdef _OPENMP
+  if (threads_ == 0)
+    threads_ = omp_get_num_procs();
+#endif // _OPENMP
   inliers.reserve (indices_->size ());
-  error_sqr_dists_.reserve (indices_->size ());
+  error_sqr_dists_.resize (indices_->size ());
 
   // c : Ellipse Center
   const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
@@ -344,7 +347,8 @@ pcl::SampleConsensusModelEllipse3D<PointT>::selectWithinDistance (
 
   const auto squared_threshold = threshold * threshold;
   // Iterate through the 3d points and calculate the distances from them to the ellipse
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+#pragma omp parallel for num_threads(threads_) default(none) shared(c, Rot_T, par_a, par_b, squared_threshold)
+  for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(indices_->size ()); ++i)
   {
     // p : Sample Point
     const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
@@ -362,12 +366,17 @@ pcl::SampleConsensusModelEllipse3D<PointT>::selectWithinDistance (
 
     const double sqr_dist = distanceVector.squaredNorm();
     if (sqr_dist < squared_threshold)
-    {
+      error_sqr_dists_[i] = sqr_dist;
+    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 (sqr_dist);
+      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

@@ -139,32 +139,42 @@ pcl::SampleConsensusModelLine<PointT>::selectWithinDistance (
   if (!isModelValid (model_coefficients))
     return;
 
+#ifdef _OPENMP
+  if (threads_ == 0)
+    threads_ = omp_get_num_procs();
+#endif // _OPENMP
+
   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, model_coefficients, 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

@@ -63,17 +63,22 @@ pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::selectWithinDistance (
     return;
   }
 
+#ifdef _OPENMP
+  if (threads_ == 0)
+    threads_ = omp_get_num_procs();
+#endif // _OPENMP
+
   // Obtain the sphere center
   Eigen::Vector4f center = model_coefficients;
   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 +89,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 +104,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_plane.hpp

@@ -176,7 +176,7 @@ pcl::SampleConsensusModelPlane<PointT>::selectWithinDistance (
                         1.0f);
 
     float distance = std::abs (model_coefficients.dot (pt));
-    
+
     if (distance < threshold)
     {
       // Returns the indices of the points whose distances are smaller than the threshold