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 * $Id: sac_model.h 3027 2011-11-01 04:04:27Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_ 00041 #define PCL_SAMPLE_CONSENSUS_MODEL_H_ 00042 00043 #include <cfloat> 00044 #include <limits.h> 00045 #include <set> 00046 00047 #include <pcl/console/print.h> 00048 #include <pcl/point_cloud.h> 00049 #include "pcl/sample_consensus/model_types.h" 00050 00051 namespace pcl 00052 { 00053 // Forward declaration 00054 #ifndef __ANDROID__ 00055 template<class T> class ProgressiveSampleConsensus; 00056 #endif 00057 00063 template <typename PointT> 00064 class SampleConsensusModel 00065 { 00066 public: 00067 typedef typename pcl::PointCloud<PointT> PointCloud; 00068 typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr; 00069 typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr; 00070 00071 typedef boost::shared_ptr<SampleConsensusModel> Ptr; 00072 typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr; 00073 00074 private: 00076 SampleConsensusModel () : radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX) {}; 00077 00078 public: 00082 SampleConsensusModel (const PointCloudConstPtr &cloud) : 00083 radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX) 00084 { 00085 // Sets the input cloud and creates a vector of "fake" indices 00086 setInputCloud (cloud); 00087 } 00088 00093 SampleConsensusModel (const PointCloudConstPtr &cloud, const std::vector<int> &indices) : 00094 input_ (cloud), 00095 radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX) 00096 00097 { 00098 indices_.reset (new std::vector<int> (indices)); 00099 if (indices_->size () > input_->points.size ()) 00100 { 00101 PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n", (unsigned long)indices_->size (), (unsigned long)input_->points.size ()); 00102 indices_->clear (); 00103 } 00104 shuffled_indices_ = *indices_; 00105 }; 00106 00108 virtual ~SampleConsensusModel () {}; 00109 00115 void 00116 getSamples (int &iterations, std::vector<int> &samples) 00117 { 00118 // We're assuming that indices_ have already been set in the constructor 00119 if (indices_->size () < getSampleSize ()) 00120 { 00121 PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n", 00122 (unsigned long)samples.size (), (unsigned long)indices_->size ()); 00123 // one of these will make it stop :) 00124 samples.clear (); 00125 iterations = INT_MAX - 1; 00126 return; 00127 } 00128 00129 // Get a second point which is different than the first 00130 samples.resize (getSampleSize()); 00131 for (unsigned int iter = 0; iter < max_sample_checks_; ++iter) 00132 { 00133 // Choose the random indices 00134 SampleConsensusModel<PointT>::drawIndexSample (samples); 00135 00136 // If it's a good sample, stop here 00137 if (isSampleGood (samples)) 00138 return; 00139 } 00140 PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_); 00141 samples.clear (); 00142 } 00143 00151 virtual bool 00152 computeModelCoefficients (const std::vector<int> &samples, 00153 Eigen::VectorXf &model_coefficients) = 0; 00154 00165 virtual void 00166 optimizeModelCoefficients (const std::vector<int> &inliers, 00167 const Eigen::VectorXf &model_coefficients, 00168 Eigen::VectorXf &optimized_coefficients) = 0; 00169 00175 virtual void 00176 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00177 std::vector<double> &distances) = 0; 00178 00187 virtual void 00188 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00189 const double threshold, 00190 std::vector<int> &inliers) = 0; 00191 00201 virtual int 00202 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00203 const double threshold) = 0; 00204 00213 virtual void 00214 projectPoints (const std::vector<int> &inliers, 00215 const Eigen::VectorXf &model_coefficients, 00216 PointCloud &projected_points, 00217 bool copy_data_fields = true) = 0; 00218 00227 virtual bool 00228 doSamplesVerifyModel (const std::set<int> &indices, 00229 const Eigen::VectorXf &model_coefficients, 00230 const double threshold) = 0; 00231 00235 inline virtual void 00236 setInputCloud (const PointCloudConstPtr &cloud) 00237 { 00238 input_ = cloud; 00239 if (!indices_) 00240 indices_.reset (new std::vector<int> ()); 00241 if (indices_->empty ()) 00242 { 00243 // Prepare a set of indices to be used (entire cloud) 00244 indices_->resize (cloud->points.size ()); 00245 for (size_t i = 0; i < cloud->points.size (); ++i) 00246 (*indices_)[i] = (int) i; 00247 } 00248 shuffled_indices_ = *indices_; 00249 } 00250 00252 inline PointCloudConstPtr 00253 getInputCloud () const { return (input_); } 00254 00258 inline void 00259 setIndices (const boost::shared_ptr <std::vector<int> > &indices) 00260 { 00261 indices_ = indices; 00262 shuffled_indices_ = *indices_; 00263 } 00264 00268 inline void 00269 setIndices (const std::vector<int> &indices) 00270 { 00271 indices_.reset (new std::vector<int> (indices)); 00272 shuffled_indices_ = indices; 00273 } 00274 00276 inline boost::shared_ptr <std::vector<int> > 00277 getIndices () const { return (indices_); } 00278 00280 virtual SacModel 00281 getModelType () const = 0; 00282 00284 inline unsigned int 00285 getSampleSize () const 00286 { 00287 std::map<pcl::SacModel, unsigned int>::const_iterator it = SAC_SAMPLE_SIZE.find (getModelType ()); 00288 if (it == SAC_SAMPLE_SIZE.end ()) 00289 throw InvalidSACModelTypeException ("No sample size defined for given model type!\n"); 00290 return (it->second); 00291 } 00292 00299 inline void 00300 setRadiusLimits (const double &min_radius, const double &max_radius) 00301 { 00302 radius_min_ = min_radius; 00303 radius_max_ = max_radius; 00304 } 00305 00312 inline void 00313 getRadiusLimits (double &min_radius, double &max_radius) 00314 { 00315 min_radius = radius_min_; 00316 max_radius = radius_max_; 00317 } 00318 #ifndef __ANDROID__ 00319 friend class ProgressiveSampleConsensus<PointT>; 00320 #endif 00321 protected: 00325 inline void 00326 drawIndexSample (std::vector<int> &sample) 00327 { 00328 size_t sample_size = sample.size (); 00329 size_t index_size = shuffled_indices_.size (); 00330 for (unsigned int i = 0; i < sample_size; ++i) 00331 // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo 00332 // elements, that does not matter (and nowadays, random number generators are good) 00333 std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]); 00334 std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ()); 00335 } 00336 00340 virtual inline bool 00341 isModelValid (const Eigen::VectorXf &model_coefficients) = 0; 00342 00347 virtual bool 00348 isSampleGood (const std::vector<int> &samples) const = 0; 00349 00351 PointCloudConstPtr input_; 00352 00354 boost::shared_ptr <std::vector<int> > indices_; 00355 00357 static const unsigned int max_sample_checks_ = 1000; 00358 00362 double radius_min_, radius_max_; 00363 00365 std::vector<int> shuffled_indices_; 00366 }; 00367 00371 template <typename PointT, typename PointNT> 00372 class SampleConsensusModelFromNormals 00373 { 00374 public: 00375 typedef typename pcl::PointCloud<PointNT>::ConstPtr PointCloudNConstPtr; 00376 typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudNPtr; 00377 00378 typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr; 00379 typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr; 00380 00382 SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0) {}; 00383 00389 inline void 00390 setNormalDistanceWeight (const double w) 00391 { 00392 normal_distance_weight_ = w; 00393 } 00394 00396 inline double 00397 getNormalDistanceWeight () { return (normal_distance_weight_); } 00398 00404 inline void 00405 setInputNormals (const PointCloudNConstPtr &normals) 00406 { 00407 normals_ = normals; 00408 } 00409 00411 inline PointCloudNConstPtr 00412 getInputNormals () { return (normals_); } 00413 00414 protected: 00418 double normal_distance_weight_; 00419 00423 PointCloudNConstPtr normals_; 00424 }; 00425 00430 template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic> 00431 struct Functor 00432 { 00433 typedef _Scalar Scalar; 00434 enum { 00435 InputsAtCompileTime = NX, 00436 ValuesAtCompileTime = NY 00437 }; 00438 typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType; 00439 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType; 00440 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 00441 00442 const int m_inputs, m_values; 00443 00444 Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 00445 Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 00446 00447 int inputs() const { return m_inputs; } 00448 int values() const { return m_values; } 00449 }; 00450 } 00451 00452 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_