Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model.h
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_