Point Cloud Library (PCL)
1.7.0
|
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 #ifndef IA_RANSAC_H_ 00041 #define IA_RANSAC_H_ 00042 00043 #include <pcl/registration/registration.h> 00044 #include <pcl/registration/transformation_estimation_svd.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief @b SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in 00049 * section IV of "Fast Point Feature Histograms (FPFH) for 3D Registration," Rusu et al. 00050 * \author Michael Dixon, Radu B. Rusu 00051 * \ingroup registration 00052 */ 00053 template <typename PointSource, typename PointTarget, typename FeatureT> 00054 class SampleConsensusInitialAlignment : public Registration<PointSource, PointTarget> 00055 { 00056 public: 00057 using Registration<PointSource, PointTarget>::reg_name_; 00058 using Registration<PointSource, PointTarget>::input_; 00059 using Registration<PointSource, PointTarget>::indices_; 00060 using Registration<PointSource, PointTarget>::target_; 00061 using Registration<PointSource, PointTarget>::final_transformation_; 00062 using Registration<PointSource, PointTarget>::transformation_; 00063 using Registration<PointSource, PointTarget>::corr_dist_threshold_; 00064 using Registration<PointSource, PointTarget>::min_number_correspondences_; 00065 using Registration<PointSource, PointTarget>::max_iterations_; 00066 using Registration<PointSource, PointTarget>::tree_; 00067 using Registration<PointSource, PointTarget>::transformation_estimation_; 00068 using Registration<PointSource, PointTarget>::getClassName; 00069 00070 typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource; 00071 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00072 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00073 00074 typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget; 00075 00076 typedef PointIndices::Ptr PointIndicesPtr; 00077 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00078 00079 typedef pcl::PointCloud<FeatureT> FeatureCloud; 00080 typedef typename FeatureCloud::Ptr FeatureCloudPtr; 00081 typedef typename FeatureCloud::ConstPtr FeatureCloudConstPtr; 00082 00083 typedef boost::shared_ptr<SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT> > Ptr; 00084 typedef boost::shared_ptr<const SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT> > ConstPtr; 00085 00086 00087 class ErrorFunctor 00088 { 00089 public: 00090 virtual ~ErrorFunctor () {} 00091 virtual float operator () (float d) const = 0; 00092 }; 00093 00094 class HuberPenalty : public ErrorFunctor 00095 { 00096 private: 00097 HuberPenalty () {} 00098 public: 00099 HuberPenalty (float threshold) : threshold_ (threshold) {} 00100 virtual float operator () (float e) const 00101 { 00102 if (e <= threshold_) 00103 return (0.5 * e*e); 00104 else 00105 return (0.5 * threshold_ * (2.0 * fabs (e) - threshold_)); 00106 } 00107 protected: 00108 float threshold_; 00109 }; 00110 00111 class TruncatedError : public ErrorFunctor 00112 { 00113 private: 00114 TruncatedError () {} 00115 public: 00116 virtual ~TruncatedError () {} 00117 00118 TruncatedError (float threshold) : threshold_ (threshold) {} 00119 virtual float operator () (float e) const 00120 { 00121 if (e <= threshold_) 00122 return (e / threshold_); 00123 else 00124 return (1.0); 00125 } 00126 protected: 00127 float threshold_; 00128 }; 00129 00130 typedef typename KdTreeFLANN<FeatureT>::Ptr FeatureKdTreePtr; 00131 /** \brief Constructor. */ 00132 SampleConsensusInitialAlignment () : 00133 input_features_ (), target_features_ (), 00134 nr_samples_(3), min_sample_distance_ (0.0f), k_correspondences_ (10), 00135 feature_tree_ (new pcl::KdTreeFLANN<FeatureT>), 00136 error_functor_ () 00137 { 00138 reg_name_ = "SampleConsensusInitialAlignment"; 00139 max_iterations_ = 1000; 00140 00141 // Setting a non-std::numeric_limits<double>::max () value to corr_dist_threshold_ to make it play nicely with TruncatedError 00142 corr_dist_threshold_ = 100.0f; 00143 transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>); 00144 }; 00145 00146 /** \brief Provide a boost shared pointer to the source point cloud's feature descriptors 00147 * \param features the source point cloud's features 00148 */ 00149 void 00150 setSourceFeatures (const FeatureCloudConstPtr &features); 00151 00152 /** \brief Get a pointer to the source point cloud's features */ 00153 inline FeatureCloudConstPtr const 00154 getSourceFeatures () { return (input_features_); } 00155 00156 /** \brief Provide a boost shared pointer to the target point cloud's feature descriptors 00157 * \param features the target point cloud's features 00158 */ 00159 void 00160 setTargetFeatures (const FeatureCloudConstPtr &features); 00161 00162 /** \brief Get a pointer to the target point cloud's features */ 00163 inline FeatureCloudConstPtr const 00164 getTargetFeatures () { return (target_features_); } 00165 00166 /** \brief Set the minimum distances between samples 00167 * \param min_sample_distance the minimum distances between samples 00168 */ 00169 void 00170 setMinSampleDistance (float min_sample_distance) { min_sample_distance_ = min_sample_distance; } 00171 00172 /** \brief Get the minimum distances between samples, as set by the user */ 00173 float 00174 getMinSampleDistance () { return (min_sample_distance_); } 00175 00176 /** \brief Set the number of samples to use during each iteration 00177 * \param nr_samples the number of samples to use during each iteration 00178 */ 00179 void 00180 setNumberOfSamples (int nr_samples) { nr_samples_ = nr_samples; } 00181 00182 /** \brief Get the number of samples to use during each iteration, as set by the user */ 00183 int 00184 getNumberOfSamples () { return (nr_samples_); } 00185 00186 /** \brief Set the number of neighbors to use when selecting a random feature correspondence. A higher value will 00187 * add more randomness to the feature matching. 00188 * \param k the number of neighbors to use when selecting a random feature correspondence. 00189 */ 00190 void 00191 setCorrespondenceRandomness (int k) { k_correspondences_ = k; } 00192 00193 /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */ 00194 int 00195 getCorrespondenceRandomness () { return (k_correspondences_); } 00196 00197 /** \brief Specify the error function to minimize 00198 * \note This call is optional. TruncatedError will be used by default 00199 * \param[in] error_functor a shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor 00200 */ 00201 void 00202 setErrorFunction (const boost::shared_ptr<ErrorFunctor> & error_functor) { error_functor_ = error_functor; } 00203 00204 /** \brief Get a shared pointer to the ErrorFunctor that is to be minimized 00205 * \return A shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor 00206 */ 00207 boost::shared_ptr<ErrorFunctor> 00208 getErrorFunction () { return (error_functor_); } 00209 00210 protected: 00211 /** \brief Choose a random index between 0 and n-1 00212 * \param n the number of possible indices to choose from 00213 */ 00214 inline int 00215 getRandomIndex (int n) { return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0)))); }; 00216 00217 /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are 00218 * greater than a user-defined minimum distance, \a min_sample_distance. 00219 * \param cloud the input point cloud 00220 * \param nr_samples the number of samples to select 00221 * \param min_sample_distance the minimum distance between any two samples 00222 * \param sample_indices the resulting sample indices 00223 */ 00224 void 00225 selectSamples (const PointCloudSource &cloud, int nr_samples, float min_sample_distance, 00226 std::vector<int> &sample_indices); 00227 00228 /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to 00229 * the sample points' features. From these, select one randomly which will be considered that sample point's 00230 * correspondence. 00231 * \param input_features a cloud of feature descriptors 00232 * \param sample_indices the indices of each sample point 00233 * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud 00234 */ 00235 void 00236 findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices, 00237 std::vector<int> &corresponding_indices); 00238 00239 /** \brief An error metric for that computes the quality of the alignment between the given cloud and the target. 00240 * \param cloud the input cloud 00241 * \param threshold distances greater than this value are capped 00242 */ 00243 float 00244 computeErrorMetric (const PointCloudSource &cloud, float threshold); 00245 00246 /** \brief Rigid transformation computation method. 00247 * \param output the transformed input point cloud dataset using the rigid transformation found 00248 */ 00249 virtual void 00250 computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess); 00251 00252 /** \brief The source point cloud's feature descriptors. */ 00253 FeatureCloudConstPtr input_features_; 00254 00255 /** \brief The target point cloud's feature descriptors. */ 00256 FeatureCloudConstPtr target_features_; 00257 00258 /** \brief The number of samples to use during each iteration. */ 00259 int nr_samples_; 00260 00261 /** \brief The minimum distances between samples. */ 00262 float min_sample_distance_; 00263 00264 /** \brief The number of neighbors to use when selecting a random feature correspondence. */ 00265 int k_correspondences_; 00266 00267 /** \brief The KdTree used to compare feature descriptors. */ 00268 FeatureKdTreePtr feature_tree_; 00269 00270 /** */ 00271 boost::shared_ptr<ErrorFunctor> error_functor_; 00272 public: 00273 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00274 }; 00275 } 00276 00277 #include <pcl/registration/impl/ia_ransac.hpp> 00278 00279 #endif //#ifndef IA_RANSAC_H_