Merge pull request #1285 from Vladimir-Engelgardt/compVariance

Speed-up SampleConsensusModel::computeVariance
PointCloudLibrary · Jul 28, 2015 · 77edc1b · 77edc1b
2 parents 7bf2996 + 2911c52
commit 77edc1b
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model.h b/sample_consensus/include/pcl/sample_consensus/sac_model.h
@@ -421,8 +421,9 @@ namespace pcl
       computeVariance (const std::vector<double> &error_sqr_dists)
       {
         std::vector<double> dists (error_sqr_dists);
-        std::sort (dists.begin (), dists.end ());
-        double median_error_sqr = dists[dists.size () >> 1];
+        const size_t medIdx = dists.size () >> 1;
+        std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ());
+        double median_error_sqr = dists[medIdx];
         return (2.1981 * median_error_sqr);
       }