Improve logging errors during sample consensus model registration

sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp

@@ -83,7 +83,14 @@ pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const I
   Indices indices_tgt (3);
   for (int i = 0; i < 3; ++i)
   {
-    indices_tgt[i] = correspondences_.at (samples[i]);
+    const auto it = correspondences_.find (samples[i]);
+    if (it == correspondences_.cend ())
+    {
+      PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] Element with key %i is not in map (map contains %lu elements).\n",
+                 samples[i], correspondences_.size ());
+      return (false);
+    }
+    indices_tgt[i] = it->second;
   }
 
   estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
@@ -264,7 +271,15 @@ pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const
   for (std::size_t i = 0; i < inliers.size (); ++i)
   {
     indices_src[i] = inliers[i];
-    indices_tgt[i] = correspondences_.at (indices_src[i]);
+    const auto it = correspondences_.find (indices_src[i]);
+    if (it == correspondences_.cend ())
+    {
+      PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Element with key %i is not in map (map contains %lu elements).\n",
+                 indices_src[i], correspondences_.size ());
+      optimized_coefficients = model_coefficients;
+      return;
+    }
+    indices_tgt[i] = it->second;
   }
 
   estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);

sample_consensus/include/pcl/sample_consensus/sac_model_registration.h

@@ -49,6 +49,7 @@
 #include <pcl/common/eigen.h>
 #include <pcl/common/centroid.h>
 #include <map>
+#include <numeric> // for std::iota
 
 namespace pcl
 {
@@ -130,13 +131,10 @@ namespace pcl
       setInputTarget (const PointCloudConstPtr &target)
       {
         target_ = target;
-        indices_tgt_.reset (new Indices);
         // Cache the size and fill the target indices
-        int target_size = static_cast<int> (target->size ());
-        indices_tgt_->resize (target_size);
-
-        for (int i = 0; i < target_size; ++i)
-          (*indices_tgt_)[i] = i;
+        const index_t target_size = static_cast<index_t> (target->size ());
+        indices_tgt_.reset (new Indices (target_size));
+        std::iota (indices_tgt_->begin (), indices_tgt_->end (), 0);
         computeOriginalIndexMapping ();
       }
 
@@ -307,10 +305,30 @@ namespace pcl
       void
       computeOriginalIndexMapping ()
       {
-        if (!indices_tgt_ || !indices_ || indices_->empty () || indices_->size () != indices_tgt_->size ())
+        if (!indices_tgt_)
+        {
+          PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_tgt_ is null.\n");
+          return;
+        }
+        if (!indices_)
+        {
+          PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is null.\n");
+          return;
+        }
+        if (indices_->empty ())
+        {
+          PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is empty.\n");
+          return;
+        }
+        if (indices_->size () != indices_tgt_->size ())
+        {
+          PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ and indices_tgt_ are not the same size (%zu vs %zu).\n",
+                     indices_->size (), indices_tgt_->size ());
           return;
+        }
         for (std::size_t i = 0; i < indices_->size (); ++i)
           correspondences_[(*indices_)[i]] = (*indices_tgt_)[i];
+        PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Successfully computed mapping.\n");
       }
 
       /** \brief A boost shared pointer to the target point cloud data array. */
@@ -320,7 +338,7 @@ namespace pcl
       IndicesPtr indices_tgt_;
 
       /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */
-      std::map<int, int> correspondences_;
+      std::map<index_t, index_t> correspondences_;
 
       /** \brief Internal distance threshold used for the sample selection step. */
       double sample_dist_thresh_;