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-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_