Merge pull request #212 from andersgb1/sample_consensus_prerejective

Fixed a small problem with the alignment prerejective (tutorial) code

doc/tutorials/content/alignment_prerejective.rst

@@ -92,13 +92,14 @@ After you have made the executable, you can run it like so::
 
 After a few seconds, you will see a visualization and a terminal output similar to::
   
-       |  0.027 -0.977  0.212 | 
-   R = | -0.991 -0.055 -0.124 | 
-       |  0.133 -0.207 -0.969 | 
+      | -0.003 -0.972  0.235 | 
+  R = | -0.993 -0.026 -0.119 | 
+      |  0.122 -0.233 -0.965 | 
 
-   t = < 0.084, -0.024, 0.069 >
+  t = < 0.095, -0.022, 0.069 >
+
+  Inliers: 890/3432
 
-   Inliers: 912/3432
 
 The visualization window should look something like the below figures. The scene is shown with green color, and the aligned object model is shown with blue color. Note the high number of non-visible object points.
 

doc/tutorials/content/sources/alignment_prerejective/alignment_prerejective.cpp

@@ -61,7 +61,7 @@ main (int argc, char **argv)
   // Estimate normals for scene
   pcl::console::print_highlight ("Estimating scene normals...\n");
   pcl::NormalEstimationOMP<PointNT,PointNT> nest;
-  nest.setRadiusSearch (0.015);
+  nest.setRadiusSearch (0.01);
   nest.setInputCloud (scene);
   nest.compute (*scene);
 

registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp

@@ -184,12 +184,13 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTra
   correspondence_rejector_poly_->setInputSource (input_);
   correspondence_rejector_poly_->setInputTarget (target_);
   correspondence_rejector_poly_->setCardinality (nr_samples_);
+  std::vector<bool> accepted (input_->size (), false); // Indices of sampled points that passed prerejection
   int num_rejections = 0; // For debugging
 
   // Initialize results
   final_transformation_ = guess;
   inliers_.clear ();
-  float highest_inlier_fraction = inlier_fraction_;
+  float lowest_error = std::numeric_limits<float>::max ();
   converged_ = false;
 
   // Temporaries
@@ -202,11 +203,12 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTra
   {
     getFitness (inliers, error);
     inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
+    error /= static_cast<float> (inliers.size ());
 
-    if (inlier_fraction > highest_inlier_fraction)
+    if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
     {
       inliers_ = inliers;
-      highest_inlier_fraction = inlier_fraction;
+      lowest_error = error;
       converged_ = true;
     }
   }
@@ -220,12 +222,25 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTra
 
     // Draw nr_samples_ random samples
     selectSamples (*input_, nr_samples_, sample_indices);
+
+    // Check if all sampled points already been accepted
+    bool samples_accepted = true;
+    for (unsigned int j = 0; j < sample_indices.size(); ++j) {
+      if (!accepted[j]) {
+        samples_accepted = false;
+        break;
+      }
+    }
 
+    // All points have already been accepted, avoid
+    if (samples_accepted)
+      continue;
+
     // Find corresponding features in the target cloud
     findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
 
     // Apply prerejection
-    if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)){
+    if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)) {
       ++num_rejections;
       continue;
     }
@@ -241,19 +256,25 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTra
 
     // Transform the input and compute the error (uses input_ and final_transformation_)
     getFitness (inliers, error);
+
+    // Restore previous result
+    final_transformation_ = final_transformation_prev;
 
     // If the new fit is better, update results
-    const float inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
-    if (inlier_fraction > highest_inlier_fraction)
-    {
-      inliers_ = inliers;
-      highest_inlier_fraction = inlier_fraction;
-      converged_ = true;
-    }
-    else
-    {
-      // Restore previous result
-      final_transformation_ = final_transformation_prev;
+    inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
+
+    if (inlier_fraction >= inlier_fraction_) {
+      // Mark the sampled points accepted
+      for (int j = 0; j < nr_samples_; ++j)
+        accepted[j] = true;
+
+      // Update result if pose hypothesis is better
+      if (error < lowest_error) {
+        inliers_ = inliers;
+        lowest_error = error;
+        converged_ = true;
+        final_transformation_ = transformation_;
+      }
     }
   }
 
@@ -282,7 +303,7 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness
   PointCloudSource input_transformed;
   input_transformed.resize (input_->size ());
   transformPointCloud (*input_, input_transformed, final_transformation_);
-
+  
   // For each point in the source dataset
   for (size_t i = 0; i < input_transformed.points.size (); ++i)
   {

registration/include/pcl/registration/sample_consensus_prerejective.h

@@ -56,9 +56,9 @@ namespace pcl
    * constraints, as also implemented in the class
    * \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly".
    * 
-   * In order to robustly align partial/occluded models, this routine does not
-   * try to minimize the fit error, but instead tries to maximize the inlier rate,
-   * above a threshold specifiable using \ref setInlierFraction().
+   * In order to robustly align partial/occluded models, this routine performs
+   * fit error evaluation using only inliers, i.e. points closer than a
+   * Euclidean threshold, which is specifiable using \ref setInlierFraction().
    * 
    * The amount of prerejection or "greedyness" of the algorithm can be specified
    * using \ref setSimilarityThreshold() in [0,1[, where a value of 0 means disabled,