1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2011, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ 40 41 #pragma once 42 43 #include <ctime> 44 #include <climits> 45 #include <memory> 46 #include <set> 47 #include <boost/random/mersenne_twister.hpp> // for mt19937 48 #include <boost/random/uniform_int.hpp> // for uniform_int 49 #include <boost/random/variate_generator.hpp> // for variate_generator 50 51 #include <pcl/memory.h> 52 #include <pcl/console/print.h> 53 #include <pcl/point_cloud.h> 54 #include <pcl/types.h> // for index_t, Indices 55 #include <pcl/sample_consensus/model_types.h> 56 57 #include <pcl/search/search.h> 58 59 namespace pcl 60 { 61 template<class T> class ProgressiveSampleConsensus; 62 63 /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit 64 * from this class. 65 * \author Radu B. Rusu 66 * \ingroup sample_consensus 67 */ 68 template <typename PointT> 69 class SampleConsensusModel 70 { 71 public: 72 using PointCloud = pcl::PointCloud<PointT>; 73 using PointCloudConstPtr = typename PointCloud::ConstPtr; 74 using PointCloudPtr = typename PointCloud::Ptr; 75 using SearchPtr = typename pcl::search::Search<PointT>::Ptr; 76 77 using Ptr = shared_ptr<SampleConsensusModel<PointT> >; 78 using ConstPtr = shared_ptr<const SampleConsensusModel<PointT> >; 79 80 protected: 81 /** \brief Empty constructor for base SampleConsensusModel. 82 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 83 */ 84 SampleConsensusModel (bool random = false) input_()85 : input_ () 86 , radius_min_ (-std::numeric_limits<double>::max ()) 87 , radius_max_ (std::numeric_limits<double>::max ()) 88 , samples_radius_ (0.) 89 , samples_radius_search_ () 90 , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())) 91 , custom_model_constraints_ ([](auto){return true;}) 92 { 93 // Create a random number generator object 94 if (random) 95 rng_alg_.seed (static_cast<unsigned> (std::time(nullptr))); 96 else 97 rng_alg_.seed (12345u); 98 99 rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 100 } 101 102 public: 103 /** \brief Constructor for base SampleConsensusModel. 104 * \param[in] cloud the input point cloud dataset 105 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 106 */ 107 SampleConsensusModel (const PointCloudConstPtr &cloud, bool random = false) input_()108 : input_ () 109 , radius_min_ (-std::numeric_limits<double>::max ()) 110 , radius_max_ (std::numeric_limits<double>::max ()) 111 , samples_radius_ (0.) 112 , samples_radius_search_ () 113 , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())) 114 , custom_model_constraints_ ([](auto){return true;}) 115 { 116 if (random) 117 rng_alg_.seed (static_cast<unsigned> (std::time (nullptr))); 118 else 119 rng_alg_.seed (12345u); 120 121 // Sets the input cloud and creates a vector of "fake" indices 122 setInputCloud (cloud); 123 124 // Create a random number generator object 125 rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 126 } 127 128 /** \brief Constructor for base SampleConsensusModel. 129 * \param[in] cloud the input point cloud dataset 130 * \param[in] indices a vector of point indices to be used from \a cloud 131 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 132 */ 133 SampleConsensusModel (const PointCloudConstPtr &cloud, 134 const Indices &indices, 135 bool random = false) input_(cloud)136 : input_ (cloud) 137 , indices_ (new Indices (indices)) 138 , radius_min_ (-std::numeric_limits<double>::max ()) 139 , radius_max_ (std::numeric_limits<double>::max ()) 140 , samples_radius_ (0.) 141 , samples_radius_search_ () 142 , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())) 143 , custom_model_constraints_ ([](auto){return true;}) 144 { 145 if (random) 146 rng_alg_.seed (static_cast<unsigned> (std::time(nullptr))); 147 else 148 rng_alg_.seed (12345u); 149 150 if (indices_->size () > input_->size ()) 151 { 152 PCL_ERROR("[pcl::SampleConsensusModel] Invalid index vector given with size " 153 "%zu while the input PointCloud has size %zu!\n", 154 indices_->size(), 155 static_cast<std::size_t>(input_->size())); 156 indices_->clear (); 157 } 158 shuffled_indices_ = *indices_; 159 160 // Create a random number generator object 161 rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 162 }; 163 164 /** \brief Destructor for base SampleConsensusModel. */ ~SampleConsensusModel()165 virtual ~SampleConsensusModel () {}; 166 167 /** \brief Get a set of random data samples and return them as point 168 * indices. 169 * \param[out] iterations the internal number of iterations used by SAC methods 170 * \param[out] samples the resultant model samples 171 */ 172 virtual void getSamples(int & iterations,Indices & samples)173 getSamples (int &iterations, Indices &samples) 174 { 175 // We're assuming that indices_ have already been set in the constructor 176 if (indices_->size () < getSampleSize ()) 177 { 178 PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n", 179 samples.size (), indices_->size ()); 180 // one of these will make it stop :) 181 samples.clear (); 182 iterations = INT_MAX - 1; 183 return; 184 } 185 186 // Get a second point which is different than the first 187 samples.resize (getSampleSize ()); 188 for (unsigned int iter = 0; iter < max_sample_checks_; ++iter) 189 { 190 // Choose the random indices 191 if (samples_radius_ < std::numeric_limits<double>::epsilon ()) 192 SampleConsensusModel<PointT>::drawIndexSample (samples); 193 else 194 SampleConsensusModel<PointT>::drawIndexSampleRadius (samples); 195 196 // If it's a good sample, stop here 197 if (isSampleGood (samples)) 198 { 199 PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ()); 200 return; 201 } 202 } 203 PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_); 204 samples.clear (); 205 } 206 207 /** \brief Check whether the given index samples can form a valid model, 208 * compute the model coefficients from these samples and store them 209 * in model_coefficients. Pure virtual. 210 * Implementations of this function must be thread-safe. 211 * \param[in] samples the point indices found as possible good candidates 212 * for creating a valid model 213 * \param[out] model_coefficients the computed model coefficients 214 */ 215 virtual bool 216 computeModelCoefficients (const Indices &samples, 217 Eigen::VectorXf &model_coefficients) const = 0; 218 219 /** \brief Recompute the model coefficients using the given inlier set 220 * and return them to the user. Pure virtual. 221 * 222 * @note: these are the coefficients of the model after refinement 223 * (e.g., after a least-squares optimization) 224 * 225 * \param[in] inliers the data inliers supporting the model 226 * \param[in] model_coefficients the initial guess for the model coefficients 227 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization 228 */ 229 virtual void 230 optimizeModelCoefficients (const Indices &inliers, 231 const Eigen::VectorXf &model_coefficients, 232 Eigen::VectorXf &optimized_coefficients) const = 0; 233 234 /** \brief Compute all distances from the cloud data to a given model. Pure virtual. 235 * 236 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 237 * \param[out] distances the resultant estimated distances 238 */ 239 virtual void 240 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 241 std::vector<double> &distances) const = 0; 242 243 /** \brief Select all the points which respect the given model 244 * coefficients as inliers. Pure virtual. 245 * 246 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 247 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from 248 * the outliers 249 * \param[out] inliers the resultant model inliers 250 */ 251 virtual void 252 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 253 const double threshold, 254 Indices &inliers) = 0; 255 256 /** \brief Count all the points which respect the given model 257 * coefficients as inliers. Pure virtual. 258 * Implementations of this function must be thread-safe. 259 * \param[in] model_coefficients the coefficients of a model that we need to 260 * compute distances to 261 * \param[in] threshold a maximum admissible distance threshold for 262 * determining the inliers from the outliers 263 * \return the resultant number of inliers 264 */ 265 virtual std::size_t 266 countWithinDistance (const Eigen::VectorXf &model_coefficients, 267 const double threshold) const = 0; 268 269 /** \brief Create a new point cloud with inliers projected onto the model. Pure virtual. 270 * \param[in] inliers the data inliers that we want to project on the model 271 * \param[in] model_coefficients the coefficients of a model 272 * \param[out] projected_points the resultant projected points 273 * \param[in] copy_data_fields set to true (default) if we want the \a 274 * projected_points cloud to be an exact copy of the input dataset minus 275 * the point projections on the plane model 276 */ 277 virtual void 278 projectPoints (const Indices &inliers, 279 const Eigen::VectorXf &model_coefficients, 280 PointCloud &projected_points, 281 bool copy_data_fields = true) const = 0; 282 283 /** \brief Verify whether a subset of indices verifies a given set of 284 * model coefficients. Pure virtual. 285 * 286 * \param[in] indices the data indices that need to be tested against the model 287 * \param[in] model_coefficients the set of model coefficients 288 * \param[in] threshold a maximum admissible distance threshold for 289 * determining the inliers from the outliers 290 */ 291 virtual bool 292 doSamplesVerifyModel (const std::set<index_t> &indices, 293 const Eigen::VectorXf &model_coefficients, 294 const double threshold) const = 0; 295 296 /** \brief Provide a pointer to the input dataset 297 * \param[in] cloud the const boost shared pointer to a PointCloud message 298 */ 299 inline virtual void setInputCloud(const PointCloudConstPtr & cloud)300 setInputCloud (const PointCloudConstPtr &cloud) 301 { 302 input_ = cloud; 303 if (!indices_) 304 indices_.reset (new Indices ()); 305 if (indices_->empty ()) 306 { 307 // Prepare a set of indices to be used (entire cloud) 308 indices_->resize (cloud->size ()); 309 for (std::size_t i = 0; i < cloud->size (); ++i) 310 (*indices_)[i] = static_cast<index_t> (i); 311 } 312 shuffled_indices_ = *indices_; 313 } 314 315 /** \brief Get a pointer to the input point cloud dataset. */ 316 inline PointCloudConstPtr getInputCloud()317 getInputCloud () const { return (input_); } 318 319 /** \brief Provide a pointer to the vector of indices that represents the input data. 320 * \param[in] indices a pointer to the vector of indices that represents the input data. 321 */ 322 inline void setIndices(const IndicesPtr & indices)323 setIndices (const IndicesPtr &indices) 324 { 325 indices_ = indices; 326 shuffled_indices_ = *indices_; 327 } 328 329 /** \brief Provide the vector of indices that represents the input data. 330 * \param[out] indices the vector of indices that represents the input data. 331 */ 332 inline void setIndices(const Indices & indices)333 setIndices (const Indices &indices) 334 { 335 indices_.reset (new Indices (indices)); 336 shuffled_indices_ = indices; 337 } 338 339 /** \brief Get a pointer to the vector of indices used. */ 340 inline IndicesPtr getIndices()341 getIndices () const { return (indices_); } 342 343 /** \brief Return a unique id for each type of model employed. */ 344 virtual SacModel 345 getModelType () const = 0; 346 347 /** \brief Get a string representation of the name of this class. */ 348 inline const std::string& getClassName()349 getClassName () const 350 { 351 return (model_name_); 352 } 353 354 /** \brief Return the size of a sample from which the model is computed. */ 355 inline unsigned int getSampleSize()356 getSampleSize () const 357 { 358 return sample_size_; 359 } 360 361 /** \brief Return the number of coefficients in the model. */ 362 inline unsigned int getModelSize()363 getModelSize () const 364 { 365 return model_size_; 366 } 367 368 /** \brief Set the minimum and maximum allowable radius limits for the 369 * model (applicable to models that estimate a radius) 370 * \param[in] min_radius the minimum radius model 371 * \param[in] max_radius the maximum radius model 372 * \todo change this to set limits on the entire model 373 */ 374 inline void setRadiusLimits(const double & min_radius,const double & max_radius)375 setRadiusLimits (const double &min_radius, const double &max_radius) 376 { 377 radius_min_ = min_radius; 378 radius_max_ = max_radius; 379 } 380 381 /** \brief Get the minimum and maximum allowable radius limits for the 382 * model as set by the user. 383 * 384 * \param[out] min_radius the resultant minimum radius model 385 * \param[out] max_radius the resultant maximum radius model 386 */ 387 inline void getRadiusLimits(double & min_radius,double & max_radius)388 getRadiusLimits (double &min_radius, double &max_radius) const 389 { 390 min_radius = radius_min_; 391 max_radius = radius_max_; 392 } 393 394 /** \brief This can be used to impose any kind of constraint on the model, 395 * e.g. that it has a specific direction, size, or anything else. 396 * \param[in] function A function that gets model coefficients and returns whether the model is acceptable or not. 397 */ 398 inline void setModelConstraints(std::function<bool (const Eigen::VectorXf &)> function)399 setModelConstraints (std::function<bool(const Eigen::VectorXf &)> function) 400 { 401 if (!function) 402 { 403 PCL_ERROR ("[pcl::SampleConsensusModel::setModelConstraints] The given function is empty (i.e. does not contain a callable target)!\n"); 404 return; 405 } 406 custom_model_constraints_ = std::move (function); 407 } 408 409 /** \brief Set the maximum distance allowed when drawing random samples 410 * \param[in] radius the maximum distance (L2 norm) 411 * \param search 412 */ 413 inline void setSamplesMaxDist(const double & radius,SearchPtr search)414 setSamplesMaxDist (const double &radius, SearchPtr search) 415 { 416 samples_radius_ = radius; 417 samples_radius_search_ = search; 418 } 419 420 /** \brief Get maximum distance allowed when drawing random samples 421 * 422 * \param[out] radius the maximum distance (L2 norm) 423 */ 424 inline void getSamplesMaxDist(double & radius)425 getSamplesMaxDist (double &radius) const 426 { 427 radius = samples_radius_; 428 } 429 430 friend class ProgressiveSampleConsensus<PointT>; 431 432 /** \brief Compute the variance of the errors to the model. 433 * \param[in] error_sqr_dists a vector holding the distances 434 */ 435 inline double computeVariance(const std::vector<double> & error_sqr_dists)436 computeVariance (const std::vector<double> &error_sqr_dists) const 437 { 438 std::vector<double> dists (error_sqr_dists); 439 const std::size_t medIdx = dists.size () >> 1; 440 std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ()); 441 double median_error_sqr = dists[medIdx]; 442 return (2.1981 * median_error_sqr); 443 } 444 445 /** \brief Compute the variance of the errors to the model from the internally 446 * estimated vector of distances. The model must be computed first (or at least 447 * selectWithinDistance must be called). 448 */ 449 inline double computeVariance()450 computeVariance () const 451 { 452 if (error_sqr_dists_.empty ()) 453 { 454 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"); 455 return (std::numeric_limits<double>::quiet_NaN ()); 456 } 457 return (computeVariance (error_sqr_dists_)); 458 } 459 460 protected: 461 462 /** \brief Fills a sample array with random samples from the indices_ vector 463 * \param[out] sample the set of indices of target_ to analyze 464 */ 465 inline void drawIndexSample(Indices & sample)466 drawIndexSample (Indices &sample) 467 { 468 std::size_t sample_size = sample.size (); 469 std::size_t index_size = shuffled_indices_.size (); 470 for (std::size_t i = 0; i < sample_size; ++i) 471 // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo 472 // elements, that does not matter (and nowadays, random number generators are good) 473 //std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]); 474 std::swap (shuffled_indices_[i], shuffled_indices_[i + (rnd () % (index_size - i))]); 475 std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ()); 476 } 477 478 /** \brief Fills a sample array with one random sample from the indices_ vector 479 * and other random samples that are closer than samples_radius_ 480 * \param[out] sample the set of indices of target_ to analyze 481 */ 482 inline void drawIndexSampleRadius(Indices & sample)483 drawIndexSampleRadius (Indices &sample) 484 { 485 std::size_t sample_size = sample.size (); 486 std::size_t index_size = shuffled_indices_.size (); 487 488 std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]); 489 //const PointT& pt0 = (*input_)[shuffled_indices_[0]]; 490 491 Indices indices; 492 std::vector<float> sqr_dists; 493 494 // If indices have been set when the search object was constructed, 495 // radiusSearch() expects an index into the indices vector as its 496 // first parameter. This can't be determined efficiently, so we use 497 // the point instead of the index. 498 // Returned indices are converted automatically. 499 samples_radius_search_->radiusSearch (input_->at(shuffled_indices_[0]), 500 samples_radius_, indices, sqr_dists ); 501 502 if (indices.size () < sample_size - 1) 503 { 504 // radius search failed, make an invalid model 505 for(std::size_t i = 1; i < sample_size; ++i) 506 shuffled_indices_[i] = shuffled_indices_[0]; 507 } 508 else 509 { 510 for (std::size_t i = 0; i < sample_size-1; ++i) 511 std::swap (indices[i], indices[i + (rnd () % (indices.size () - i))]); 512 for (std::size_t i = 1; i < sample_size; ++i) 513 shuffled_indices_[i] = indices[i-1]; 514 } 515 516 std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ()); 517 } 518 519 /** \brief Check whether a model is valid given the user constraints. 520 * 521 * Default implementation verifies that the number of coefficients in the supplied model is as expected for this 522 * SAC model type. Specific SAC models should extend this function by checking the user constraints (if any). 523 * 524 * \param[in] model_coefficients the set of model coefficients 525 */ 526 virtual bool isModelValid(const Eigen::VectorXf & model_coefficients)527 isModelValid (const Eigen::VectorXf &model_coefficients) const 528 { 529 if (model_coefficients.size () != model_size_) 530 { 531 PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (is %lu, should be %lu)!\n", getClassName ().c_str (), model_coefficients.size (), model_size_); 532 return (false); 533 } 534 if (!custom_model_constraints_(model_coefficients)) 535 { 536 PCL_DEBUG ("[pcl::%s::isModelValid] The user defined isModelValid function returned false.\n", getClassName ().c_str ()); 537 return (false); 538 } 539 return (true); 540 } 541 542 /** \brief Check if a sample of indices results in a good sample of points 543 * indices. Pure virtual. 544 * \param[in] samples the resultant index samples 545 */ 546 virtual bool 547 isSampleGood (const Indices &samples) const = 0; 548 549 /** \brief The model name. */ 550 std::string model_name_; 551 552 /** \brief A boost shared pointer to the point cloud data array. */ 553 PointCloudConstPtr input_; 554 555 /** \brief A pointer to the vector of point indices to use. */ 556 IndicesPtr indices_; 557 558 /** The maximum number of samples to try until we get a good one */ 559 static const unsigned int max_sample_checks_ = 1000; 560 561 /** \brief The minimum and maximum radius limits for the model. 562 * Applicable to all models that estimate a radius. 563 */ 564 double radius_min_, radius_max_; 565 566 /** \brief The maximum distance of subsequent samples from the first (radius search) */ 567 double samples_radius_; 568 569 /** \brief The search object for picking subsequent samples using radius search */ 570 SearchPtr samples_radius_search_; 571 572 /** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */ 573 Indices shuffled_indices_; 574 575 /** \brief Boost-based random number generator algorithm. */ 576 boost::mt19937 rng_alg_; 577 578 /** \brief Boost-based random number generator distribution. */ 579 std::shared_ptr<boost::uniform_int<> > rng_dist_; 580 581 /** \brief Boost-based random number generator. */ 582 std::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > > rng_gen_; 583 584 /** \brief A vector holding the distances to the computed model. Used internally. */ 585 std::vector<double> error_sqr_dists_; 586 587 /** \brief The size of a sample from which the model is computed. Every subclass should initialize this appropriately. */ 588 unsigned int sample_size_; 589 590 /** \brief The number of coefficients in the model. Every subclass should initialize this appropriately. */ 591 unsigned int model_size_; 592 593 /** \brief Boost-based random number generator. */ 594 inline int rnd()595 rnd () 596 { 597 return ((*rng_gen_) ()); 598 } 599 600 /** \brief A user defined function that takes model coefficients and returns whether the model is acceptable or not. */ 601 std::function<bool(const Eigen::VectorXf &)> custom_model_constraints_; 602 public: 603 PCL_MAKE_ALIGNED_OPERATOR_NEW 604 }; 605 606 /** \brief @b SampleConsensusModelFromNormals represents the base model class 607 * for models that require the use of surface normals for estimation. 608 * \ingroup sample_consensus 609 */ 610 template <typename PointT, typename PointNT> 611 class SampleConsensusModelFromNormals //: public SampleConsensusModel<PointT> 612 { 613 public: 614 using PointCloudNConstPtr = typename pcl::PointCloud<PointNT>::ConstPtr; 615 using PointCloudNPtr = typename pcl::PointCloud<PointNT>::Ptr; 616 617 using Ptr = shared_ptr<SampleConsensusModelFromNormals<PointT, PointNT> >; 618 using ConstPtr = shared_ptr<const SampleConsensusModelFromNormals<PointT, PointNT> >; 619 620 /** \brief Empty constructor for base SampleConsensusModelFromNormals. */ SampleConsensusModelFromNormals()621 SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0), normals_ () {}; 622 623 /** \brief Destructor. */ ~SampleConsensusModelFromNormals()624 virtual ~SampleConsensusModelFromNormals () {} 625 626 /** \brief Set the normal angular distance weight. 627 * \param[in] w the relative weight (between 0 and 1) to give to the angular 628 * distance (0 to pi/2) between point normals and the plane normal. 629 * (The Euclidean distance will have weight 1-w.) 630 */ 631 inline void setNormalDistanceWeight(const double w)632 setNormalDistanceWeight (const double w) 633 { 634 if (w < 0.0 || w > 1.0) 635 { 636 PCL_ERROR ("[pcl::SampleConsensusModel::setNormalDistanceWeight] w is %g, but should be in [0; 1]. Weight will not be set.", w); 637 return; 638 } 639 normal_distance_weight_ = w; 640 } 641 642 /** \brief Get the normal angular distance weight. */ 643 inline double getNormalDistanceWeight()644 getNormalDistanceWeight () const { return (normal_distance_weight_); } 645 646 /** \brief Provide a pointer to the input dataset that contains the point 647 * normals of the XYZ dataset. 648 * 649 * \param[in] normals the const boost shared pointer to a PointCloud message 650 */ 651 inline void setInputNormals(const PointCloudNConstPtr & normals)652 setInputNormals (const PointCloudNConstPtr &normals) 653 { 654 normals_ = normals; 655 } 656 657 /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */ 658 inline PointCloudNConstPtr getInputNormals()659 getInputNormals () const { return (normals_); } 660 661 protected: 662 /** \brief The relative weight (between 0 and 1) to give to the angular 663 * distance (0 to pi/2) between point normals and the plane normal. 664 */ 665 double normal_distance_weight_; 666 667 /** \brief A pointer to the input dataset that contains the point normals 668 * of the XYZ dataset. 669 */ 670 PointCloudNConstPtr normals_; 671 }; 672 673 /** Base functor all the models that need non linear optimization must 674 * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec) 675 * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar 676 */ 677 template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic> 678 struct Functor 679 { 680 using Scalar = _Scalar; 681 enum 682 { 683 InputsAtCompileTime = NX, 684 ValuesAtCompileTime = NY 685 }; 686 687 using ValueType = Eigen::Matrix<Scalar,ValuesAtCompileTime,1>; 688 using InputType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>; 689 using JacobianType = Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime>; 690 691 /** \brief Empty Constructor. */ FunctorFunctor692 Functor () : m_data_points_ (ValuesAtCompileTime) {} 693 694 /** \brief Constructor 695 * \param[in] m_data_points number of data points to evaluate. 696 */ FunctorFunctor697 Functor (int m_data_points) : m_data_points_ (m_data_points) {} 698 ~FunctorFunctor699 virtual ~Functor () {} 700 701 /** \brief Get the number of values. */ 702 int valuesFunctor703 values () const { return (m_data_points_); } 704 705 private: 706 const int m_data_points_; 707 }; 708 } 709