[BUG FIX] getCorrespondenceScore must not go out-of-bounds.

registration/include/pcl/registration/correspondence_rejection.h

@@ -347,6 +347,12 @@ class DataContainer : public DataContainerInterface {
   inline double
   getCorrespondenceScore(int index) override
   {
+    // Check correspondence is valid
+    if ((size_t) index >= (*input_).size())
+    {
+      return (std::numeric_limits<double>::max ());
+    }
+
     if (target_cloud_updated_ && !force_no_recompute_) {
       tree_->setInputCloud(target_);
     }
@@ -363,6 +369,12 @@ class DataContainer : public DataContainerInterface {
   inline double
   getCorrespondenceScore(const pcl::Correspondence& corr) override
   {
+    // Check correspondence is valid
+    if ((size_t) corr.index_query >= (*input_).size() || (size_t) corr.index_match >= (*target_).size())
+    {
+      return (std::numeric_limits<double>::max ());
+    }
+
     // Get the source and the target feature from the list
     const PointT& src = (*input_)[corr.index_query];
     const PointT& tgt = (*target_)[corr.index_match];
@@ -377,6 +389,12 @@ class DataContainer : public DataContainerInterface {
   inline double
   getCorrespondenceScoreFromNormals(const pcl::Correspondence& corr) override
   {
+    // Check correspondence is valid
+    if ((size_t) corr.index_query >= (*input_normals_).size() || (size_t) corr.index_match >= (*target_normals_).size())
+    {
+      return (std::numeric_limits<double>::max ());
+    }
+
     // assert ( (input_normals_->size () != 0) && (target_normals_->size () != 0) &&
     // "Normals are not set for the input and target point clouds");
     assert(input_normals_ && target_normals_ &&

registration/include/pcl/registration/correspondence_rejection_features.h

@@ -257,6 +257,12 @@ class PCL_EXPORTS CorrespondenceRejectorFeatures : public CorrespondenceRejector
     inline double
     getCorrespondenceScore(int index) override
     {
+      // Check correspondence is valid
+      if (index >= (*source_features_).size() || index >= (*target_features_).size())
+      {
+        return (std::numeric_limits<double>::max ());
+      }
+
       // If no feature representation was given, reset to the default implementation for
       // FeatureT
       if (!feature_representation_)

registration/src/correspondence_rejection_median_distance.cpp

@@ -55,16 +55,18 @@ pcl::registration::CorrespondenceRejectorMedianDistance::getRemainingCorresponde
       dists[i] = original_correspondences[i].distance;
   }
 
+  unsigned int number_valid_correspondences = 0;
   std::vector<double> nth(dists);
-  nth_element(nth.begin(), nth.begin() + (nth.size() / 2), nth.end());
-  median_distance_ = nth[nth.size() / 2];
+  if (!nth.empty()) {
+    nth_element(nth.begin(), nth.begin() + (nth.size() / 2), nth.end());
+    median_distance_ = nth[nth.size() / 2];
 
-  unsigned int number_valid_correspondences = 0;
-  remaining_correspondences.resize(original_correspondences.size());
+    remaining_correspondences.resize(original_correspondences.size());
 
-  for (std::size_t i = 0; i < original_correspondences.size(); ++i)
-    if (dists[i] <= median_distance_ * factor_)
-      remaining_correspondences[number_valid_correspondences++] =
-          original_correspondences[i];
+    for (std::size_t i = 0; i < original_correspondences.size(); ++i)
+      if (dists[i] <= median_distance_ * factor_)
+        remaining_correspondences[number_valid_correspondences++] =
+            original_correspondences[i];
+  }
   remaining_correspondences.resize(number_valid_correspondences);
 }