Point Cloud Library (PCL)  1.3.1
sac_segmentation.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: sac_segmentation.hpp 2400 2011-09-06 23:48:36Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
00039 #define PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
00040 
00041 #include "pcl/segmentation/sac_segmentation.h"
00042 
00043 // Sample Consensus methods
00044 #include "pcl/sample_consensus/method_types.h"
00045 #include "pcl/sample_consensus/sac.h"
00046 #include "pcl/sample_consensus/lmeds.h"
00047 #include "pcl/sample_consensus/mlesac.h"
00048 #include "pcl/sample_consensus/msac.h"
00049 #include "pcl/sample_consensus/ransac.h"
00050 #include "pcl/sample_consensus/rmsac.h"
00051 #include "pcl/sample_consensus/rransac.h"
00052 #include "pcl/sample_consensus/prosac.h"
00053 
00054 // Sample Consensus models
00055 #include "pcl/sample_consensus/model_types.h"
00056 #include "pcl/sample_consensus/sac_model.h"
00057 #include "pcl/sample_consensus/sac_model_circle.h"
00058 #include "pcl/sample_consensus/sac_model_cylinder.h"
00059 #include "pcl/sample_consensus/sac_model_line.h"
00060 #include "pcl/sample_consensus/sac_model_normal_plane.h"
00061 #include "pcl/sample_consensus/sac_model_parallel_plane.h"
00062 #include "pcl/sample_consensus/sac_model_normal_parallel_plane.h"
00063 #include "pcl/sample_consensus/sac_model_parallel_line.h"
00064 #include "pcl/sample_consensus/sac_model_perpendicular_plane.h"
00065 #include "pcl/sample_consensus/sac_model_plane.h"
00066 #include "pcl/sample_consensus/sac_model_sphere.h"
00067 #include "pcl/sample_consensus/sac_model_stick.h"
00068 
00070 template <typename PointT> void
00071 pcl::SACSegmentation<PointT>::segment (PointIndices &inliers, ModelCoefficients &model_coefficients)
00072 {
00073   // Copy the header information
00074   inliers.header = model_coefficients.header = input_->header;
00075 
00076   if (!initCompute ()) 
00077   {
00078     inliers.indices.clear (); model_coefficients.values.clear ();
00079     return;
00080   }
00081 
00082   // Initialize the Sample Consensus model and set its parameters
00083   if (!initSACModel (model_type_))
00084   {
00085     PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
00086     deinitCompute ();
00087     inliers.indices.clear (); model_coefficients.values.clear ();
00088     return;
00089   }
00090   // Initialize the Sample Consensus method and set its parameters
00091   initSAC (method_type_);
00092 
00093   if (!sac_->computeModel (0))
00094   {
00095     PCL_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
00096     deinitCompute ();
00097     inliers.indices.clear (); model_coefficients.values.clear ();
00098     return;
00099   }
00100 
00101   // Get the model inliers
00102   sac_->getInliers (inliers.indices);
00103 
00104   // Get the model coefficients
00105   Eigen::VectorXf coeff;
00106   sac_->getModelCoefficients (coeff);
00107 
00108   // If the user needs optimized coefficients
00109   if (optimize_coefficients_)
00110   {
00111     Eigen::VectorXf coeff_refined;
00112     model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined);
00113     model_coefficients.values.resize (coeff_refined.size ());
00114     memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float));
00115     // Refine inliers
00116     model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices);
00117   }
00118   else
00119   {
00120     model_coefficients.values.resize (coeff.size ());
00121     memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float));
00122   }
00123 
00124   deinitCompute ();
00125 }
00126 
00128 template <typename PointT> bool
00129 pcl::SACSegmentation<PointT>::initSACModel (const int model_type)
00130 {
00131   if (model_)
00132     model_.reset ();
00133 
00134   // Build the model
00135   switch (model_type)
00136   {
00137     case SACMODEL_PLANE:
00138     {
00139       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
00140       model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_));
00141       break;
00142     }
00143     case SACMODEL_LINE:
00144     {
00145       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
00146       model_.reset (new SampleConsensusModelLine<PointT> (input_, *indices_));
00147       break;
00148     }
00149     case SACMODEL_STICK:
00150     {
00151       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ());
00152       model_.reset (new SampleConsensusModelStick<PointT> (input_, *indices_));
00153       double min_radius, max_radius;
00154       model_->getRadiusLimits (min_radius, max_radius);
00155       if (radius_min_ != min_radius && radius_max_ != max_radius)
00156       {
00157         PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
00158         model_->setRadiusLimits (radius_min_, radius_max_);
00159       }
00160       break;
00161     }
00162     case SACMODEL_CIRCLE2D:
00163     {
00164       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
00165       model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_));
00166       typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = boost::static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_);
00167       double min_radius, max_radius;
00168       model_circle->getRadiusLimits (min_radius, max_radius);
00169       if (radius_min_ != min_radius && radius_max_ != max_radius)
00170       {
00171         PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
00172         model_circle->setRadiusLimits (radius_min_, radius_max_);
00173       }
00174       break;
00175     }
00176     case SACMODEL_SPHERE:
00177     {
00178       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
00179       model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_));
00180       typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = boost::static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_);
00181       double min_radius, max_radius;
00182       model_sphere->getRadiusLimits (min_radius, max_radius);
00183       if (radius_min_ != min_radius && radius_max_ != max_radius)
00184       {
00185         PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
00186         model_sphere->setRadiusLimits (radius_min_, radius_max_);
00187       }
00188       break;
00189     }
00190     case SACMODEL_PARALLEL_LINE:
00191     {
00192       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
00193       model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_));
00194       typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_);
00195       if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
00196       {
00197         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
00198         model_parallel->setAxis (axis_);
00199       }
00200       if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
00201       {
00202         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
00203         model_parallel->setEpsAngle (eps_angle_);
00204       }
00205       break;
00206     }
00207     case SACMODEL_PERPENDICULAR_PLANE:
00208     {
00209       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
00210       model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_));
00211       typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = boost::static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_);
00212       if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_)
00213       {
00214         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
00215         model_perpendicular->setAxis (axis_);
00216       }
00217       if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_)
00218       {
00219         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
00220         model_perpendicular->setEpsAngle (eps_angle_);
00221       }
00222       break;
00223     }
00224     case SACMODEL_PARALLEL_PLANE:
00225     {
00226       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
00227       model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_));
00228       typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_);
00229       if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
00230       {
00231         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
00232         model_parallel->setAxis (axis_);
00233       }
00234       if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
00235       {
00236         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
00237         model_parallel->setEpsAngle (eps_angle_);
00238       }
00239       break;
00240     }
00241     default:
00242     {
00243       PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
00244       return (false);
00245     }
00246   }
00247   return (true);
00248 }
00249 
00251 template <typename PointT> void
00252 pcl::SACSegmentation<PointT>::initSAC (const int method_type)
00253 {
00254   if (sac_)
00255     sac_.reset ();
00256   // Build the sample consensus method
00257   switch (method_type)
00258   {
00259     case SAC_RANSAC:
00260     default:
00261     {
00262       PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
00263       sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_));
00264       break;
00265     }
00266     case SAC_LMEDS:
00267     {
00268       PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
00269       sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_));
00270       break;
00271     }
00272     case SAC_MSAC:
00273     {
00274       PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
00275       sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_));
00276       break;
00277     }
00278     case SAC_RRANSAC:
00279     {
00280       PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
00281       sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_));
00282       break;
00283     }
00284     case SAC_RMSAC:
00285     {
00286       PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
00287       sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_));
00288       break;
00289     }
00290     case SAC_MLESAC:
00291     {
00292       PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
00293       sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_));
00294       break;
00295     }
00296     case SAC_PROSAC:
00297     {
00298       PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
00299       sac_.reset (new ProgressiveSampleConsensus<PointT> (model_, threshold_));
00300       break;
00301     }
00302   }
00303   // Set the Sample Consensus parameters if they are given/changed
00304   if (sac_->getProbability () != probability_)
00305   {
00306     PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
00307     sac_->setProbability (probability_);
00308   }
00309   if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
00310   {
00311     PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_);
00312     sac_->setMaxIterations (max_iterations_);
00313   }
00314 }
00315 
00317 template <typename PointT, typename PointNT> bool
00318 pcl::SACSegmentationFromNormals<PointT, PointNT>::initSACModel (const int model_type)
00319 {
00320   // Check if input is synced with the normals
00321   if (input_->points.size () != normals_->points.size ())
00322   {
00323     PCL_ERROR ("[pcl::%s::initSACModel] The number of points inthe input point cloud differs than the number of points in the normals!\n", getClassName ().c_str ());
00324     return (false);
00325   }
00326 
00327   if (model_)
00328     model_.reset ();
00329 
00330   // Build the model
00331   switch (model_type)
00332   {
00333     case SACMODEL_CYLINDER:
00334     {
00335       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
00336       model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_));
00337       typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_);
00338 
00339       // Set the input normals
00340       model_cylinder->setInputNormals (normals_);
00341       double min_radius, max_radius;
00342       model_cylinder->getRadiusLimits (min_radius, max_radius);
00343       if (radius_min_ != min_radius && radius_max_ != max_radius)
00344       {
00345         PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
00346         model_cylinder->setRadiusLimits (radius_min_, radius_max_);
00347       }
00348       if (distance_weight_ != model_cylinder->getNormalDistanceWeight ())
00349       {
00350         PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
00351         model_cylinder->setNormalDistanceWeight (distance_weight_);
00352       }
00353       if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_)
00354       {
00355         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
00356         model_cylinder->setAxis (axis_);
00357       }
00358       if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_)
00359       {
00360         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
00361         model_cylinder->setEpsAngle (eps_angle_);
00362       }
00363       break;
00364     }
00365     case SACMODEL_NORMAL_PLANE:
00366     {
00367       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
00368       model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_));
00369       typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_);
00370       // Set the input normals
00371       model_normals->setInputNormals (normals_);
00372       if (distance_weight_ != model_normals->getNormalDistanceWeight ())
00373       {
00374         PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
00375         model_normals->setNormalDistanceWeight (distance_weight_);
00376       }
00377       break;
00378     }
00379     case SACMODEL_NORMAL_PARALLEL_PLANE:
00380     {
00381       PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
00382       model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_));
00383       typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_);
00384       // Set the input normals
00385       model_normals->setInputNormals (normals_);
00386       if (distance_weight_ != model_normals->getNormalDistanceWeight ())
00387       {
00388         PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
00389         model_normals->setNormalDistanceWeight (distance_weight_);
00390       }
00391       if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
00392       {
00393         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
00394         model_normals->setAxis (axis_);
00395       }
00396       if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
00397       {
00398         PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
00399         model_normals->setEpsAngle (eps_angle_);
00400       }
00401       break;
00402     }
00403     // If nothing else, try SACSegmentation
00404     default:
00405     {
00406       return (pcl::SACSegmentation<PointT>::initSACModel (model_type));
00407     }
00408   }
00409   return (true);
00410 }
00411 
00412 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>;
00413 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>;
00414 
00415 #endif        // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
00416 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines