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-2010, 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_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ 00042 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ 00043 00044 #include <pcl/sample_consensus/eigen.h> 00045 #include <pcl/sample_consensus/sac_model_cylinder.h> 00046 #include <pcl/common/concatenate.h> 00047 00048 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00049 template <typename PointT, typename PointNT> bool 00050 pcl::SampleConsensusModelCylinder<PointT, PointNT>::isSampleGood (const std::vector<int> &) const 00051 { 00052 return (true); 00053 } 00054 00055 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00056 template <typename PointT, typename PointNT> bool 00057 pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients ( 00058 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) 00059 { 00060 // Need 2 samples 00061 if (samples.size () != 2) 00062 { 00063 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); 00064 return (false); 00065 } 00066 00067 if (!normals_) 00068 { 00069 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n"); 00070 return (false); 00071 } 00072 00073 if (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () && 00074 fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () && 00075 fabs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ()) 00076 { 00077 return (false); 00078 } 00079 00080 Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0); 00081 Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0); 00082 00083 Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0); 00084 Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0); 00085 Eigen::Vector4f w = n1 + p1 - p2; 00086 00087 float a = n1.dot (n1); 00088 float b = n1.dot (n2); 00089 float c = n2.dot (n2); 00090 float d = n1.dot (w); 00091 float e = n2.dot (w); 00092 float denominator = a*c - b*b; 00093 float sc, tc; 00094 // Compute the line parameters of the two closest points 00095 if (denominator < 1e-8) // The lines are almost parallel 00096 { 00097 sc = 0.0f; 00098 tc = (b > c ? d / b : e / c); // Use the largest denominator 00099 } 00100 else 00101 { 00102 sc = (b*e - c*d) / denominator; 00103 tc = (a*e - b*d) / denominator; 00104 } 00105 00106 // point_on_axis, axis_direction 00107 Eigen::Vector4f line_pt = p1 + n1 + sc * n1; 00108 Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt; 00109 line_dir.normalize (); 00110 00111 model_coefficients.resize (7); 00112 // model_coefficients.template head<3> () = line_pt.template head<3> (); 00113 model_coefficients[0] = line_pt[0]; 00114 model_coefficients[1] = line_pt[1]; 00115 model_coefficients[2] = line_pt[2]; 00116 // model_coefficients.template segment<3> (3) = line_dir.template head<3> (); 00117 model_coefficients[3] = line_dir[0]; 00118 model_coefficients[4] = line_dir[1]; 00119 model_coefficients[5] = line_dir[2]; 00120 // cylinder radius 00121 model_coefficients[6] = static_cast<float> (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir))); 00122 00123 if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_) 00124 return (false); 00125 00126 return (true); 00127 } 00128 00129 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00130 template <typename PointT, typename PointNT> void 00131 pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel ( 00132 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00133 { 00134 // Check if the model is valid given the user constraints 00135 if (!isModelValid (model_coefficients)) 00136 { 00137 distances.clear (); 00138 return; 00139 } 00140 00141 distances.resize (indices_->size ()); 00142 00143 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00144 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00145 float ptdotdir = line_pt.dot (line_dir); 00146 float dirdotdir = 1.0f / line_dir.dot (line_dir); 00147 // Iterate through the 3d points and calculate the distances from them to the sphere 00148 for (size_t i = 0; i < indices_->size (); ++i) 00149 { 00150 // Aproximate the distance from the point to the cylinder as the difference between 00151 // dist(point,cylinder_axis) and cylinder radius 00152 // @note need to revise this. 00153 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); 00154 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); 00155 00156 double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); 00157 00158 // Calculate the point's projection on the cylinder axis 00159 float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; 00160 Eigen::Vector4f pt_proj = line_pt + k * line_dir; 00161 Eigen::Vector4f dir = pt - pt_proj; 00162 dir.normalize (); 00163 00164 // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector 00165 double d_normal = fabs (getAngle3D (n, dir)); 00166 d_normal = (std::min) (d_normal, M_PI - d_normal); 00167 00168 distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); 00169 } 00170 } 00171 00172 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00173 template <typename PointT, typename PointNT> void 00174 pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance ( 00175 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 00176 { 00177 // Check if the model is valid given the user constraints 00178 if (!isModelValid (model_coefficients)) 00179 { 00180 inliers.clear (); 00181 return; 00182 } 00183 00184 int nr_p = 0; 00185 inliers.resize (indices_->size ()); 00186 error_sqr_dists_.resize (indices_->size ()); 00187 00188 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00189 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00190 float ptdotdir = line_pt.dot (line_dir); 00191 float dirdotdir = 1.0f / line_dir.dot (line_dir); 00192 // Iterate through the 3d points and calculate the distances from them to the sphere 00193 for (size_t i = 0; i < indices_->size (); ++i) 00194 { 00195 // Aproximate the distance from the point to the cylinder as the difference between 00196 // dist(point,cylinder_axis) and cylinder radius 00197 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); 00198 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); 00199 double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); 00200 00201 // Calculate the point's projection on the cylinder axis 00202 float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; 00203 Eigen::Vector4f pt_proj = line_pt + k * line_dir; 00204 Eigen::Vector4f dir = pt - pt_proj; 00205 dir.normalize (); 00206 00207 // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector 00208 double d_normal = fabs (getAngle3D (n, dir)); 00209 d_normal = (std::min) (d_normal, M_PI - d_normal); 00210 00211 double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); 00212 if (distance < threshold) 00213 { 00214 // Returns the indices of the points whose distances are smaller than the threshold 00215 inliers[nr_p] = (*indices_)[i]; 00216 error_sqr_dists_[nr_p] = distance; 00217 ++nr_p; 00218 } 00219 } 00220 inliers.resize (nr_p); 00221 error_sqr_dists_.resize (nr_p); 00222 } 00223 00224 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00225 template <typename PointT, typename PointNT> int 00226 pcl::SampleConsensusModelCylinder<PointT, PointNT>::countWithinDistance ( 00227 const Eigen::VectorXf &model_coefficients, const double threshold) 00228 { 00229 // Check if the model is valid given the user constraints 00230 if (!isModelValid (model_coefficients)) 00231 return (0); 00232 00233 int nr_p = 0; 00234 00235 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00236 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00237 float ptdotdir = line_pt.dot (line_dir); 00238 float dirdotdir = 1.0f / line_dir.dot (line_dir); 00239 // Iterate through the 3d points and calculate the distances from them to the sphere 00240 for (size_t i = 0; i < indices_->size (); ++i) 00241 { 00242 // Aproximate the distance from the point to the cylinder as the difference between 00243 // dist(point,cylinder_axis) and cylinder radius 00244 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); 00245 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); 00246 double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); 00247 00248 // Calculate the point's projection on the cylinder axis 00249 float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; 00250 Eigen::Vector4f pt_proj = line_pt + k * line_dir; 00251 Eigen::Vector4f dir = pt - pt_proj; 00252 dir.normalize (); 00253 00254 // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector 00255 double d_normal = fabs (getAngle3D (n, dir)); 00256 d_normal = (std::min) (d_normal, M_PI - d_normal); 00257 00258 if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) 00259 nr_p++; 00260 } 00261 return (nr_p); 00262 } 00263 00264 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00265 template <typename PointT, typename PointNT> void 00266 pcl::SampleConsensusModelCylinder<PointT, PointNT>::optimizeModelCoefficients ( 00267 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 00268 { 00269 optimized_coefficients = model_coefficients; 00270 00271 // Needs a set of valid model coefficients 00272 if (model_coefficients.size () != 7) 00273 { 00274 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00275 return; 00276 } 00277 00278 if (inliers.empty ()) 00279 { 00280 PCL_DEBUG ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n"); 00281 return; 00282 } 00283 00284 tmp_inliers_ = &inliers; 00285 00286 OptimizationFunctor functor (static_cast<int> (inliers.size ()), this); 00287 Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor); 00288 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff); 00289 int info = lm.minimize (optimized_coefficients); 00290 00291 // Compute the L2 norm of the residuals 00292 PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n", 00293 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], 00294 model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]); 00295 00296 Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]); 00297 line_dir.normalize (); 00298 optimized_coefficients[3] = line_dir[0]; 00299 optimized_coefficients[4] = line_dir[1]; 00300 optimized_coefficients[5] = line_dir[2]; 00301 } 00302 00303 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00304 template <typename PointT, typename PointNT> void 00305 pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints ( 00306 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) 00307 { 00308 // Needs a valid set of model coefficients 00309 if (model_coefficients.size () != 7) 00310 { 00311 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00312 return; 00313 } 00314 00315 projected_points.header = input_->header; 00316 projected_points.is_dense = input_->is_dense; 00317 00318 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00319 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00320 float ptdotdir = line_pt.dot (line_dir); 00321 float dirdotdir = 1.0f / line_dir.dot (line_dir); 00322 00323 // Copy all the data fields from the input cloud to the projected one? 00324 if (copy_data_fields) 00325 { 00326 // Allocate enough space and copy the basics 00327 projected_points.points.resize (input_->points.size ()); 00328 projected_points.width = input_->width; 00329 projected_points.height = input_->height; 00330 00331 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00332 // Iterate over each point 00333 for (size_t i = 0; i < projected_points.points.size (); ++i) 00334 // Iterate over each dimension 00335 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); 00336 00337 // Iterate through the 3d points and calculate the distances from them to the cylinder 00338 for (size_t i = 0; i < inliers.size (); ++i) 00339 { 00340 Eigen::Vector4f p (input_->points[inliers[i]].x, 00341 input_->points[inliers[i]].y, 00342 input_->points[inliers[i]].z, 00343 1); 00344 00345 float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; 00346 00347 pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap (); 00348 pp.matrix () = line_pt + k * line_dir; 00349 00350 Eigen::Vector4f dir = p - pp; 00351 dir.normalize (); 00352 00353 // Calculate the projection of the point onto the cylinder 00354 pp += dir * model_coefficients[6]; 00355 } 00356 } 00357 else 00358 { 00359 // Allocate enough space and copy the basics 00360 projected_points.points.resize (inliers.size ()); 00361 projected_points.width = static_cast<uint32_t> (inliers.size ()); 00362 projected_points.height = 1; 00363 00364 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00365 // Iterate over each point 00366 for (size_t i = 0; i < inliers.size (); ++i) 00367 // Iterate over each dimension 00368 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); 00369 00370 // Iterate through the 3d points and calculate the distances from them to the plane 00371 for (size_t i = 0; i < inliers.size (); ++i) 00372 { 00373 pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap (); 00374 pcl::Vector4fMapConst p = input_->points[inliers[i]].getVector4fMap (); 00375 00376 float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; 00377 // Calculate the projection of the point on the line 00378 pp.matrix () = line_pt + k * line_dir; 00379 00380 Eigen::Vector4f dir = p - pp; 00381 dir.normalize (); 00382 00383 // Calculate the projection of the point onto the cylinder 00384 pp += dir * model_coefficients[6]; 00385 } 00386 } 00387 } 00388 00389 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00390 template <typename PointT, typename PointNT> bool 00391 pcl::SampleConsensusModelCylinder<PointT, PointNT>::doSamplesVerifyModel ( 00392 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) 00393 { 00394 // Needs a valid model coefficients 00395 if (model_coefficients.size () != 7) 00396 { 00397 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00398 return (false); 00399 } 00400 00401 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00402 { 00403 // Aproximate the distance from the point to the cylinder as the difference between 00404 // dist(point,cylinder_axis) and cylinder radius 00405 // @note need to revise this. 00406 Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0); 00407 if (fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold) 00408 return (false); 00409 } 00410 00411 return (true); 00412 } 00413 00414 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00415 template <typename PointT, typename PointNT> double 00416 pcl::SampleConsensusModelCylinder<PointT, PointNT>::pointToLineDistance ( 00417 const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) 00418 { 00419 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00420 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00421 return sqrt(pcl::sqrPointToLineDistance (pt, line_pt, line_dir)); 00422 } 00423 00424 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00425 template <typename PointT, typename PointNT> void 00426 pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPointToCylinder ( 00427 const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) 00428 { 00429 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00430 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00431 00432 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir); 00433 pt_proj = line_pt + k * line_dir; 00434 00435 Eigen::Vector4f dir = pt - pt_proj; 00436 dir.normalize (); 00437 00438 // Calculate the projection of the point onto the cylinder 00439 pt_proj += dir * model_coefficients[6]; 00440 } 00441 00442 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00443 template <typename PointT, typename PointNT> bool 00444 pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) 00445 { 00446 // Needs a valid model coefficients 00447 if (model_coefficients.size () != 7) 00448 { 00449 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00450 return (false); 00451 } 00452 00453 // Check against template, if given 00454 if (eps_angle_ > 0.0) 00455 { 00456 // Obtain the cylinder direction 00457 Eigen::Vector4f coeff; 00458 coeff[0] = model_coefficients[3]; 00459 coeff[1] = model_coefficients[4]; 00460 coeff[2] = model_coefficients[5]; 00461 coeff[3] = 0; 00462 00463 Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); 00464 double angle_diff = fabs (getAngle3D (axis, coeff)); 00465 angle_diff = (std::min) (angle_diff, M_PI - angle_diff); 00466 // Check whether the current cylinder model satisfies our angle threshold criterion with respect to the given axis 00467 if (angle_diff > eps_angle_) 00468 return (false); 00469 } 00470 00471 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[6] < radius_min_) 00472 return (false); 00473 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[6] > radius_max_) 00474 return (false); 00475 00476 return (true); 00477 } 00478 00479 #define PCL_INSTANTIATE_SampleConsensusModelCylinder(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCylinder<PointT, PointNT>; 00480 00481 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ 00482