Bug fixes and optimizations for sample consensus prerejective (replaces #736)

doc/tutorials/content/alignment_prerejective.rst

@@ -9,7 +9,7 @@ In this tutorial, we show how to find the alignment pose of a rigid object in a
 The code
 --------
 
-First, download the datasets from :download:`here <./sources/alignment_prerejective/data/alignment_prerejective.tar.gz>` and extract the files.
+First, download the test models: :download:`object <./sources/alignment_prerejective/chef.pcd>` and :download:`scene <./sources/alignment_prerejective/rs1.pcd>`.
 
 Next, copy and paste the following code into your editor and save it as ``alignment_prerejective.cpp`` (or download the source file :download:`here <./sources/alignment_prerejective/alignment_prerejective.cpp>`).
 
@@ -54,27 +54,27 @@ We are now ready to setup the alignment process. We use the class :pcl:`SampleCo
 
 .. literalinclude:: sources/alignment_prerejective/alignment_prerejective.cpp
    :language: cpp
-   :lines: 79-90
+   :lines: 79-91
 
 .. note:: Apart from the usual input point clouds and features, this class takes some additional runtime parameters which have great influence on the performance of the alignment algorithm. The first two have the same meaning as in the alignment class :pcl:`SampleConsensusInitialAlignment <pcl::SampleConsensusInitialAlignment>`:
 
   - Number of samples - *setNumberOfSamples ()*: The number of point correspondences to sample between the object and the scene. At minimum, 3 points are required to calculate a pose.
   - Correspondence randomness - *setCorrespondenceRandomness ()*: Instead of matching each object FPFH descriptor to its nearest matching feature in the scene, we can choose between the *N* best matches at random. This increases the iterations necessary, but also makes the algorithm robust towards outlier matches.
-  - Polygonal similarity threshold - *setSimlarityThreshold ()*: The alignment class uses the :pcl:`CorrespondenceRejectorPoly <pcl::registration::CorrespondenceRejectorPoly>` class for early elimination of bad poses based on pose-invariant geometric consistencies of the inter-distances between sampled points on the object and the scene. The closer this value is set to 1, the more greedy and thereby fast the algorithm becomes. However, this also increases the risk of eliminating good poses when noise is present.
+  - Polygonal similarity threshold - *setSimilarityThreshold ()*: The alignment class uses the :pcl:`CorrespondenceRejectorPoly <pcl::registration::CorrespondenceRejectorPoly>` class for early elimination of bad poses based on pose-invariant geometric consistencies of the inter-distances between sampled points on the object and the scene. The closer this value is set to 1, the more greedy and thereby fast the algorithm becomes. However, this also increases the risk of eliminating good poses when noise is present.
   - Inlier threshold - *setMaxCorrespondenceDistance ()*: This is the Euclidean distance threshold used for determining whether a transformed object point is correctly aligned to the nearest scene point or not. In this example, we have used a heuristic value of 1.5 times the point cloud resolution.
-  - Inlier fraction - *setInlierFraction ()*: In many practical scenarios, large parts of the observed object in the scene are not visible, either due to clutter, occlusions or both. In such cases, we need to allow for pose hypotheses that do not align all object points to the scene. The absolute number of correctly aligned points is determined using the inlier threshold, and if the ratio of this number to the total number of points in the object is higher than the specified inlier fraction, we accept a pose hypothesis as valid. Furthermore, if a pose generates the highest number of inliers so far, the pose is stored as the current output. In other words, this class tries to maximize the inliers instead of minimizing the fit error during the alignment process.
+  - Inlier fraction - *setInlierFraction ()*: In many practical scenarios, large parts of the observed object in the scene are not visible, either due to clutter, occlusions or both. In such cases, we need to allow for pose hypotheses that do not align all object points to the scene. The absolute number of correctly aligned points is determined using the inlier threshold, and if the ratio of this number to the total number of points in the object is higher than the specified inlier fraction, we accept a pose hypothesis as valid.
 
 Finally, we are ready to execute the alignment process.
 
 .. literalinclude:: sources/alignment_prerejective/alignment_prerejective.cpp
    :language: cpp
-   :lines: 91
+   :lines: 92-95
 
 The aligned object is stored in the point cloud *object_aligned*. If a pose with enough inliers was found (more than 25 % of the total number of object points), the algorithm is said to converge, and we can print and visualize the results.
 
 .. literalinclude:: sources/alignment_prerejective/alignment_prerejective.cpp
    :language: cpp
-   :lines: 95-109
+   :lines: 99-114
 
 
 Compiling and running the program
@@ -91,14 +91,16 @@ After you have made the executable, you can run it like so::
   $ ./alignment_prerejective chef.pcd rs1.pcd
 
 After a few seconds, you will see a visualization and a terminal output similar to::
-
-      | -0.003 -0.972  0.235 | 
-  R = | -0.993 -0.026 -0.119 | 
-      |  0.122 -0.233 -0.965 | 
 
-  t = < 0.095, -0.022, 0.069 >
+  Alignment took 352ms.
 
-  Inliers: 890/3432
+      |  0.040 -0.929 -0.369 | 
+  R = | -0.999 -0.035 -0.020 | 
+      |  0.006  0.369 -0.929 | 
+
+  t = < -0.287, 0.045, 0.126 >
+
+  Inliers: 987/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/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
 
 project(alignment_prerejective)
 
-find_package(PCL 1.7 REQUIRED)
+find_package(PCL 1.7 REQUIRED REQUIRED COMPONENTS io registration segmentation visualization)
 
 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})

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

@@ -83,16 +83,21 @@ main (int argc, char **argv)
   align.setSourceFeatures (object_features);
   align.setInputTarget (scene);
   align.setTargetFeatures (scene_features);
+  align.setMaximumIterations (10000); // Number of RANSAC iterations
   align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
   align.setCorrespondenceRandomness (2); // Number of nearest features to use
-  align.setSimilarityThreshold (0.6f); // Polygonal edge length similarity threshold
-  align.setMaxCorrespondenceDistance (1.5f * leaf); // Set inlier threshold
-  align.setInlierFraction (0.25f); // Set required inlier fraction
-  align.align (*object_aligned);
+  align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
+  align.setMaxCorrespondenceDistance (1.5f * leaf); // Inlier threshold
+  align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
+  {
+    pcl::ScopeTime t("Alignment");
+    align.align (*object_aligned);
+  }
 
   if (align.hasConverged ())
   {
     // Print results
+    printf ("\n");
     Eigen::Matrix4f transformation = align.getFinalTransformation ();
     pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
     pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));

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

@@ -69,8 +69,7 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setTargetF
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointSource, typename PointTarget, typename FeatureT> void 
 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples (
-    const PointCloudSource &cloud, int nr_samples, 
-    std::vector<int> &sample_indices)
+    const PointCloudSource &cloud, int nr_samples, std::vector<int> &sample_indices)
 {
   if (nr_samples > static_cast<int> (cloud.points.size ()))
   {
@@ -79,53 +78,64 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamp
                nr_samples, cloud.points.size ());
     return;
   }
+
+  sample_indices.resize (nr_samples);
+  int temp_sample;
 
-  // Iteratively draw random samples until nr_samples is reached
-  sample_indices.clear ();
-  std::vector<bool> sampled_indices (cloud.points.size (), false);
-  while (static_cast<int> (sample_indices.size ()) < nr_samples)
+  // Draw random samples until n samples is reached
+  for (int i = 0; i < nr_samples; i++)
   {
-    // Choose a unique sample at random
-    int sample_index;
-    do
+    // Select a random number
+    sample_indices[i] = getRandomIndex (static_cast<int> (cloud.points.size ()) - i);
+
+    // Run trough list of numbers, starting at the lowest, to avoid duplicates
+    for (int j = 0; j < i; j++)
     {
-      sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));
+      // Move value up if it is higher than previous selections to ensure true randomness
+      if (sample_indices[i] >= sample_indices[j])
+      {
+        sample_indices[i]++;
+      }
+      else
+      {
+        // The new number is lower, place it at the correct point and break for a sorted list
+        temp_sample = sample_indices[i];
+        for (int k = i; k > j; k--)
+          sample_indices[k] = sample_indices[k - 1];
+
+        sample_indices[j] = temp_sample;
+        break;
+      }
     }
-    while (sampled_indices[sample_index]);
-
-    // Mark index as sampled
-    sampled_indices[sample_index] = true;
-
-    // Store
-    sample_indices.push_back (sample_index);
   }
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointSource, typename PointTarget, typename FeatureT> void 
 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
-    const FeatureCloud &input_features, const std::vector<int> &sample_indices, 
-    std::vector<int> &corresponding_indices)
+        const std::vector<int> &sample_indices,
+        std::vector<std::vector<int> >& similar_features,
+        std::vector<int> &corresponding_indices)
 {
-  std::vector<int> nn_indices (k_correspondences_);
-  std::vector<float> nn_distances (k_correspondences_);
-
+  // Allocate results
   corresponding_indices.resize (sample_indices.size ());
+  std::vector<float> nn_distances (k_correspondences_);
+
+  // Loop over the sampled features
   for (size_t i = 0; i < sample_indices.size (); ++i)
   {
-    // Find the k features nearest to input_features.points[sample_indices[i]]
-    feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
+    // Current feature index
+    const int idx = sample_indices[i];
+
+    // Find the k nearest feature neighbors to the sampled input feature if they are not in the cache already
+    if (similar_features[idx].empty ())
+      feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances);
 
     // Select one at random and add it to corresponding_indices
     if (k_correspondences_ == 1)
-    {
-      corresponding_indices[i] = nn_indices[0];
-    }
+      corresponding_indices[i] = similar_features[idx][0];
     else
-    {
-      int random_correspondence = getRandomIndex (k_correspondences_);
-      corresponding_indices[i] = nn_indices[random_correspondence];
-    }
+      corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)];
   }
 }
 
@@ -180,11 +190,18 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTra
     return;
   }
 
+  if (k_correspondences_ <= 0)
+  {
+    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
+    PCL_ERROR ("Illegal correspondence randomness %d, must be > 0!\n",
+            k_correspondences_);
+    return;
+  }
+
   // Initialize prerejector (similarity threshold already set to default value in constructor)
   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
@@ -213,34 +230,25 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTra
     }
   }
 
+  // Feature correspondence cache
+  std::vector<std::vector<int> > similar_features (input_->size ());
+
   // Start
   for (int i = 0; i < max_iterations_; ++i)
   {
     // Temporary containers
-    std::vector<int> sample_indices (nr_samples_);
-    std::vector<int> corresponding_indices (nr_samples_);
+    std::vector<int> sample_indices;
+    std::vector<int> corresponding_indices;
 
     // 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);
+    findSimilarFeatures (sample_indices, similar_features, 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;
     }
@@ -262,19 +270,14 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTra
 
     // If the new fit is better, update results
     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_;
-      }
+
+    // Update result if pose hypothesis is better
+    if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
+    {
+      inliers_ = inliers;
+      lowest_error = error;
+      converged_ = true;
+      final_transformation_ = transformation_;
     }
   }
 
@@ -315,16 +318,11 @@ pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness
     // Check if point is an inlier
     if (nn_dists[0] < max_range)
     {
-      // Errors
-      const float dx = input_transformed.points[i].x - target_->points[nn_indices[0]].x;
-      const float dy = input_transformed.points[i].y - target_->points[nn_indices[0]].y;
-      const float dz = input_transformed.points[i].z - target_->points[nn_indices[0]].z;
-
       // Update inliers
       inliers.push_back (static_cast<int> (i));
 
       // Update fitness score
-      fitness_score += dx*dx + dy*dy + dz*dz;
+      fitness_score += nn_dists[0];
     }
   }
 

registration/include/pcl/registration/sample_consensus_prerejective.h

@@ -257,19 +257,19 @@ namespace pcl
         * \param sample_indices the resulting sample indices
         */
       void 
-      selectSamples (const PointCloudSource &cloud, int nr_samples, 
-                     std::vector<int> &sample_indices);
+      selectSamples (const PointCloudSource &cloud, int nr_samples, std::vector<int> &sample_indices);
 
       /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to 
         * the sample points' features. From these, select one randomly which will be considered that sample point's 
-        * correspondence. 
-        * \param input_features a cloud of feature descriptors
+        * correspondence.
         * \param sample_indices the indices of each sample point
+        * \param similar_features correspondence cache, which is used to read/write already computed correspondences
         * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud
         */
       void 
-      findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices, 
-                           std::vector<int> &corresponding_indices);
+      findSimilarFeatures (const std::vector<int> &sample_indices,
+              std::vector<std::vector<int> >& similar_features,
+              std::vector<int> &corresponding_indices);
 
       /** \brief Rigid transformation computation method.
         * \param output the transformed input point cloud dataset using the rigid transformation found