|
Point Cloud Library (PCL)
1.6.0
|
|
Point Cloud Library (PCL)
1.6.0
|
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 6155 2012-07-04 23:10:00Z aichim $ 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/sac.h> 00045 #include <pcl/sample_consensus/lmeds.h> 00046 #include <pcl/sample_consensus/mlesac.h> 00047 #include <pcl/sample_consensus/msac.h> 00048 #include <pcl/sample_consensus/ransac.h> 00049 #include <pcl/sample_consensus/rmsac.h> 00050 #include <pcl/sample_consensus/rransac.h> 00051 #include <pcl/sample_consensus/prosac.h> 00052 00053 // Sample Consensus models 00054 #include <pcl/sample_consensus/sac_model.h> 00055 #include <pcl/sample_consensus/sac_model_circle.h> 00056 #include <pcl/sample_consensus/sac_model_cylinder.h> 00057 #include <pcl/sample_consensus/sac_model_cone.h> 00058 #include <pcl/sample_consensus/sac_model_line.h> 00059 #include <pcl/sample_consensus/sac_model_normal_plane.h> 00060 #include <pcl/sample_consensus/sac_model_normal_sphere.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 00248 if (samples_radius_ > 0. ) 00249 { 00250 PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum distance to %f\n", getClassName ().c_str (), samples_radius_); 00251 // Set maximum distance for radius search during random sampling 00252 model_->setSamplesMaxDist(samples_radius_, samples_radius_search_); 00253 } 00254 00255 return (true); 00256 } 00257 00259 template <typename PointT> void 00260 pcl::SACSegmentation<PointT>::initSAC (const int method_type) 00261 { 00262 if (sac_) 00263 sac_.reset (); 00264 // Build the sample consensus method 00265 switch (method_type) 00266 { 00267 case SAC_RANSAC: 00268 default: 00269 { 00270 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00271 sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_)); 00272 break; 00273 } 00274 case SAC_LMEDS: 00275 { 00276 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00277 sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_)); 00278 break; 00279 } 00280 case SAC_MSAC: 00281 { 00282 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00283 sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_)); 00284 break; 00285 } 00286 case SAC_RRANSAC: 00287 { 00288 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00289 sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_)); 00290 break; 00291 } 00292 case SAC_RMSAC: 00293 { 00294 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00295 sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_)); 00296 break; 00297 } 00298 case SAC_MLESAC: 00299 { 00300 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00301 sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_)); 00302 break; 00303 } 00304 case SAC_PROSAC: 00305 { 00306 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00307 sac_.reset (new ProgressiveSampleConsensus<PointT> (model_, threshold_)); 00308 break; 00309 } 00310 } 00311 // Set the Sample Consensus parameters if they are given/changed 00312 if (sac_->getProbability () != probability_) 00313 { 00314 PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_); 00315 sac_->setProbability (probability_); 00316 } 00317 if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_) 00318 { 00319 PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_); 00320 sac_->setMaxIterations (max_iterations_); 00321 } 00322 } 00323 00325 template <typename PointT, typename PointNT> bool 00326 pcl::SACSegmentationFromNormals<PointT, PointNT>::initSACModel (const int model_type) 00327 { 00328 if (!input_ || !normals_) 00329 { 00330 PCL_ERROR ("[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ()); 00331 return (false); 00332 } 00333 // Check if input is synced with the normals 00334 if (input_->points.size () != normals_->points.size ()) 00335 { 00336 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 ()); 00337 return (false); 00338 } 00339 00340 if (model_) 00341 model_.reset (); 00342 00343 // Build the model 00344 switch (model_type) 00345 { 00346 case SACMODEL_CYLINDER: 00347 { 00348 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); 00349 model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_)); 00350 typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_); 00351 00352 // Set the input normals 00353 model_cylinder->setInputNormals (normals_); 00354 double min_radius, max_radius; 00355 model_cylinder->getRadiusLimits (min_radius, max_radius); 00356 if (radius_min_ != min_radius && radius_max_ != max_radius) 00357 { 00358 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00359 model_cylinder->setRadiusLimits (radius_min_, radius_max_); 00360 } 00361 if (distance_weight_ != model_cylinder->getNormalDistanceWeight ()) 00362 { 00363 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00364 model_cylinder->setNormalDistanceWeight (distance_weight_); 00365 } 00366 if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_) 00367 { 00368 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00369 model_cylinder->setAxis (axis_); 00370 } 00371 if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_) 00372 { 00373 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); 00374 model_cylinder->setEpsAngle (eps_angle_); 00375 } 00376 break; 00377 } 00378 case SACMODEL_NORMAL_PLANE: 00379 { 00380 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ()); 00381 model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_)); 00382 typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_); 00383 // Set the input normals 00384 model_normals->setInputNormals (normals_); 00385 if (distance_weight_ != model_normals->getNormalDistanceWeight ()) 00386 { 00387 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00388 model_normals->setNormalDistanceWeight (distance_weight_); 00389 } 00390 break; 00391 } 00392 case SACMODEL_NORMAL_PARALLEL_PLANE: 00393 { 00394 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); 00395 model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_)); 00396 typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_); 00397 // Set the input normals 00398 model_normals->setInputNormals (normals_); 00399 if (distance_weight_ != model_normals->getNormalDistanceWeight ()) 00400 { 00401 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00402 model_normals->setNormalDistanceWeight (distance_weight_); 00403 } 00404 if (distance_from_origin_ != model_normals->getDistanceFromOrigin ()) 00405 { 00406 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_); 00407 model_normals->setDistanceFromOrigin (distance_from_origin_); 00408 } 00409 if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_) 00410 { 00411 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00412 model_normals->setAxis (axis_); 00413 } 00414 if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_) 00415 { 00416 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); 00417 model_normals->setEpsAngle (eps_angle_); 00418 } 00419 break; 00420 } 00421 case SACMODEL_CONE: 00422 { 00423 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ()); 00424 model_.reset (new SampleConsensusModelCone<PointT, PointNT > (input_, *indices_)); 00425 typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone = boost::static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_); 00426 00427 // Set the input normals 00428 model_cone->setInputNormals (normals_); 00429 double min_angle, max_angle; 00430 model_cone->getMinMaxOpeningAngle(min_angle, max_angle); 00431 if (min_angle_ != min_angle && max_angle_ != max_angle) 00432 { 00433 PCL_DEBUG ("[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_); 00434 model_cone->setMinMaxOpeningAngle (min_angle_, max_angle_); 00435 } 00436 00437 if (distance_weight_ != model_cone->getNormalDistanceWeight ()) 00438 { 00439 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00440 model_cone->setNormalDistanceWeight (distance_weight_); 00441 } 00442 if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_) 00443 { 00444 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00445 model_cone->setAxis (axis_); 00446 } 00447 if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_) 00448 { 00449 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); 00450 model_cone->setEpsAngle (eps_angle_); 00451 } 00452 break; 00453 } 00454 case SACMODEL_NORMAL_SPHERE: 00455 { 00456 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ()); 00457 model_.reset (new SampleConsensusModelNormalSphere<PointT, PointNT> (input_, *indices_)); 00458 typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere = boost::static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_); 00459 // Set the input normals 00460 model_normals_sphere->setInputNormals (normals_); 00461 double min_radius, max_radius; 00462 model_normals_sphere->getRadiusLimits (min_radius, max_radius); 00463 if (radius_min_ != min_radius && radius_max_ != max_radius) 00464 { 00465 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00466 model_normals_sphere->setRadiusLimits (radius_min_, radius_max_); 00467 } 00468 00469 if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ()) 00470 { 00471 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00472 model_normals_sphere->setNormalDistanceWeight (distance_weight_); 00473 } 00474 break; 00475 } 00476 // If nothing else, try SACSegmentation 00477 default: 00478 { 00479 return (pcl::SACSegmentation<PointT>::initSACModel (model_type)); 00480 } 00481 } 00482 00483 if (SACSegmentation<PointT>::samples_radius_ > 0. ) 00484 { 00485 PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum distance to %f\n", getClassName ().c_str (), SACSegmentation<PointT>::samples_radius_); 00486 // Set maximum distance for radius search during random sampling 00487 model_->setSamplesMaxDist(SACSegmentation<PointT>::samples_radius_, SACSegmentation<PointT>::samples_radius_search_); 00488 } 00489 00490 return (true); 00491 } 00492 00493 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>; 00494 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>; 00495 00496 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_ 00497
1.7.6.1
