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