Point Cloud Library (PCL)  1.3.1
ia_ransac.h
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-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  *
00037  */
00038 #ifndef IA_RANSAC_H_
00039 #define IA_RANSAC_H_
00040 
00041 #include "pcl/registration/registration.h"
00042 #include "pcl/registration/transformation_estimation_svd.h"
00043 
00044 namespace pcl
00045 {
00047 
00052   template <typename PointSource, typename PointTarget, typename FeatureT>
00053   class SampleConsensusInitialAlignment : public Registration<PointSource, PointTarget>
00054   {
00055     public:
00056       using Registration<PointSource, PointTarget>::reg_name_;
00057       using Registration<PointSource, PointTarget>::input_;
00058       using Registration<PointSource, PointTarget>::indices_;
00059       using Registration<PointSource, PointTarget>::target_;
00060       using Registration<PointSource, PointTarget>::final_transformation_;
00061       using Registration<PointSource, PointTarget>::transformation_;
00062       using Registration<PointSource, PointTarget>::corr_dist_threshold_;
00063       using Registration<PointSource, PointTarget>::min_number_correspondences_;
00064       using Registration<PointSource, PointTarget>::max_iterations_;
00065       using Registration<PointSource, PointTarget>::tree_;
00066       using Registration<PointSource, PointTarget>::transformation_estimation_;
00067       using Registration<PointSource, PointTarget>::getClassName;
00068 
00069       typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
00070       typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00071       typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00072 
00073       typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
00074 
00075       typedef PointIndices::Ptr PointIndicesPtr;
00076       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00077 
00078       typedef pcl::PointCloud<FeatureT> FeatureCloud;
00079       typedef typename FeatureCloud::Ptr FeatureCloudPtr;
00080       typedef typename FeatureCloud::ConstPtr FeatureCloudConstPtr;
00081 
00082 
00083       class ErrorFunctor
00084       {
00085         public:
00086           virtual float operator () (float d) const = 0;
00087       };
00088 
00089       class HuberPenalty : public ErrorFunctor
00090       {
00091         private:
00092           HuberPenalty () {}
00093         public:
00094           HuberPenalty (float threshold)  : threshold_ (threshold) {}
00095           virtual float operator () (float e) const
00096           { 
00097             if (e <= threshold_)
00098               return (0.5 * e*e); 
00099             else
00100               return (0.5 * threshold_ * (2.0 * fabs (e) - threshold_));
00101           }
00102         protected:
00103           float threshold_;
00104       };
00105 
00106       class TruncatedError : public ErrorFunctor
00107       {
00108         private:
00109           TruncatedError () {}
00110         public:
00111           TruncatedError (float threshold) : threshold_ (threshold) {}
00112           virtual float operator () (float e) const
00113           { 
00114             if (e <= threshold_)
00115               return (e / threshold_);
00116             else
00117               return (1.0);
00118           }
00119         protected:
00120           float threshold_;
00121       };
00122 
00123       typedef typename KdTreeFLANN<FeatureT>::Ptr FeatureKdTreePtr; 
00125       SampleConsensusInitialAlignment () : nr_samples_(3), min_sample_distance_ (0), k_correspondences_ (10)
00126       {
00127         reg_name_ = "SampleConsensusInitialAlignment";
00128         feature_tree_.reset (new pcl::KdTreeFLANN<FeatureT>);
00129         max_iterations_ = 1000;
00130         transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
00131       };
00132 
00136       void 
00137       setSourceFeatures (const FeatureCloudConstPtr &features);
00138 
00140       inline FeatureCloudConstPtr const 
00141       getSourceFeatures () { return (input_features_); }
00142 
00146       void 
00147       setTargetFeatures (const FeatureCloudConstPtr &features);
00148 
00150       inline FeatureCloudConstPtr const 
00151       getTargetFeatures () { return (target_features_); }
00152 
00156       void 
00157       setMinSampleDistance (float min_sample_distance) { min_sample_distance_ = min_sample_distance; }
00158 
00160       float 
00161       getMinSampleDistance () { return (min_sample_distance_); }
00162 
00166       void 
00167       setNumberOfSamples (int nr_samples) { nr_samples_ = nr_samples; }
00168 
00170       int 
00171       getNumberOfSamples () { return (nr_samples_); }
00172 
00177       void
00178       setCorrespondenceRandomness (int k) { k_correspondences_ = k; }
00179 
00181       int
00182       getCorrespondenceRandomness () { return (k_correspondences_); }
00183 
00188       void
00189       setErrorFunction (const boost::shared_ptr<ErrorFunctor> & error_functor) { error_functor_ = error_functor; }
00190 
00194       boost::shared_ptr<ErrorFunctor>
00195       getErrorFunction () { return (error_functor_); }
00196 
00197     protected:
00201       inline int 
00202       getRandomIndex (int n) { return (n * (rand () / (RAND_MAX + 1.0))); };
00203       
00211       void 
00212       selectSamples (const PointCloudSource &cloud, int nr_samples, float min_sample_distance, 
00213                      std::vector<int> &sample_indices);
00214 
00222       void 
00223       findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices, 
00224                            std::vector<int> &corresponding_indices);
00225 
00230       float 
00231       computeErrorMetric (const PointCloudSource &cloud, float threshold);
00232 
00236       virtual void 
00237       computeTransformation (PointCloudSource &output);
00238 
00240       FeatureCloudConstPtr input_features_;
00241 
00243       FeatureCloudConstPtr target_features_;  
00244 
00246       int nr_samples_;
00247 
00249       float min_sample_distance_;
00250 
00252       int k_correspondences_;
00253      
00255       FeatureKdTreePtr feature_tree_;               
00256 
00258       boost::shared_ptr<ErrorFunctor> error_functor_;
00259 
00260   };
00261 }
00262 
00263 #include "pcl/registration/impl/ia_ransac.hpp"
00264 
00265 #endif  //#ifndef IA_RANSAC_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines