sample_consensus_prerejective.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
00042 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
00043 
00045 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00046 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features)
00047 {
00048   if (features == NULL || features->empty ())
00049   {
00050     PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
00051     return;
00052   }
00053   input_features_ = features;
00054 }
00055 
00057 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00058 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features)
00059 {
00060   if (features == NULL || features->empty ())
00061   {
00062     PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
00063     return;
00064   }
00065   target_features_ = features;
00066   feature_tree_->setInputCloud (target_features_);
00067 }
00068 
00070 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00071 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples (
00072     const PointCloudSource &cloud, int nr_samples, 
00073     std::vector<int> &sample_indices)
00074 {
00075   if (nr_samples > static_cast<int> (cloud.points.size ()))
00076   {
00077     PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
00078     PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%zu)!\n",
00079                nr_samples, cloud.points.size ());
00080     return;
00081   }
00082 
00083   // Iteratively draw random samples until nr_samples is reached
00084   sample_indices.clear ();
00085   std::vector<bool> sampled_indices (cloud.points.size (), false);
00086   while (static_cast<int> (sample_indices.size ()) < nr_samples)
00087   {
00088     // Choose a unique sample at random
00089     int sample_index;
00090     do
00091     {
00092       sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));
00093     }
00094     while (sampled_indices[sample_index]);
00095     
00096     // Mark index as sampled
00097     sampled_indices[sample_index] = true;
00098     
00099     // Store
00100     sample_indices.push_back (sample_index);
00101   }
00102 }
00103 
00105 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00106 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
00107     const FeatureCloud &input_features, const std::vector<int> &sample_indices, 
00108     std::vector<int> &corresponding_indices)
00109 {
00110   std::vector<int> nn_indices (k_correspondences_);
00111   std::vector<float> nn_distances (k_correspondences_);
00112 
00113   corresponding_indices.resize (sample_indices.size ());
00114   for (size_t i = 0; i < sample_indices.size (); ++i)
00115   {
00116     // Find the k features nearest to input_features.points[sample_indices[i]]
00117     feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
00118 
00119     // Select one at random and add it to corresponding_indices
00120     if (k_correspondences_ == 1)
00121     {
00122       corresponding_indices[i] = nn_indices[0];
00123     }
00124     else
00125     {
00126       int random_correspondence = getRandomIndex (k_correspondences_);
00127       corresponding_indices[i] = nn_indices[random_correspondence];
00128     }
00129   }
00130 }
00131 
00133 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00134 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
00135 {
00136   // Some sanity checks first
00137   if (!input_features_)
00138   {
00139     PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
00140     PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
00141     return;
00142   }
00143   if (!target_features_)
00144   {
00145     PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
00146     PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
00147     return;
00148   }
00149 
00150   if (input_->size () != input_features_->size ())
00151   {
00152     PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
00153     PCL_ERROR ("The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
00154                input_->size (), input_features_->size ());
00155     return;
00156   }
00157 
00158   if (target_->size () != target_features_->size ())
00159   {
00160     PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
00161     PCL_ERROR ("The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
00162                target_->size (), target_features_->size ());
00163     return;
00164   }
00165 
00166   if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f)
00167   {
00168     PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
00169     PCL_ERROR ("Illegal inlier fraction %f, must be in [0,1]!\n",
00170                inlier_fraction_);
00171     return;
00172   }
00173   
00174   const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold ();
00175   if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f)
00176   {
00177     PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
00178     PCL_ERROR ("Illegal prerejection similarity threshold %f, must be in [0,1[!\n",
00179                similarity_threshold);
00180     return;
00181   }
00182   
00183   // Initialize prerejector (similarity threshold already set to default value in constructor)
00184   correspondence_rejector_poly_->setInputSource (input_);
00185   correspondence_rejector_poly_->setInputTarget (target_);
00186   correspondence_rejector_poly_->setCardinality (nr_samples_);
00187   int num_rejections = 0; // For debugging
00188   
00189   // Initialize results
00190   final_transformation_ = guess;
00191   inliers_.clear ();
00192   float highest_inlier_fraction = inlier_fraction_;
00193   converged_ = false;
00194   
00195   // Temporaries
00196   std::vector<int> inliers;
00197   float inlier_fraction;
00198   float error;
00199   
00200   // If guess is not the Identity matrix we check it
00201   if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
00202   {
00203     getFitness (inliers, error);
00204     inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
00205     
00206     if (inlier_fraction > highest_inlier_fraction)
00207     {
00208       inliers_ = inliers;
00209       highest_inlier_fraction = inlier_fraction;
00210       converged_ = true;
00211     }
00212   }
00213   
00214   // Start
00215   for (int i = 0; i < max_iterations_; ++i)
00216   {
00217     // Temporary containers
00218     std::vector<int> sample_indices (nr_samples_);
00219     std::vector<int> corresponding_indices (nr_samples_);
00220     
00221     // Draw nr_samples_ random samples
00222     selectSamples (*input_, nr_samples_, sample_indices);
00223 
00224     // Find corresponding features in the target cloud
00225     findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
00226     
00227     // Apply prerejection
00228     if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)){
00229       ++num_rejections;
00230       continue;
00231     }
00232 
00233     // Estimate the transform from the correspondences, write to transformation_
00234     transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
00235     
00236     // Take a backup of previous result
00237     const Matrix4 final_transformation_prev = final_transformation_;
00238     
00239     // Set final result to current transformation
00240     final_transformation_ = transformation_;
00241     
00242     // Transform the input and compute the error (uses input_ and final_transformation_)
00243     getFitness (inliers, error);
00244 
00245     // If the new fit is better, update results
00246     const float inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
00247     if (inlier_fraction > highest_inlier_fraction)
00248     {
00249       inliers_ = inliers;
00250       highest_inlier_fraction = inlier_fraction;
00251       converged_ = true;
00252     }
00253     else
00254     {
00255       // Restore previous result
00256       final_transformation_ = final_transformation_prev;
00257     }
00258   }
00259 
00260   // Apply the final transformation
00261   if (converged_)
00262     transformPointCloud (*input_, output, final_transformation_);
00263   
00264   // Debug output
00265   PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n",
00266             getClassName ().c_str (), num_rejections, max_iterations_);
00267 }
00268 
00270 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00271 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness (std::vector<int>& inliers, float& fitness_score)
00272 {
00273   // Initialize variables
00274   inliers.clear ();
00275   inliers.reserve (input_->size ());
00276   fitness_score = 0.0f;
00277   
00278   // Use squared distance for comparison with NN search results
00279   const float max_range = corr_dist_threshold_ * corr_dist_threshold_;
00280 
00281   // Transform the input dataset using the final transformation
00282   PointCloudSource input_transformed;
00283   input_transformed.resize (input_->size ());
00284   transformPointCloud (*input_, input_transformed, final_transformation_);
00285 
00286   // For each point in the source dataset
00287   for (size_t i = 0; i < input_transformed.points.size (); ++i)
00288   {
00289     // Find its nearest neighbor in the target
00290     std::vector<int> nn_indices (1);
00291     std::vector<float> nn_dists (1);
00292     tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
00293     
00294     // Check if point is an inlier
00295     if (nn_dists[0] < max_range)
00296     {
00297       // Errors
00298       const float dx = input_transformed.points[i].x - target_->points[nn_indices[0]].x;
00299       const float dy = input_transformed.points[i].y - target_->points[nn_indices[0]].y;
00300       const float dz = input_transformed.points[i].z - target_->points[nn_indices[0]].z;
00301       
00302       // Update inliers
00303       inliers.push_back (static_cast<int> (i));
00304       
00305       // Update fitness score
00306       fitness_score += dx*dx + dy*dy + dz*dz;
00307     }
00308   }
00309 
00310   // Calculate MSE
00311   if (inliers.size () > 0)
00312     fitness_score /= static_cast<float> (inliers.size ());
00313   else
00314     fitness_score = std::numeric_limits<float>::max ();
00315 }
00316 
00317 #endif
00318 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:38