Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/sample_consensus_prerejective.h
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_H_
00042 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_H_
00043 
00044 #include <pcl/registration/registration.h>
00045 #include <pcl/registration/transformation_estimation_svd.h>
00046 #include <pcl/registration/transformation_validation.h>
00047 #include <pcl/registration/correspondence_rejection_poly.h>
00048 
00049 namespace pcl
00050 {
00051   /** \brief Pose estimation and alignment class using a prerejective RANSAC routine.
00052    * 
00053    * This class inserts a simple, yet effective "prerejection" step into the standard
00054    * RANSAC pose estimation loop in order to avoid verification of pose hypotheses
00055    * that are likely to be wrong. This is achieved by local pose-invariant geometric
00056    * constraints, as also implemented in the class
00057    * \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly".
00058    * 
00059    * In order to robustly align partial/occluded models, this routine does not
00060    * try to minimize the fit error, but instead tries to maximize the inlier rate,
00061    * above a threshold specifiable using \ref setInlierFraction().
00062    * 
00063    * The amount of prerejection or "greedyness" of the algorithm can be specified
00064    * using \ref setSimilarityThreshold() in [0,1[, where a value of 0 means disabled,
00065    * and 1 is maximally rejective.
00066    * 
00067    * If you use this in academic work, please cite:
00068    * 
00069    * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
00070    * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
00071    * International Conference on Robotics and Automation (ICRA), 2013.
00072    *  
00073    * \author Anders Glent Buch (andersgb1@gmail.com)
00074    * \ingroup registration
00075    */
00076   template <typename PointSource, typename PointTarget, typename FeatureT>
00077   class SampleConsensusPrerejective : public Registration<PointSource, PointTarget>
00078   {
00079     public:
00080       typedef typename Registration<PointSource, PointTarget>::Matrix4 Matrix4;
00081       
00082       using Registration<PointSource, PointTarget>::reg_name_;
00083       using Registration<PointSource, PointTarget>::getClassName;
00084       using Registration<PointSource, PointTarget>::input_;
00085       using Registration<PointSource, PointTarget>::target_;
00086       using Registration<PointSource, PointTarget>::tree_;
00087       using Registration<PointSource, PointTarget>::max_iterations_;
00088       using Registration<PointSource, PointTarget>::corr_dist_threshold_;
00089       using Registration<PointSource, PointTarget>::transformation_;
00090       using Registration<PointSource, PointTarget>::final_transformation_;
00091       using Registration<PointSource, PointTarget>::transformation_estimation_;
00092       using Registration<PointSource, PointTarget>::getFitnessScore;
00093       using Registration<PointSource, PointTarget>::converged_;
00094 
00095       typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
00096       typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00097       typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00098 
00099       typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
00100 
00101       typedef PointIndices::Ptr PointIndicesPtr;
00102       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00103 
00104       typedef pcl::PointCloud<FeatureT> FeatureCloud;
00105       typedef typename FeatureCloud::Ptr FeatureCloudPtr;
00106       typedef typename FeatureCloud::ConstPtr FeatureCloudConstPtr;
00107 
00108       typedef boost::shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > Ptr;
00109       typedef boost::shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > ConstPtr;
00110 
00111       typedef typename KdTreeFLANN<FeatureT>::Ptr FeatureKdTreePtr;
00112       
00113       typedef pcl::registration::CorrespondenceRejectorPoly<PointSource, PointTarget> CorrespondenceRejectorPoly;
00114       typedef typename CorrespondenceRejectorPoly::Ptr CorrespondenceRejectorPolyPtr;
00115       typedef typename CorrespondenceRejectorPoly::ConstPtr CorrespondenceRejectorPolyConstPtr;
00116       
00117       /** \brief Constructor */
00118       SampleConsensusPrerejective ()
00119         : input_features_ ()
00120         , target_features_ ()
00121         , nr_samples_(3)
00122         , k_correspondences_ (2)
00123         , feature_tree_ (new pcl::KdTreeFLANN<FeatureT>)
00124         , correspondence_rejector_poly_ (new CorrespondenceRejectorPoly)
00125         , inlier_fraction_ (0.0f)
00126       {
00127         reg_name_ = "SampleConsensusPrerejective";
00128         correspondence_rejector_poly_->setSimilarityThreshold (0.6f);
00129         max_iterations_ = 5000;
00130         transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
00131       };
00132       
00133       /** \brief Destructor */
00134       virtual ~SampleConsensusPrerejective ()
00135       {
00136       }
00137 
00138       /** \brief Provide a boost shared pointer to the source point cloud's feature descriptors
00139         * \param features the source point cloud's features
00140         */
00141       void 
00142       setSourceFeatures (const FeatureCloudConstPtr &features);
00143 
00144       /** \brief Get a pointer to the source point cloud's features */
00145       inline const FeatureCloudConstPtr
00146       getSourceFeatures () const
00147       { 
00148         return (input_features_);
00149       }
00150 
00151       /** \brief Provide a boost shared pointer to the target point cloud's feature descriptors
00152         * \param features the target point cloud's features
00153         */
00154       void 
00155       setTargetFeatures (const FeatureCloudConstPtr &features);
00156 
00157       /** \brief Get a pointer to the target point cloud's features */
00158       inline const FeatureCloudConstPtr 
00159       getTargetFeatures () const
00160       {
00161         return (target_features_);
00162       }
00163 
00164       /** \brief Set the number of samples to use during each iteration
00165         * \param nr_samples the number of samples to use during each iteration
00166         */
00167       inline void 
00168       setNumberOfSamples (int nr_samples)
00169       {
00170         nr_samples_ = nr_samples;
00171       }
00172 
00173       /** \brief Get the number of samples to use during each iteration, as set by the user */
00174       inline int 
00175       getNumberOfSamples () const
00176       {
00177         return (nr_samples_);
00178       }
00179 
00180       /** \brief Set the number of neighbors to use when selecting a random feature correspondence.  A higher value will
00181         * add more randomness to the feature matching.
00182         * \param k the number of neighbors to use when selecting a random feature correspondence.
00183         */
00184       inline void
00185       setCorrespondenceRandomness (int k)
00186       {
00187         k_correspondences_ = k;
00188       }
00189 
00190       /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */
00191       inline int
00192       getCorrespondenceRandomness () const
00193       {
00194         return (k_correspondences_);
00195       }
00196       
00197       /** \brief Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object,
00198        * where 1 is a perfect match
00199        * \param similarity_threshold edge length similarity threshold
00200        */
00201       inline void
00202       setSimilarityThreshold (float similarity_threshold)
00203       {
00204         correspondence_rejector_poly_->setSimilarityThreshold (similarity_threshold);
00205       }
00206       
00207       /** \brief Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector object,
00208        * \return edge length similarity threshold
00209        */
00210       inline float
00211       getSimilarityThreshold () const
00212       {
00213         return correspondence_rejector_poly_->getSimilarityThreshold ();
00214       }
00215       
00216       /** \brief Set the required inlier fraction (of the input)
00217        * \param inlier_fraction required inlier fraction, must be in [0,1]
00218        */
00219       inline void
00220       setInlierFraction (float inlier_fraction)
00221       {
00222         inlier_fraction_ = inlier_fraction;
00223       }
00224       
00225       /** \brief Get the required inlier fraction
00226        * \return required inlier fraction in [0,1]
00227        */
00228       inline float
00229       getInlierFraction () const
00230       {
00231         return inlier_fraction_;
00232       }
00233       
00234       /** \brief Get the inlier indices of the source point cloud under the final transformation
00235        * @return inlier indices
00236        */
00237       inline const std::vector<int>&
00238       getInliers () const
00239       {
00240         return inliers_;
00241       }
00242 
00243     protected:
00244       /** \brief Choose a random index between 0 and n-1
00245         * \param n the number of possible indices to choose from
00246         */
00247       inline int 
00248       getRandomIndex (int n) const
00249       {
00250         return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0))));
00251       };
00252       
00253       /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are 
00254         * greater than a user-defined minimum distance, \a min_sample_distance.
00255         * \param cloud the input point cloud
00256         * \param nr_samples the number of samples to select
00257         * \param sample_indices the resulting sample indices
00258         */
00259       void 
00260       selectSamples (const PointCloudSource &cloud, int nr_samples, 
00261                      std::vector<int> &sample_indices);
00262 
00263       /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to 
00264         * the sample points' features. From these, select one randomly which will be considered that sample point's 
00265         * correspondence. 
00266         * \param input_features a cloud of feature descriptors
00267         * \param sample_indices the indices of each sample point
00268         * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud
00269         */
00270       void 
00271       findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices, 
00272                            std::vector<int> &corresponding_indices);
00273 
00274       /** \brief Rigid transformation computation method.
00275         * \param output the transformed input point cloud dataset using the rigid transformation found
00276         */
00277       void 
00278       computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess);
00279 
00280       /** \brief Obtain the fitness of a transformation
00281         * The following metrics are calculated, based on
00282         * \b final_transformation_ and \b corr_dist_threshold_:
00283         *   - Inliers: the number of transformed points which are closer than threshold to NN
00284         *   - Error score: the MSE of the inliers  
00285         * \param inliers indices of source point cloud inliers
00286         * \param fitness_score output fitness score as RMSE 
00287         */
00288       void 
00289       getFitness (std::vector<int>& inliers, float& fitness_score);
00290 
00291       /** \brief The source point cloud's feature descriptors. */
00292       FeatureCloudConstPtr input_features_;
00293 
00294       /** \brief The target point cloud's feature descriptors. */
00295       FeatureCloudConstPtr target_features_;  
00296 
00297       /** \brief The number of samples to use during each iteration. */
00298       int nr_samples_;
00299 
00300       /** \brief The number of neighbors to use when selecting a random feature correspondence. */
00301       int k_correspondences_;
00302      
00303       /** \brief The KdTree used to compare feature descriptors. */
00304       FeatureKdTreePtr feature_tree_;
00305       
00306       /** \brief The polygonal correspondence rejector used for prerejection */
00307       CorrespondenceRejectorPolyPtr correspondence_rejector_poly_;
00308       
00309       /** \brief The fraction [0,1] of inlier points required for accepting a transformation */
00310       float inlier_fraction_;
00311       
00312       /** \brief Inlier points of final transformation as indices into source */
00313       std::vector<int> inliers_;
00314   };
00315 }
00316 
00317 #include <pcl/registration/impl/sample_consensus_prerejective.hpp>
00318 
00319 #endif