Add checks for is_dense to lmeds

PointCloudLibrary · Mar 2, 2020 · 0ad9e94 · 0ad9e94
1 parent 3201899
commit 0ad9e94
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp
@@ -98,8 +98,8 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
       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);
+    const auto new_end = (sac_model_->getInputCloud()->is_dense ? distances.end() : std::partition (distances.begin(), distances.end(), [](double d){return !std::isnan (d);}));
+    const auto nr_valid_dists = (sac_model_->getInputCloud()->is_dense ? distances.size() : std::distance (distances.begin (), new_end));
 
     // d_cur_penalty = median (distances)
     const std::size_t mid = nr_valid_dists / 2;