Fix and improve median searching in lmeds

- If there are NaN values in distances, sorting does not work. So move all NaN value to the end and continue with the valid sub range. erase-ing them could also be an option, but that might be slower.
- When searching for the median, it is much faster to use nth_element instead of sort-ing the entire array. My benchmarks suggest that this can make computeModel 4 times faster

sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp

@@ -97,22 +97,36 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
       ++skipped_count;
       continue;
     }
+    // Move all NaNs in distances to the end
+    const auto new_end = std::partition (distances.begin(), distances.end(), [](double d){return !std::isnan (d);});
+    const auto nr_valid_dists = std::distance (distances.begin (), new_end);
 
-    std::sort (distances.begin (), distances.end ());
     // d_cur_penalty = median (distances)
-    std::size_t mid = sac_model_->getIndices ()->size () / 2;
-    if (mid >= distances.size ())
+    const std::size_t mid = nr_valid_dists / 2;
+    PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] There are %lu valid distances remaining after removing NaN values.\n", nr_valid_dists);
+    if (mid >= distances.size () || nr_valid_dists == 0)
     {
       //iterations_++;
       ++skipped_count;
       continue;
     }
 
     // Do we have a "middle" point or should we "estimate" one ?
-    if (sac_model_->getIndices ()->size () % 2 == 0)
-      d_cur_penalty = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
+    if ((nr_valid_dists % 2) == 0)
+    {
+      // Looking at two values instead of one probably doesn't matter because they are mostly barely different, but let's do it for accuracy's sake
+      std::nth_element (distances.begin (), distances.begin () + (mid - 1), new_end);
+      const double tmp = distances[mid-1];
+      std::nth_element (distances.begin (), distances.begin () + mid, new_end);
+      d_cur_penalty = (sqrt (tmp) + sqrt (distances[mid])) / 2.0;
+      PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with two values (%g and %g) because number of distances is even.\n", tmp, distances[mid]);
+    }
     else
+    {
+      std::nth_element (distances.begin (), distances.begin () + mid, new_end);
       d_cur_penalty = sqrt (distances[mid]);
+      PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with one value (%g) because number of distances is odd.\n", distances[mid]);
+    }
 
     // Better match ?
     if (d_cur_penalty < d_best_penalty)