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) 2009, 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_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_ 00042 #define PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_ 00043 00044 #include <pcl/segmentation/sac_segmentation.h> 00045 00046 // Sample Consensus methods 00047 #include <pcl/sample_consensus/sac.h> 00048 #include <pcl/sample_consensus/lmeds.h> 00049 #include <pcl/sample_consensus/mlesac.h> 00050 #include <pcl/sample_consensus/msac.h> 00051 #include <pcl/sample_consensus/ransac.h> 00052 #include <pcl/sample_consensus/rmsac.h> 00053 #include <pcl/sample_consensus/rransac.h> 00054 #include <pcl/sample_consensus/prosac.h> 00055 00056 // Sample Consensus models 00057 #include <pcl/sample_consensus/sac_model.h> 00058 #include <pcl/sample_consensus/sac_model_circle.h> 00059 #include <pcl/sample_consensus/sac_model_circle3d.h> 00060 #include <pcl/sample_consensus/sac_model_cone.h> 00061 #include <pcl/sample_consensus/sac_model_cylinder.h> 00062 #include <pcl/sample_consensus/sac_model_line.h> 00063 #include <pcl/sample_consensus/sac_model_normal_plane.h> 00064 #include <pcl/sample_consensus/sac_model_parallel_plane.h> 00065 #include <pcl/sample_consensus/sac_model_normal_parallel_plane.h> 00066 #include <pcl/sample_consensus/sac_model_parallel_line.h> 00067 #include <pcl/sample_consensus/sac_model_perpendicular_plane.h> 00068 #include <pcl/sample_consensus/sac_model_plane.h> 00069 #include <pcl/sample_consensus/sac_model_sphere.h> 00070 #include <pcl/sample_consensus/sac_model_normal_sphere.h> 00071 #include <pcl/sample_consensus/sac_model_stick.h> 00072 00073 ////////////////////////////////////////////////////////////////////////////////////////////// 00074 template <typename PointT> void 00075 pcl::SACSegmentation<PointT>::segment (PointIndices &inliers, ModelCoefficients &model_coefficients) 00076 { 00077 // Copy the header information 00078 inliers.header = model_coefficients.header = input_->header; 00079 00080 if (!initCompute ()) 00081 { 00082 inliers.indices.clear (); model_coefficients.values.clear (); 00083 return; 00084 } 00085 00086 // Initialize the Sample Consensus model and set its parameters 00087 if (!initSACModel (model_type_)) 00088 { 00089 PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ()); 00090 deinitCompute (); 00091 inliers.indices.clear (); model_coefficients.values.clear (); 00092 return; 00093 } 00094 // Initialize the Sample Consensus method and set its parameters 00095 initSAC (method_type_); 00096 00097 if (!sac_->computeModel (0)) 00098 { 00099 PCL_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ()); 00100 deinitCompute (); 00101 inliers.indices.clear (); model_coefficients.values.clear (); 00102 return; 00103 } 00104 00105 // Get the model inliers 00106 sac_->getInliers (inliers.indices); 00107 00108 // Get the model coefficients 00109 Eigen::VectorXf coeff; 00110 sac_->getModelCoefficients (coeff); 00111 00112 // If the user needs optimized coefficients 00113 if (optimize_coefficients_) 00114 { 00115 Eigen::VectorXf coeff_refined; 00116 model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined); 00117 model_coefficients.values.resize (coeff_refined.size ()); 00118 memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float)); 00119 // Refine inliers 00120 model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices); 00121 } 00122 else 00123 { 00124 model_coefficients.values.resize (coeff.size ()); 00125 memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float)); 00126 } 00127 00128 deinitCompute (); 00129 } 00130 00131 ////////////////////////////////////////////////////////////////////////////////////////////// 00132 template <typename PointT> bool 00133 pcl::SACSegmentation<PointT>::initSACModel (const int model_type) 00134 { 00135 if (model_) 00136 model_.reset (); 00137 00138 // Build the model 00139 switch (model_type) 00140 { 00141 case SACMODEL_PLANE: 00142 { 00143 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ()); 00144 model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_, random_)); 00145 break; 00146 } 00147 case SACMODEL_LINE: 00148 { 00149 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ()); 00150 model_.reset (new SampleConsensusModelLine<PointT> (input_, *indices_, random_)); 00151 break; 00152 } 00153 case SACMODEL_STICK: 00154 { 00155 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ()); 00156 model_.reset (new SampleConsensusModelStick<PointT> (input_, *indices_)); 00157 double min_radius, max_radius; 00158 model_->getRadiusLimits (min_radius, max_radius); 00159 if (radius_min_ != min_radius && radius_max_ != max_radius) 00160 { 00161 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00162 model_->setRadiusLimits (radius_min_, radius_max_); 00163 } 00164 break; 00165 } 00166 case SACMODEL_CIRCLE2D: 00167 { 00168 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ()); 00169 model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_, random_)); 00170 typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = boost::static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_); 00171 double min_radius, max_radius; 00172 model_circle->getRadiusLimits (min_radius, max_radius); 00173 if (radius_min_ != min_radius && radius_max_ != max_radius) 00174 { 00175 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00176 model_circle->setRadiusLimits (radius_min_, radius_max_); 00177 } 00178 break; 00179 } 00180 case SACMODEL_CIRCLE3D: 00181 { 00182 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ()); 00183 model_.reset (new SampleConsensusModelCircle3D<PointT> (input_, *indices_)); 00184 typename SampleConsensusModelCircle3D<PointT>::Ptr model_circle3d = boost::static_pointer_cast<SampleConsensusModelCircle3D<PointT> > (model_); 00185 double min_radius, max_radius; 00186 model_circle3d->getRadiusLimits (min_radius, max_radius); 00187 if (radius_min_ != min_radius && radius_max_ != max_radius) 00188 { 00189 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00190 model_circle3d->setRadiusLimits (radius_min_, radius_max_); 00191 } 00192 break; 00193 } 00194 case SACMODEL_SPHERE: 00195 { 00196 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ()); 00197 model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_, random_)); 00198 typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = boost::static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_); 00199 double min_radius, max_radius; 00200 model_sphere->getRadiusLimits (min_radius, max_radius); 00201 if (radius_min_ != min_radius && radius_max_ != max_radius) 00202 { 00203 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00204 model_sphere->setRadiusLimits (radius_min_, radius_max_); 00205 } 00206 break; 00207 } 00208 case SACMODEL_PARALLEL_LINE: 00209 { 00210 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ()); 00211 model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_, random_)); 00212 typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_); 00213 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_) 00214 { 00215 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00216 model_parallel->setAxis (axis_); 00217 } 00218 if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_) 00219 { 00220 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); 00221 model_parallel->setEpsAngle (eps_angle_); 00222 } 00223 break; 00224 } 00225 case SACMODEL_PERPENDICULAR_PLANE: 00226 { 00227 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ()); 00228 model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_, random_)); 00229 typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = boost::static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_); 00230 if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_) 00231 { 00232 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00233 model_perpendicular->setAxis (axis_); 00234 } 00235 if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_) 00236 { 00237 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); 00238 model_perpendicular->setEpsAngle (eps_angle_); 00239 } 00240 break; 00241 } 00242 case SACMODEL_PARALLEL_PLANE: 00243 { 00244 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ()); 00245 model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_, random_)); 00246 typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_); 00247 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_) 00248 { 00249 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00250 model_parallel->setAxis (axis_); 00251 } 00252 if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_) 00253 { 00254 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); 00255 model_parallel->setEpsAngle (eps_angle_); 00256 } 00257 break; 00258 } 00259 default: 00260 { 00261 PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); 00262 return (false); 00263 } 00264 } 00265 return (true); 00266 } 00267 00268 ////////////////////////////////////////////////////////////////////////////////////////////// 00269 template <typename PointT> void 00270 pcl::SACSegmentation<PointT>::initSAC (const int method_type) 00271 { 00272 if (sac_) 00273 sac_.reset (); 00274 // Build the sample consensus method 00275 switch (method_type) 00276 { 00277 case SAC_RANSAC: 00278 default: 00279 { 00280 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00281 sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_)); 00282 break; 00283 } 00284 case SAC_LMEDS: 00285 { 00286 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00287 sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_)); 00288 break; 00289 } 00290 case SAC_MSAC: 00291 { 00292 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00293 sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_)); 00294 break; 00295 } 00296 case SAC_RRANSAC: 00297 { 00298 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00299 sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_)); 00300 break; 00301 } 00302 case SAC_RMSAC: 00303 { 00304 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00305 sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_)); 00306 break; 00307 } 00308 case SAC_MLESAC: 00309 { 00310 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00311 sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_)); 00312 break; 00313 } 00314 case SAC_PROSAC: 00315 { 00316 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00317 sac_.reset (new ProgressiveSampleConsensus<PointT> (model_, threshold_)); 00318 break; 00319 } 00320 } 00321 // Set the Sample Consensus parameters if they are given/changed 00322 if (sac_->getProbability () != probability_) 00323 { 00324 PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_); 00325 sac_->setProbability (probability_); 00326 } 00327 if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_) 00328 { 00329 PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_); 00330 sac_->setMaxIterations (max_iterations_); 00331 } 00332 if (samples_radius_ > 0.) 00333 { 00334 PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_); 00335 // Set maximum distance for radius search during random sampling 00336 model_->setSamplesMaxDist (samples_radius_, samples_radius_search_); 00337 } 00338 } 00339 00340 ////////////////////////////////////////////////////////////////////////////////////////////// 00341 template <typename PointT, typename PointNT> bool 00342 pcl::SACSegmentationFromNormals<PointT, PointNT>::initSACModel (const int model_type) 00343 { 00344 if (!input_ || !normals_) 00345 { 00346 PCL_ERROR ("[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ()); 00347 return (false); 00348 } 00349 // Check if input is synced with the normals 00350 if (input_->points.size () != normals_->points.size ()) 00351 { 00352 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 ()); 00353 return (false); 00354 } 00355 00356 if (model_) 00357 model_.reset (); 00358 00359 // Build the model 00360 switch (model_type) 00361 { 00362 case SACMODEL_CYLINDER: 00363 { 00364 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); 00365 model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_, random_)); 00366 typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_); 00367 00368 // Set the input normals 00369 model_cylinder->setInputNormals (normals_); 00370 double min_radius, max_radius; 00371 model_cylinder->getRadiusLimits (min_radius, max_radius); 00372 if (radius_min_ != min_radius && radius_max_ != max_radius) 00373 { 00374 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00375 model_cylinder->setRadiusLimits (radius_min_, radius_max_); 00376 } 00377 if (distance_weight_ != model_cylinder->getNormalDistanceWeight ()) 00378 { 00379 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00380 model_cylinder->setNormalDistanceWeight (distance_weight_); 00381 } 00382 if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_) 00383 { 00384 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00385 model_cylinder->setAxis (axis_); 00386 } 00387 if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_) 00388 { 00389 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); 00390 model_cylinder->setEpsAngle (eps_angle_); 00391 } 00392 break; 00393 } 00394 case SACMODEL_NORMAL_PLANE: 00395 { 00396 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ()); 00397 model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_, random_)); 00398 typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_); 00399 // Set the input normals 00400 model_normals->setInputNormals (normals_); 00401 if (distance_weight_ != model_normals->getNormalDistanceWeight ()) 00402 { 00403 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00404 model_normals->setNormalDistanceWeight (distance_weight_); 00405 } 00406 break; 00407 } 00408 case SACMODEL_NORMAL_PARALLEL_PLANE: 00409 { 00410 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); 00411 model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_, random_)); 00412 typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_); 00413 // Set the input normals 00414 model_normals->setInputNormals (normals_); 00415 if (distance_weight_ != model_normals->getNormalDistanceWeight ()) 00416 { 00417 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00418 model_normals->setNormalDistanceWeight (distance_weight_); 00419 } 00420 if (distance_from_origin_ != model_normals->getDistanceFromOrigin ()) 00421 { 00422 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_); 00423 model_normals->setDistanceFromOrigin (distance_from_origin_); 00424 } 00425 if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_) 00426 { 00427 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00428 model_normals->setAxis (axis_); 00429 } 00430 if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_) 00431 { 00432 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); 00433 model_normals->setEpsAngle (eps_angle_); 00434 } 00435 break; 00436 } 00437 case SACMODEL_CONE: 00438 { 00439 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ()); 00440 model_.reset (new SampleConsensusModelCone<PointT, PointNT> (input_, *indices_, random_)); 00441 typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone = boost::static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_); 00442 00443 // Set the input normals 00444 model_cone->setInputNormals (normals_); 00445 double min_angle, max_angle; 00446 model_cone->getMinMaxOpeningAngle(min_angle, max_angle); 00447 if (min_angle_ != min_angle && max_angle_ != max_angle) 00448 { 00449 PCL_DEBUG ("[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_); 00450 model_cone->setMinMaxOpeningAngle (min_angle_, max_angle_); 00451 } 00452 00453 if (distance_weight_ != model_cone->getNormalDistanceWeight ()) 00454 { 00455 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00456 model_cone->setNormalDistanceWeight (distance_weight_); 00457 } 00458 if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_) 00459 { 00460 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00461 model_cone->setAxis (axis_); 00462 } 00463 if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_) 00464 { 00465 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); 00466 model_cone->setEpsAngle (eps_angle_); 00467 } 00468 break; 00469 } 00470 case SACMODEL_NORMAL_SPHERE: 00471 { 00472 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ()); 00473 model_.reset (new SampleConsensusModelNormalSphere<PointT, PointNT> (input_, *indices_, random_)); 00474 typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere = boost::static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_); 00475 // Set the input normals 00476 model_normals_sphere->setInputNormals (normals_); 00477 double min_radius, max_radius; 00478 model_normals_sphere->getRadiusLimits (min_radius, max_radius); 00479 if (radius_min_ != min_radius && radius_max_ != max_radius) 00480 { 00481 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00482 model_normals_sphere->setRadiusLimits (radius_min_, radius_max_); 00483 } 00484 00485 if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ()) 00486 { 00487 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00488 model_normals_sphere->setNormalDistanceWeight (distance_weight_); 00489 } 00490 break; 00491 } 00492 // If nothing else, try SACSegmentation 00493 default: 00494 { 00495 return (pcl::SACSegmentation<PointT>::initSACModel (model_type)); 00496 } 00497 } 00498 00499 return (true); 00500 } 00501 00502 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>; 00503 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>; 00504 00505 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_ 00506