Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/segmentation/include/pcl/segmentation/sac_segmentation.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
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 the copyright holder(s) 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$
00037  *
00038  */
00039 
00040 #ifndef PCL_SEGMENTATION_SAC_SEGMENTATION_H_
00041 #define PCL_SEGMENTATION_SAC_SEGMENTATION_H_
00042 
00043 #include <pcl/pcl_base.h>
00044 #include <pcl/PointIndices.h>
00045 #include <pcl/ModelCoefficients.h>
00046 
00047 // Sample Consensus methods
00048 #include <pcl/sample_consensus/method_types.h>
00049 #include <pcl/sample_consensus/sac.h>
00050 // Sample Consensus models
00051 #include <pcl/sample_consensus/model_types.h>
00052 #include <pcl/sample_consensus/sac_model.h>
00053 
00054 #include <pcl/search/search.h>
00055 
00056 namespace pcl
00057 {
00058   /** \brief @b SACSegmentation represents the Nodelet segmentation class for
00059     * Sample Consensus methods and models, in the sense that it just creates a
00060     * Nodelet wrapper for generic-purpose SAC-based segmentation.
00061     * \author Radu Bogdan Rusu
00062     * \ingroup segmentation
00063     */
00064   template <typename PointT>
00065   class SACSegmentation : public PCLBase<PointT>
00066   {
00067     using PCLBase<PointT>::initCompute;
00068     using PCLBase<PointT>::deinitCompute;
00069 
00070      public:
00071       using PCLBase<PointT>::input_;
00072       using PCLBase<PointT>::indices_;
00073 
00074       typedef pcl::PointCloud<PointT> PointCloud;
00075       typedef typename PointCloud::Ptr PointCloudPtr;
00076       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00077       typedef typename pcl::search::Search<PointT>::Ptr SearchPtr;
00078 
00079       typedef typename SampleConsensus<PointT>::Ptr SampleConsensusPtr;
00080       typedef typename SampleConsensusModel<PointT>::Ptr SampleConsensusModelPtr;
00081 
00082       /** \brief Empty constructor. 
00083         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00084         */
00085       SACSegmentation (bool random = false) 
00086         : model_ ()
00087         , sac_ ()
00088         , model_type_ (-1)
00089         , method_type_ (0)
00090         , threshold_ (0)
00091         , optimize_coefficients_ (true)
00092         , radius_min_ (-std::numeric_limits<double>::max ())
00093         , radius_max_ (std::numeric_limits<double>::max ())
00094         , samples_radius_ (0.0)
00095         , samples_radius_search_ ()
00096         , eps_angle_ (0.0)
00097         , axis_ (Eigen::Vector3f::Zero ())
00098         , max_iterations_ (50)
00099         , probability_ (0.99)
00100         , random_ (random)
00101       {
00102       }
00103 
00104       /** \brief Empty destructor. */
00105       virtual ~SACSegmentation () { /*srv_.reset ();*/ };
00106 
00107       /** \brief The type of model to use (user given parameter).
00108         * \param[in] model the model type (check \a model_types.h)
00109         */
00110       inline void 
00111       setModelType (int model) { model_type_ = model; }
00112 
00113       /** \brief Get the type of SAC model used. */
00114       inline int 
00115       getModelType () const { return (model_type_); }
00116 
00117       /** \brief Get a pointer to the SAC method used. */
00118       inline SampleConsensusPtr 
00119       getMethod () const { return (sac_); }
00120 
00121       /** \brief Get a pointer to the SAC model used. */
00122       inline SampleConsensusModelPtr 
00123       getModel () const { return (model_); }
00124 
00125       /** \brief The type of sample consensus method to use (user given parameter).
00126         * \param[in] method the method type (check \a method_types.h)
00127         */
00128       inline void 
00129       setMethodType (int method) { method_type_ = method; }
00130 
00131       /** \brief Get the type of sample consensus method used. */
00132       inline int 
00133       getMethodType () const { return (method_type_); }
00134 
00135       /** \brief Distance to the model threshold (user given parameter).
00136         * \param[in] threshold the distance threshold to use
00137         */
00138       inline void 
00139       setDistanceThreshold (double threshold) { threshold_ = threshold; }
00140 
00141       /** \brief Get the distance to the model threshold. */
00142       inline double 
00143       getDistanceThreshold () const { return (threshold_); }
00144 
00145       /** \brief Set the maximum number of iterations before giving up.
00146         * \param[in] max_iterations the maximum number of iterations the sample consensus method will run
00147         */
00148       inline void 
00149       setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; }
00150 
00151       /** \brief Get maximum number of iterations before giving up. */
00152       inline int 
00153       getMaxIterations () const { return (max_iterations_); }
00154 
00155       /** \brief Set the probability of choosing at least one sample free from outliers.
00156         * \param[in] probability the model fitting probability
00157         */
00158       inline void 
00159       setProbability (double probability) { probability_ = probability; }
00160 
00161       /** \brief Get the probability of choosing at least one sample free from outliers. */
00162       inline double 
00163       getProbability () const { return (probability_); }
00164 
00165       /** \brief Set to true if a coefficient refinement is required.
00166         * \param[in] optimize true for enabling model coefficient refinement, false otherwise
00167         */
00168       inline void 
00169       setOptimizeCoefficients (bool optimize) { optimize_coefficients_ = optimize; }
00170 
00171       /** \brief Get the coefficient refinement internal flag. */
00172       inline bool 
00173       getOptimizeCoefficients () const { return (optimize_coefficients_); }
00174 
00175       /** \brief Set the minimum and maximum allowable radius limits for the model (applicable to models that estimate
00176         * a radius)
00177         * \param[in] min_radius the minimum radius model
00178         * \param[in] max_radius the maximum radius model
00179         */
00180       inline void
00181       setRadiusLimits (const double &min_radius, const double &max_radius)
00182       {
00183         radius_min_ = min_radius;
00184         radius_max_ = max_radius;
00185       }
00186 
00187       /** \brief Get the minimum and maximum allowable radius limits for the model as set by the user.
00188         * \param[out] min_radius the resultant minimum radius model
00189         * \param[out] max_radius the resultant maximum radius model
00190         */
00191       inline void
00192       getRadiusLimits (double &min_radius, double &max_radius)
00193       {
00194         min_radius = radius_min_;
00195         max_radius = radius_max_;
00196       }
00197 
00198       /** \brief Set the maximum distance allowed when drawing random samples
00199         * \param[in] radius the maximum distance (L2 norm)
00200         */
00201       inline void
00202       setSamplesMaxDist (const double &radius, SearchPtr search)
00203       {
00204         samples_radius_ = radius;
00205         samples_radius_search_ = search;
00206       }
00207 
00208       /** \brief Get maximum distance allowed when drawing random samples
00209         *
00210         * \param[out] radius the maximum distance (L2 norm)
00211         */
00212       inline void
00213       getSamplesMaxDist (double &radius)
00214       {
00215         radius = samples_radius_;
00216       }
00217 
00218       /** \brief Set the axis along which we need to search for a model perpendicular to.
00219         * \param[in] ax the axis along which we need to search for a model perpendicular to
00220         */
00221       inline void 
00222       setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
00223 
00224       /** \brief Get the axis along which we need to search for a model perpendicular to. */
00225       inline Eigen::Vector3f 
00226       getAxis () const { return (axis_); }
00227 
00228       /** \brief Set the angle epsilon (delta) threshold.
00229         * \param[in] ea the maximum allowed difference between the model normal and the given axis in radians.
00230         */
00231       inline void 
00232       setEpsAngle (double ea) { eps_angle_ = ea; }
00233 
00234       /** \brief Get the epsilon (delta) model angle threshold in radians. */
00235       inline double 
00236       getEpsAngle () const { return (eps_angle_); }
00237 
00238       /** \brief Base method for segmentation of a model in a PointCloud given by <setInputCloud (), setIndices ()>
00239         * \param[in] inliers the resultant point indices that support the model found (inliers)
00240         * \param[out] model_coefficients the resultant model coefficients
00241         */
00242       virtual void 
00243       segment (PointIndices &inliers, ModelCoefficients &model_coefficients);
00244 
00245     protected:
00246       /** \brief Initialize the Sample Consensus model and set its parameters.
00247         * \param[in] model_type the type of SAC model that is to be used
00248         */
00249       virtual bool 
00250       initSACModel (const int model_type);
00251 
00252       /** \brief Initialize the Sample Consensus method and set its parameters.
00253         * \param[in] method_type the type of SAC method to be used
00254         */
00255       virtual void 
00256       initSAC (const int method_type);
00257 
00258       /** \brief The model that needs to be segmented. */
00259       SampleConsensusModelPtr model_;
00260 
00261       /** \brief The sample consensus segmentation method. */
00262       SampleConsensusPtr sac_;
00263 
00264       /** \brief The type of model to use (user given parameter). */
00265       int model_type_;
00266 
00267       /** \brief The type of sample consensus method to use (user given parameter). */
00268       int method_type_;
00269 
00270       /** \brief Distance to the model threshold (user given parameter). */
00271       double threshold_;
00272 
00273       /** \brief Set to true if a coefficient refinement is required. */
00274       bool optimize_coefficients_;
00275 
00276       /** \brief The minimum and maximum radius limits for the model. Applicable to all models that estimate a radius. */
00277       double radius_min_, radius_max_;
00278 
00279       /** \brief The maximum distance of subsequent samples from the first (radius search) */
00280       double samples_radius_;
00281 
00282       /** \brief The search object for picking subsequent samples using radius search */
00283       SearchPtr samples_radius_search_;
00284 
00285       /** \brief The maximum allowed difference between the model normal and the given axis. */
00286       double eps_angle_;
00287 
00288       /** \brief The axis along which we need to search for a model perpendicular to. */
00289       Eigen::Vector3f axis_;
00290 
00291       /** \brief Maximum number of iterations before giving up (user given parameter). */
00292       int max_iterations_;
00293 
00294       /** \brief Desired probability of choosing at least one sample free from outliers (user given parameter). */
00295       double probability_;
00296 
00297       /** \brief Set to true if we need a random seed. */
00298       bool random_;
00299 
00300       /** \brief Class get name method. */
00301       virtual std::string 
00302       getClassName () const { return ("SACSegmentation"); }
00303   };
00304 
00305   /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
00306     * models that require the use of surface normals for estimation.
00307     * \ingroup segmentation
00308     */
00309   template <typename PointT, typename PointNT>
00310   class SACSegmentationFromNormals: public SACSegmentation<PointT>
00311   {
00312     using SACSegmentation<PointT>::model_;
00313     using SACSegmentation<PointT>::model_type_;
00314     using SACSegmentation<PointT>::radius_min_;
00315     using SACSegmentation<PointT>::radius_max_;
00316     using SACSegmentation<PointT>::eps_angle_;
00317     using SACSegmentation<PointT>::axis_;
00318     using SACSegmentation<PointT>::random_;
00319 
00320     public:
00321       using PCLBase<PointT>::input_;
00322       using PCLBase<PointT>::indices_;
00323 
00324       typedef typename SACSegmentation<PointT>::PointCloud PointCloud;
00325       typedef typename PointCloud::Ptr PointCloudPtr;
00326       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00327 
00328       typedef typename pcl::PointCloud<PointNT> PointCloudN;
00329       typedef typename PointCloudN::Ptr PointCloudNPtr;
00330       typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
00331 
00332       typedef typename SampleConsensus<PointT>::Ptr SampleConsensusPtr;
00333       typedef typename SampleConsensusModel<PointT>::Ptr SampleConsensusModelPtr;
00334       typedef typename SampleConsensusModelFromNormals<PointT, PointNT>::Ptr SampleConsensusModelFromNormalsPtr;
00335 
00336       /** \brief Empty constructor.
00337         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00338         */
00339       SACSegmentationFromNormals (bool random = false) 
00340         : SACSegmentation<PointT> (random)
00341         , normals_ ()
00342         , distance_weight_ (0.1)
00343         , distance_from_origin_ (0)
00344         , min_angle_ ()
00345         , max_angle_ ()
00346       {};
00347 
00348       /** \brief Provide a pointer to the input dataset that contains the point normals of 
00349         * the XYZ dataset.
00350         * \param[in] normals the const boost shared pointer to a PointCloud message
00351         */
00352       inline void 
00353       setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
00354 
00355       /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
00356       inline PointCloudNConstPtr 
00357       getInputNormals () const { return (normals_); }
00358 
00359       /** \brief Set the relative weight (between 0 and 1) to give to the angular 
00360         * distance (0 to pi/2) between point normals and the plane normal.
00361         * \param[in] distance_weight the distance/angular weight
00362         */
00363       inline void 
00364       setNormalDistanceWeight (double distance_weight) { distance_weight_ = distance_weight; }
00365 
00366       /** \brief Get the relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point
00367         * normals and the plane normal. */
00368       inline double 
00369       getNormalDistanceWeight () const { return (distance_weight_); }
00370 
00371       /** \brief Set the minimum opning angle for a cone model.
00372         * \param oa the opening angle which we need minumum to validate a cone model.
00373         */
00374       inline void
00375       setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
00376       {
00377         min_angle_ = min_angle;
00378         max_angle_ = max_angle;
00379       }
00380  
00381       /** \brief Get the opening angle which we need minumum to validate a cone model. */
00382       inline void
00383       getMinMaxOpeningAngle (double &min_angle, double &max_angle)
00384       {
00385         min_angle = min_angle_;
00386         max_angle = max_angle_;
00387       }
00388 
00389       /** \brief Set the distance we expect a plane model to be from the origin
00390         * \param[in] d distance from the template plane modl to the origin
00391         */
00392       inline void
00393       setDistanceFromOrigin (const double d) { distance_from_origin_ = d; }
00394 
00395       /** \brief Get the distance of a plane model from the origin. */
00396       inline double
00397       getDistanceFromOrigin () const { return (distance_from_origin_); }
00398 
00399     protected:
00400       /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
00401       PointCloudNConstPtr normals_;
00402 
00403       /** \brief The relative weight (between 0 and 1) to give to the angular
00404         * distance (0 to pi/2) between point normals and the plane normal. 
00405         */
00406       double distance_weight_;
00407 
00408       /** \brief The distance from the template plane to the origin. */
00409       double distance_from_origin_;
00410 
00411       /** \brief The minimum and maximum allowed opening angle of valid cone model. */
00412       double min_angle_;
00413       double max_angle_;
00414 
00415       /** \brief Initialize the Sample Consensus model and set its parameters.
00416         * \param[in] model_type the type of SAC model that is to be used
00417         */
00418       virtual bool 
00419       initSACModel (const int model_type);
00420 
00421       /** \brief Class get name method. */
00422       virtual std::string 
00423       getClassName () const { return ("SACSegmentationFromNormals"); }
00424   };
00425 }
00426 
00427 #ifdef PCL_NO_PRECOMPILE
00428 #include <pcl/segmentation/impl/sac_segmentation.hpp>
00429 #endif
00430 
00431 #endif  //#ifndef PCL_SEGMENTATION_SAC_SEGMENTATION_H_