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


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:43