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-2011, 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_SAMPLE_CONSENSUS_MODEL_H_ 00042 #define PCL_SAMPLE_CONSENSUS_MODEL_H_ 00043 00044 #include <cfloat> 00045 #include <ctime> 00046 #include <limits.h> 00047 #include <set> 00048 00049 #include <pcl/console/print.h> 00050 #include <pcl/point_cloud.h> 00051 #include <pcl/sample_consensus/boost.h> 00052 #include <pcl/sample_consensus/model_types.h> 00053 00054 #include <pcl/search/search.h> 00055 00056 namespace pcl 00057 { 00058 template<class T> class ProgressiveSampleConsensus; 00059 00060 /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit 00061 * from this class. 00062 * \author Radu B. Rusu 00063 * \ingroup sample_consensus 00064 */ 00065 template <typename PointT> 00066 class SampleConsensusModel 00067 { 00068 public: 00069 typedef typename pcl::PointCloud<PointT> PointCloud; 00070 typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr; 00071 typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr; 00072 typedef typename pcl::search::Search<PointT>::Ptr SearchPtr; 00073 00074 typedef boost::shared_ptr<SampleConsensusModel> Ptr; 00075 typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr; 00076 00077 protected: 00078 /** \brief Empty constructor for base SampleConsensusModel. 00079 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00080 */ 00081 SampleConsensusModel (bool random = false) 00082 : input_ () 00083 , indices_ () 00084 , radius_min_ (-std::numeric_limits<double>::max ()) 00085 , radius_max_ (std::numeric_limits<double>::max ()) 00086 , samples_radius_ (0.) 00087 , samples_radius_search_ () 00088 , shuffled_indices_ () 00089 , rng_alg_ () 00090 , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())) 00091 , rng_gen_ () 00092 , error_sqr_dists_ () 00093 { 00094 // Create a random number generator object 00095 if (random) 00096 rng_alg_.seed (static_cast<unsigned> (std::time(0))); 00097 else 00098 rng_alg_.seed (12345u); 00099 00100 rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 00101 } 00102 00103 public: 00104 /** \brief Constructor for base SampleConsensusModel. 00105 * \param[in] cloud the input point cloud dataset 00106 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00107 */ 00108 SampleConsensusModel (const PointCloudConstPtr &cloud, bool random = false) 00109 : input_ () 00110 , indices_ () 00111 , radius_min_ (-std::numeric_limits<double>::max ()) 00112 , radius_max_ (std::numeric_limits<double>::max ()) 00113 , samples_radius_ (0.) 00114 , samples_radius_search_ () 00115 , shuffled_indices_ () 00116 , rng_alg_ () 00117 , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())) 00118 , rng_gen_ () 00119 , error_sqr_dists_ () 00120 { 00121 if (random) 00122 rng_alg_.seed (static_cast<unsigned> (std::time (0))); 00123 else 00124 rng_alg_.seed (12345u); 00125 00126 // Sets the input cloud and creates a vector of "fake" indices 00127 setInputCloud (cloud); 00128 00129 // Create a random number generator object 00130 rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 00131 } 00132 00133 /** \brief Constructor for base SampleConsensusModel. 00134 * \param[in] cloud the input point cloud dataset 00135 * \param[in] indices a vector of point indices to be used from \a cloud 00136 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00137 */ 00138 SampleConsensusModel (const PointCloudConstPtr &cloud, 00139 const std::vector<int> &indices, 00140 bool random = false) 00141 : input_ (cloud) 00142 , indices_ (new std::vector<int> (indices)) 00143 , radius_min_ (-std::numeric_limits<double>::max ()) 00144 , radius_max_ (std::numeric_limits<double>::max ()) 00145 , samples_radius_ (0.) 00146 , samples_radius_search_ () 00147 , shuffled_indices_ () 00148 , rng_alg_ () 00149 , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())) 00150 , rng_gen_ () 00151 , error_sqr_dists_ () 00152 { 00153 if (random) 00154 rng_alg_.seed (static_cast<unsigned> (std::time(0))); 00155 else 00156 rng_alg_.seed (12345u); 00157 00158 if (indices_->size () > input_->points.size ()) 00159 { 00160 PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %zu while the input PointCloud has size %zu!\n", indices_->size (), input_->points.size ()); 00161 indices_->clear (); 00162 } 00163 shuffled_indices_ = *indices_; 00164 00165 // Create a random number generator object 00166 rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 00167 }; 00168 00169 /** \brief Destructor for base SampleConsensusModel. */ 00170 virtual ~SampleConsensusModel () {}; 00171 00172 /** \brief Get a set of random data samples and return them as point 00173 * indices. 00174 * \param[out] iterations the internal number of iterations used by SAC methods 00175 * \param[out] samples the resultant model samples 00176 */ 00177 virtual void 00178 getSamples (int &iterations, std::vector<int> &samples) 00179 { 00180 // We're assuming that indices_ have already been set in the constructor 00181 if (indices_->size () < getSampleSize ()) 00182 { 00183 PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!\n", 00184 samples.size (), indices_->size ()); 00185 // one of these will make it stop :) 00186 samples.clear (); 00187 iterations = INT_MAX - 1; 00188 return; 00189 } 00190 00191 // Get a second point which is different than the first 00192 samples.resize (getSampleSize ()); 00193 for (unsigned int iter = 0; iter < max_sample_checks_; ++iter) 00194 { 00195 // Choose the random indices 00196 if (samples_radius_ < std::numeric_limits<double>::epsilon ()) 00197 SampleConsensusModel<PointT>::drawIndexSample (samples); 00198 else 00199 SampleConsensusModel<PointT>::drawIndexSampleRadius (samples); 00200 00201 // If it's a good sample, stop here 00202 if (isSampleGood (samples)) 00203 { 00204 PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %zu samples.\n", samples.size ()); 00205 return; 00206 } 00207 } 00208 PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_); 00209 samples.clear (); 00210 } 00211 00212 /** \brief Check whether the given index samples can form a valid model, 00213 * compute the model coefficients from these samples and store them 00214 * in model_coefficients. Pure virtual. 00215 * \param[in] samples the point indices found as possible good candidates 00216 * for creating a valid model 00217 * \param[out] model_coefficients the computed model coefficients 00218 */ 00219 virtual bool 00220 computeModelCoefficients (const std::vector<int> &samples, 00221 Eigen::VectorXf &model_coefficients) = 0; 00222 00223 /** \brief Recompute the model coefficients using the given inlier set 00224 * and return them to the user. Pure virtual. 00225 * 00226 * @note: these are the coefficients of the model after refinement 00227 * (e.g., after a least-squares optimization) 00228 * 00229 * \param[in] inliers the data inliers supporting the model 00230 * \param[in] model_coefficients the initial guess for the model coefficients 00231 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization 00232 */ 00233 virtual void 00234 optimizeModelCoefficients (const std::vector<int> &inliers, 00235 const Eigen::VectorXf &model_coefficients, 00236 Eigen::VectorXf &optimized_coefficients) = 0; 00237 00238 /** \brief Compute all distances from the cloud data to a given model. Pure virtual. 00239 * 00240 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00241 * \param[out] distances the resultant estimated distances 00242 */ 00243 virtual void 00244 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00245 std::vector<double> &distances) = 0; 00246 00247 /** \brief Select all the points which respect the given model 00248 * coefficients as inliers. Pure virtual. 00249 * 00250 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00251 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from 00252 * the outliers 00253 * \param[out] inliers the resultant model inliers 00254 */ 00255 virtual void 00256 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00257 const double threshold, 00258 std::vector<int> &inliers) = 0; 00259 00260 /** \brief Count all the points which respect the given model 00261 * coefficients as inliers. Pure virtual. 00262 * 00263 * \param[in] model_coefficients the coefficients of a model that we need to 00264 * compute distances to 00265 * \param[in] threshold a maximum admissible distance threshold for 00266 * determining the inliers from the outliers 00267 * \return the resultant number of inliers 00268 */ 00269 virtual int 00270 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00271 const double threshold) = 0; 00272 00273 /** \brief Create a new point cloud with inliers projected onto the model. Pure virtual. 00274 * \param[in] inliers the data inliers that we want to project on the model 00275 * \param[in] model_coefficients the coefficients of a model 00276 * \param[out] projected_points the resultant projected points 00277 * \param[in] copy_data_fields set to true (default) if we want the \a 00278 * projected_points cloud to be an exact copy of the input dataset minus 00279 * the point projections on the plane model 00280 */ 00281 virtual void 00282 projectPoints (const std::vector<int> &inliers, 00283 const Eigen::VectorXf &model_coefficients, 00284 PointCloud &projected_points, 00285 bool copy_data_fields = true) = 0; 00286 00287 /** \brief Verify whether a subset of indices verifies a given set of 00288 * model coefficients. Pure virtual. 00289 * 00290 * \param[in] indices the data indices that need to be tested against the model 00291 * \param[in] model_coefficients the set of model coefficients 00292 * \param[in] threshold a maximum admissible distance threshold for 00293 * determining the inliers from the outliers 00294 */ 00295 virtual bool 00296 doSamplesVerifyModel (const std::set<int> &indices, 00297 const Eigen::VectorXf &model_coefficients, 00298 const double threshold) = 0; 00299 00300 /** \brief Provide a pointer to the input dataset 00301 * \param[in] cloud the const boost shared pointer to a PointCloud message 00302 */ 00303 inline virtual void 00304 setInputCloud (const PointCloudConstPtr &cloud) 00305 { 00306 input_ = cloud; 00307 if (!indices_) 00308 indices_.reset (new std::vector<int> ()); 00309 if (indices_->empty ()) 00310 { 00311 // Prepare a set of indices to be used (entire cloud) 00312 indices_->resize (cloud->points.size ()); 00313 for (size_t i = 0; i < cloud->points.size (); ++i) 00314 (*indices_)[i] = static_cast<int> (i); 00315 } 00316 shuffled_indices_ = *indices_; 00317 } 00318 00319 /** \brief Get a pointer to the input point cloud dataset. */ 00320 inline PointCloudConstPtr 00321 getInputCloud () const { return (input_); } 00322 00323 /** \brief Provide a pointer to the vector of indices that represents the input data. 00324 * \param[in] indices a pointer to the vector of indices that represents the input data. 00325 */ 00326 inline void 00327 setIndices (const boost::shared_ptr <std::vector<int> > &indices) 00328 { 00329 indices_ = indices; 00330 shuffled_indices_ = *indices_; 00331 } 00332 00333 /** \brief Provide the vector of indices that represents the input data. 00334 * \param[out] indices the vector of indices that represents the input data. 00335 */ 00336 inline void 00337 setIndices (const std::vector<int> &indices) 00338 { 00339 indices_.reset (new std::vector<int> (indices)); 00340 shuffled_indices_ = indices; 00341 } 00342 00343 /** \brief Get a pointer to the vector of indices used. */ 00344 inline boost::shared_ptr <std::vector<int> > 00345 getIndices () const { return (indices_); } 00346 00347 /** \brief Return an unique id for each type of model employed. */ 00348 virtual SacModel 00349 getModelType () const = 0; 00350 00351 /** \brief Return the size of a sample from which a model is computed */ 00352 inline unsigned int 00353 getSampleSize () const 00354 { 00355 std::map<pcl::SacModel, unsigned int>::const_iterator it = SAC_SAMPLE_SIZE.find (getModelType ()); 00356 if (it == SAC_SAMPLE_SIZE.end ()) 00357 throw InvalidSACModelTypeException ("No sample size defined for given model type!\n"); 00358 return (it->second); 00359 } 00360 00361 /** \brief Set the minimum and maximum allowable radius limits for the 00362 * model (applicable to models that estimate a radius) 00363 * \param[in] min_radius the minimum radius model 00364 * \param[in] max_radius the maximum radius model 00365 * \todo change this to set limits on the entire model 00366 */ 00367 inline void 00368 setRadiusLimits (const double &min_radius, const double &max_radius) 00369 { 00370 radius_min_ = min_radius; 00371 radius_max_ = max_radius; 00372 } 00373 00374 /** \brief Get the minimum and maximum allowable radius limits for the 00375 * model as set by the user. 00376 * 00377 * \param[out] min_radius the resultant minimum radius model 00378 * \param[out] max_radius the resultant maximum radius model 00379 */ 00380 inline void 00381 getRadiusLimits (double &min_radius, double &max_radius) 00382 { 00383 min_radius = radius_min_; 00384 max_radius = radius_max_; 00385 } 00386 00387 /** \brief Set the maximum distance allowed when drawing random samples 00388 * \param[in] radius the maximum distance (L2 norm) 00389 */ 00390 inline void 00391 setSamplesMaxDist (const double &radius, SearchPtr search) 00392 { 00393 samples_radius_ = radius; 00394 samples_radius_search_ = search; 00395 } 00396 00397 /** \brief Get maximum distance allowed when drawing random samples 00398 * 00399 * \param[out] radius the maximum distance (L2 norm) 00400 */ 00401 inline void 00402 getSamplesMaxDist (double &radius) 00403 { 00404 radius = samples_radius_; 00405 } 00406 00407 friend class ProgressiveSampleConsensus<PointT>; 00408 00409 /** \brief Compute the variance of the errors to the model. 00410 * \param[in] error_sqr_dists a vector holding the distances 00411 */ 00412 inline double 00413 computeVariance (const std::vector<double> &error_sqr_dists) 00414 { 00415 std::vector<double> dists (error_sqr_dists); 00416 std::sort (dists.begin (), dists.end ()); 00417 double median_error_sqr = dists[dists.size () >> 1]; 00418 return (2.1981 * median_error_sqr); 00419 } 00420 00421 /** \brief Compute the variance of the errors to the model from the internally 00422 * estimated vector of distances. The model must be computed first (or at least 00423 * selectWithinDistance must be called). 00424 */ 00425 inline double 00426 computeVariance () 00427 { 00428 if (error_sqr_dists_.empty ()) 00429 { 00430 PCL_ERROR ("[pcl::SampleConsensusModel::computeVariance] The variance of the Sample Consensus model distances cannot be estimated, as the model has not been computed yet. Please compute the model first or at least run selectWithinDistance before continuing. Returning NAN!\n"); 00431 return (std::numeric_limits<double>::quiet_NaN ()); 00432 } 00433 return (computeVariance (error_sqr_dists_)); 00434 } 00435 00436 protected: 00437 /** \brief Fills a sample array with random samples from the indices_ vector 00438 * \param[out] sample the set of indices of target_ to analyze 00439 */ 00440 inline void 00441 drawIndexSample (std::vector<int> &sample) 00442 { 00443 size_t sample_size = sample.size (); 00444 size_t index_size = shuffled_indices_.size (); 00445 for (unsigned int i = 0; i < sample_size; ++i) 00446 // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo 00447 // elements, that does not matter (and nowadays, random number generators are good) 00448 //std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]); 00449 std::swap (shuffled_indices_[i], shuffled_indices_[i + (rnd () % (index_size - i))]); 00450 std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ()); 00451 } 00452 00453 /** \brief Fills a sample array with one random sample from the indices_ vector 00454 * and other random samples that are closer than samples_radius_ 00455 * \param[out] sample the set of indices of target_ to analyze 00456 */ 00457 inline void 00458 drawIndexSampleRadius (std::vector<int> &sample) 00459 { 00460 size_t sample_size = sample.size (); 00461 size_t index_size = shuffled_indices_.size (); 00462 00463 std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]); 00464 //const PointT& pt0 = (*input_)[shuffled_indices_[0]]; 00465 00466 std::vector<int> indices; 00467 std::vector<float> sqr_dists; 00468 00469 // If indices have been set when the search object was constructed, 00470 // radiusSearch() expects an index into the indices vector as its 00471 // first parameter. This can't be determined efficiently, so we use 00472 // the point instead of the index. 00473 // Returned indices are converted automatically. 00474 samples_radius_search_->radiusSearch (input_->at(shuffled_indices_[0]), 00475 samples_radius_, indices, sqr_dists ); 00476 00477 if (indices.size () < sample_size - 1) 00478 { 00479 // radius search failed, make an invalid model 00480 for(unsigned int i = 1; i < sample_size; ++i) 00481 shuffled_indices_[i] = shuffled_indices_[0]; 00482 } 00483 else 00484 { 00485 for (unsigned int i = 0; i < sample_size-1; ++i) 00486 std::swap (indices[i], indices[i + (rnd () % (indices.size () - i))]); 00487 for (unsigned int i = 1; i < sample_size; ++i) 00488 shuffled_indices_[i] = indices[i-1]; 00489 } 00490 00491 std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ()); 00492 } 00493 00494 /** \brief Check whether a model is valid given the user constraints. 00495 * \param[in] model_coefficients the set of model coefficients 00496 */ 00497 virtual inline bool 00498 isModelValid (const Eigen::VectorXf &model_coefficients) = 0; 00499 00500 /** \brief Check if a sample of indices results in a good sample of points 00501 * indices. Pure virtual. 00502 * \param[in] samples the resultant index samples 00503 */ 00504 virtual bool 00505 isSampleGood (const std::vector<int> &samples) const = 0; 00506 00507 /** \brief A boost shared pointer to the point cloud data array. */ 00508 PointCloudConstPtr input_; 00509 00510 /** \brief A pointer to the vector of point indices to use. */ 00511 boost::shared_ptr <std::vector<int> > indices_; 00512 00513 /** The maximum number of samples to try until we get a good one */ 00514 static const unsigned int max_sample_checks_ = 1000; 00515 00516 /** \brief The minimum and maximum radius limits for the model. 00517 * Applicable to all models that estimate a radius. 00518 */ 00519 double radius_min_, radius_max_; 00520 00521 /** \brief The maximum distance of subsequent samples from the first (radius search) */ 00522 double samples_radius_; 00523 00524 /** \brief The search object for picking subsequent samples using radius search */ 00525 SearchPtr samples_radius_search_; 00526 00527 /** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */ 00528 std::vector<int> shuffled_indices_; 00529 00530 /** \brief Boost-based random number generator algorithm. */ 00531 boost::mt19937 rng_alg_; 00532 00533 /** \brief Boost-based random number generator distribution. */ 00534 boost::shared_ptr<boost::uniform_int<> > rng_dist_; 00535 00536 /** \brief Boost-based random number generator. */ 00537 boost::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > > rng_gen_; 00538 00539 /** \brief A vector holding the distances to the computed model. Used internally. */ 00540 std::vector<double> error_sqr_dists_; 00541 00542 /** \brief Boost-based random number generator. */ 00543 inline int 00544 rnd () 00545 { 00546 return ((*rng_gen_) ()); 00547 } 00548 public: 00549 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00550 }; 00551 00552 /** \brief @b SampleConsensusModelFromNormals represents the base model class 00553 * for models that require the use of surface normals for estimation. 00554 */ 00555 template <typename PointT, typename PointNT> 00556 class SampleConsensusModelFromNormals //: public SampleConsensusModel<PointT> 00557 { 00558 public: 00559 typedef typename pcl::PointCloud<PointNT>::ConstPtr PointCloudNConstPtr; 00560 typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudNPtr; 00561 00562 typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr; 00563 typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr; 00564 00565 /** \brief Empty constructor for base SampleConsensusModelFromNormals. */ 00566 SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0), normals_ () {}; 00567 00568 /** \brief Destructor. */ 00569 virtual ~SampleConsensusModelFromNormals () {} 00570 00571 /** \brief Set the normal angular distance weight. 00572 * \param[in] w the relative weight (between 0 and 1) to give to the angular 00573 * distance (0 to pi/2) between point normals and the plane normal. 00574 * (The Euclidean distance will have weight 1-w.) 00575 */ 00576 inline void 00577 setNormalDistanceWeight (const double w) 00578 { 00579 normal_distance_weight_ = w; 00580 } 00581 00582 /** \brief Get the normal angular distance weight. */ 00583 inline double 00584 getNormalDistanceWeight () { return (normal_distance_weight_); } 00585 00586 /** \brief Provide a pointer to the input dataset that contains the point 00587 * normals of the XYZ dataset. 00588 * 00589 * \param[in] normals the const boost shared pointer to a PointCloud message 00590 */ 00591 inline void 00592 setInputNormals (const PointCloudNConstPtr &normals) 00593 { 00594 normals_ = normals; 00595 } 00596 00597 /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */ 00598 inline PointCloudNConstPtr 00599 getInputNormals () { return (normals_); } 00600 00601 protected: 00602 /** \brief The relative weight (between 0 and 1) to give to the angular 00603 * distance (0 to pi/2) between point normals and the plane normal. 00604 */ 00605 double normal_distance_weight_; 00606 00607 /** \brief A pointer to the input dataset that contains the point normals 00608 * of the XYZ dataset. 00609 */ 00610 PointCloudNConstPtr normals_; 00611 }; 00612 00613 /** Base functor all the models that need non linear optimization must 00614 * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec) 00615 * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) dependening on the choosen _Scalar 00616 */ 00617 template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic> 00618 struct Functor 00619 { 00620 typedef _Scalar Scalar; 00621 enum 00622 { 00623 InputsAtCompileTime = NX, 00624 ValuesAtCompileTime = NY 00625 }; 00626 00627 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType; 00628 typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType; 00629 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 00630 00631 /** \brief Empty Construtor. */ 00632 Functor () : m_data_points_ (ValuesAtCompileTime) {} 00633 00634 /** \brief Constructor 00635 * \param[in] m_data_points number of data points to evaluate. 00636 */ 00637 Functor (int m_data_points) : m_data_points_ (m_data_points) {} 00638 00639 virtual ~Functor () {} 00640 00641 /** \brief Get the number of values. */ 00642 int 00643 values () const { return (m_data_points_); } 00644 00645 private: 00646 const int m_data_points_; 00647 }; 00648 } 00649 00650 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_