Point Cloud Library (PCL)
1.3.1
|
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