Point Cloud Library (PCL)
1.3.1
|
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_