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  *  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 
00047 template <typename PointT, typename PointNT> bool
00048 pcl::SampleConsensusModelCone<PointT, PointNT>::isSampleGood(const std::vector<int> &) const
00049 {
00050   return (true);
00051 }
00052 
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 
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 
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 
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 
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 
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 
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 
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 
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 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:16