|
Point Cloud Library (PCL)
1.5.1
|
|
Point Cloud Library (PCL)
1.5.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 4702 2012-02-23 09:39:33Z gedikli $ 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_line.h" 00058 #include "pcl/sample_consensus/sac_model_normal_plane.h" 00059 #include "pcl/sample_consensus/sac_model_parallel_plane.h" 00060 #include "pcl/sample_consensus/sac_model_normal_parallel_plane.h" 00061 #include "pcl/sample_consensus/sac_model_parallel_line.h" 00062 #include "pcl/sample_consensus/sac_model_perpendicular_plane.h" 00063 #include "pcl/sample_consensus/sac_model_plane.h" 00064 #include "pcl/sample_consensus/sac_model_sphere.h" 00065 #include "pcl/sample_consensus/sac_model_stick.h" 00066 00068 template <typename PointT> void 00069 pcl::SACSegmentation<PointT>::segment (PointIndices &inliers, ModelCoefficients &model_coefficients) 00070 { 00071 // Copy the header information 00072 inliers.header = model_coefficients.header = input_->header; 00073 00074 if (!initCompute ()) 00075 { 00076 inliers.indices.clear (); model_coefficients.values.clear (); 00077 return; 00078 } 00079 00080 // Initialize the Sample Consensus model and set its parameters 00081 if (!initSACModel (model_type_)) 00082 { 00083 PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ()); 00084 deinitCompute (); 00085 inliers.indices.clear (); model_coefficients.values.clear (); 00086 return; 00087 } 00088 // Initialize the Sample Consensus method and set its parameters 00089 initSAC (method_type_); 00090 00091 if (!sac_->computeModel (0)) 00092 { 00093 PCL_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ()); 00094 deinitCompute (); 00095 inliers.indices.clear (); model_coefficients.values.clear (); 00096 return; 00097 } 00098 00099 // Get the model inliers 00100 sac_->getInliers (inliers.indices); 00101 00102 // Get the model coefficients 00103 Eigen::VectorXf coeff; 00104 sac_->getModelCoefficients (coeff); 00105 00106 // If the user needs optimized coefficients 00107 if (optimize_coefficients_) 00108 { 00109 Eigen::VectorXf coeff_refined; 00110 model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined); 00111 model_coefficients.values.resize (coeff_refined.size ()); 00112 memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float)); 00113 // Refine inliers 00114 model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices); 00115 } 00116 else 00117 { 00118 model_coefficients.values.resize (coeff.size ()); 00119 memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float)); 00120 } 00121 00122 deinitCompute (); 00123 } 00124 00126 template <typename PointT> bool 00127 pcl::SACSegmentation<PointT>::initSACModel (const int model_type) 00128 { 00129 if (model_) 00130 model_.reset (); 00131 00132 // Build the model 00133 switch (model_type) 00134 { 00135 case SACMODEL_PLANE: 00136 { 00137 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ()); 00138 model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_)); 00139 break; 00140 } 00141 case SACMODEL_LINE: 00142 { 00143 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ()); 00144 model_.reset (new SampleConsensusModelLine<PointT> (input_, *indices_)); 00145 break; 00146 } 00147 case SACMODEL_STICK: 00148 { 00149 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ()); 00150 model_.reset (new SampleConsensusModelStick<PointT> (input_, *indices_)); 00151 double min_radius, max_radius; 00152 model_->getRadiusLimits (min_radius, max_radius); 00153 if (radius_min_ != min_radius && radius_max_ != max_radius) 00154 { 00155 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00156 model_->setRadiusLimits (radius_min_, radius_max_); 00157 } 00158 break; 00159 } 00160 case SACMODEL_CIRCLE2D: 00161 { 00162 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ()); 00163 model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_)); 00164 typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = boost::static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_); 00165 double min_radius, max_radius; 00166 model_circle->getRadiusLimits (min_radius, max_radius); 00167 if (radius_min_ != min_radius && radius_max_ != max_radius) 00168 { 00169 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00170 model_circle->setRadiusLimits (radius_min_, radius_max_); 00171 } 00172 break; 00173 } 00174 case SACMODEL_SPHERE: 00175 { 00176 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ()); 00177 model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_)); 00178 typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = boost::static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_); 00179 double min_radius, max_radius; 00180 model_sphere->getRadiusLimits (min_radius, max_radius); 00181 if (radius_min_ != min_radius && radius_max_ != max_radius) 00182 { 00183 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00184 model_sphere->setRadiusLimits (radius_min_, radius_max_); 00185 } 00186 break; 00187 } 00188 case SACMODEL_PARALLEL_LINE: 00189 { 00190 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ()); 00191 model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_)); 00192 typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_); 00193 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_) 00194 { 00195 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00196 model_parallel->setAxis (axis_); 00197 } 00198 if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_) 00199 { 00200 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); 00201 model_parallel->setEpsAngle (eps_angle_); 00202 } 00203 break; 00204 } 00205 case SACMODEL_PERPENDICULAR_PLANE: 00206 { 00207 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ()); 00208 model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_)); 00209 typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = boost::static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_); 00210 if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_) 00211 { 00212 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00213 model_perpendicular->setAxis (axis_); 00214 } 00215 if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_) 00216 { 00217 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); 00218 model_perpendicular->setEpsAngle (eps_angle_); 00219 } 00220 break; 00221 } 00222 case SACMODEL_PARALLEL_PLANE: 00223 { 00224 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ()); 00225 model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_)); 00226 typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_); 00227 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_) 00228 { 00229 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00230 model_parallel->setAxis (axis_); 00231 } 00232 if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_) 00233 { 00234 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); 00235 model_parallel->setEpsAngle (eps_angle_); 00236 } 00237 break; 00238 } 00239 default: 00240 { 00241 PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); 00242 return (false); 00243 } 00244 } 00245 return (true); 00246 } 00247 00249 template <typename PointT> void 00250 pcl::SACSegmentation<PointT>::initSAC (const int method_type) 00251 { 00252 if (sac_) 00253 sac_.reset (); 00254 // Build the sample consensus method 00255 switch (method_type) 00256 { 00257 case SAC_RANSAC: 00258 default: 00259 { 00260 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00261 sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_)); 00262 break; 00263 } 00264 case SAC_LMEDS: 00265 { 00266 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00267 sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_)); 00268 break; 00269 } 00270 case SAC_MSAC: 00271 { 00272 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00273 sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_)); 00274 break; 00275 } 00276 case SAC_RRANSAC: 00277 { 00278 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00279 sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_)); 00280 break; 00281 } 00282 case SAC_RMSAC: 00283 { 00284 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00285 sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_)); 00286 break; 00287 } 00288 case SAC_MLESAC: 00289 { 00290 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00291 sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_)); 00292 break; 00293 } 00294 case SAC_PROSAC: 00295 { 00296 PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); 00297 sac_.reset (new ProgressiveSampleConsensus<PointT> (model_, threshold_)); 00298 break; 00299 } 00300 } 00301 // Set the Sample Consensus parameters if they are given/changed 00302 if (sac_->getProbability () != probability_) 00303 { 00304 PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_); 00305 sac_->setProbability (probability_); 00306 } 00307 if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_) 00308 { 00309 PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_); 00310 sac_->setMaxIterations (max_iterations_); 00311 } 00312 } 00313 00315 template <typename PointT, typename PointNT> bool 00316 pcl::SACSegmentationFromNormals<PointT, PointNT>::initSACModel (const int model_type) 00317 { 00318 if (!input_ || !normals_) 00319 { 00320 PCL_ERROR ("[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ()); 00321 return (false); 00322 } 00323 // Check if input is synced with the normals 00324 if (input_->points.size () != normals_->points.size ()) 00325 { 00326 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 ()); 00327 return (false); 00328 } 00329 00330 if (model_) 00331 model_.reset (); 00332 00333 // Build the model 00334 switch (model_type) 00335 { 00336 case SACMODEL_CYLINDER: 00337 { 00338 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); 00339 model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_)); 00340 typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_); 00341 00342 // Set the input normals 00343 model_cylinder->setInputNormals (normals_); 00344 double min_radius, max_radius; 00345 model_cylinder->getRadiusLimits (min_radius, max_radius); 00346 if (radius_min_ != min_radius && radius_max_ != max_radius) 00347 { 00348 PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); 00349 model_cylinder->setRadiusLimits (radius_min_, radius_max_); 00350 } 00351 if (distance_weight_ != model_cylinder->getNormalDistanceWeight ()) 00352 { 00353 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00354 model_cylinder->setNormalDistanceWeight (distance_weight_); 00355 } 00356 if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_) 00357 { 00358 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00359 model_cylinder->setAxis (axis_); 00360 } 00361 if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_) 00362 { 00363 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); 00364 model_cylinder->setEpsAngle (eps_angle_); 00365 } 00366 break; 00367 } 00368 case SACMODEL_NORMAL_PLANE: 00369 { 00370 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ()); 00371 model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_)); 00372 typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_); 00373 // Set the input normals 00374 model_normals->setInputNormals (normals_); 00375 if (distance_weight_ != model_normals->getNormalDistanceWeight ()) 00376 { 00377 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00378 model_normals->setNormalDistanceWeight (distance_weight_); 00379 } 00380 break; 00381 } 00382 case SACMODEL_NORMAL_PARALLEL_PLANE: 00383 { 00384 PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); 00385 model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_)); 00386 typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_); 00387 // Set the input normals 00388 model_normals->setInputNormals (normals_); 00389 if (distance_weight_ != model_normals->getNormalDistanceWeight ()) 00390 { 00391 PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); 00392 model_normals->setNormalDistanceWeight (distance_weight_); 00393 } 00394 if (distance_from_origin_ != model_normals->getDistanceFromOrigin ()) 00395 { 00396 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_); 00397 model_normals->setDistanceFromOrigin (distance_from_origin_); 00398 } 00399 if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_) 00400 { 00401 PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); 00402 model_normals->setAxis (axis_); 00403 } 00404 if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_) 00405 { 00406 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); 00407 model_normals->setEpsAngle (eps_angle_); 00408 } 00409 break; 00410 } 00411 // If nothing else, try SACSegmentation 00412 default: 00413 { 00414 return (pcl::SACSegmentation<PointT>::initSACModel (model_type)); 00415 } 00416 } 00417 return (true); 00418 } 00419 00420 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>; 00421 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>; 00422 00423 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_ 00424
1.8.0
