Point Cloud Library (PCL)  1.3.1
sac_model_circle.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: sac_model_circle.hpp 3027 2011-11-01 04:04:27Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
00040 
00041 #include "pcl/sample_consensus/sac_model_circle.h"
00042 #include "pcl/common/concatenate.h"
00043 #include <unsupported/Eigen/NonLinearOptimization>
00044 
00046 template <typename PointT> bool
00047 pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const std::vector<int> &samples) const
00048 {
00049   // Get the values at the two points
00050   Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
00051   Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
00052   Eigen::Array2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y);
00053 
00054   // Compute the segment values (in 2d) between p1 and p0
00055   p1 -= p0;
00056   // Compute the segment values (in 2d) between p2 and p0
00057   p2 -= p0;
00058 
00059   Eigen::Array2d dy1dy2 = p1 / p2;
00060 
00061   return (dy1dy2[0] != dy1dy2[1]);
00062 }
00063 
00065 template <typename PointT> bool
00066 pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
00067 {
00068   // Need 3 samples
00069   if (samples.size () != 3)
00070   {
00071     PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", (unsigned long)samples.size ());
00072     return (false);
00073   }
00074 
00075   model_coefficients.resize (3);
00076 
00077   Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
00078   Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
00079   Eigen::Vector2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y);
00080 
00081   Eigen::Vector2d u = (p0 + p1) / 2.0;
00082   Eigen::Vector2d v = (p1 + p2) / 2.0;
00083 
00084   Eigen::Vector2d p1p0dif = p1 - p0;
00085   Eigen::Vector2d p2p1dif = p2 - p1;
00086   Eigen::Vector2d uvdif   = u - v;
00087 
00088   Eigen::Vector2d m (- p1p0dif[0] / p1p0dif[1], - p2p1dif[0] / p2p1dif[1]);
00089 
00090   // Center (x, y)
00091   model_coefficients[0] = (m[0] * u[0] -  m[1] * v[0]  - uvdif[1] )             / (m[0] - m[1]);
00092   model_coefficients[1] = (m[0] * m[1] * uvdif[0] +  m[0] * v[1] - m[1] * u[1]) / (m[0] - m[1]);
00093 
00094   // Radius
00095   model_coefficients[2] = sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) +
00096                                 (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1]));
00097   return (true);
00098 }
00099 
00101 template <typename PointT> void
00102 pcl::SampleConsensusModelCircle2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
00103 {
00104   // Check if the model is valid given the user constraints
00105   if (!isModelValid (model_coefficients))
00106   {
00107     distances.clear ();
00108     return;
00109   }
00110   distances.resize (indices_->size ());
00111 
00112   // Iterate through the 3d points and calculate the distances from them to the sphere
00113   for (size_t i = 0; i < indices_->size (); ++i)
00114     // Calculate the distance from the point to the circle as the difference between
00115     // dist(point,circle_origin) and circle_radius
00116     distances[i] = fabs (sqrt (
00117                                ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
00118                                ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
00119 
00120                                ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
00121                                ( input_->points[(*indices_)[i]].y - model_coefficients[1] )
00122                               ) - model_coefficients[2]);
00123 }
00124 
00126 template <typename PointT> void
00127 pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
00128     const Eigen::VectorXf &model_coefficients, const double threshold, 
00129     std::vector<int> &inliers)
00130 {
00131   // Check if the model is valid given the user constraints
00132   if (!isModelValid (model_coefficients))
00133   {
00134     inliers.clear ();
00135     return;
00136   }
00137   int nr_p = 0;
00138   inliers.resize (indices_->size ());
00139 
00140   // Iterate through the 3d points and calculate the distances from them to the sphere
00141   for (size_t i = 0; i < indices_->size (); ++i)
00142   {
00143     // Calculate the distance from the point to the sphere as the difference between
00144     // dist(point,sphere_origin) and sphere_radius
00145     float distance = fabs (sqrt (
00146                                  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
00147                                  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
00148 
00149                                  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
00150                                  ( input_->points[(*indices_)[i]].y - model_coefficients[1] )
00151                                 ) - model_coefficients[2]);
00152     if (distance < threshold)
00153     {
00154       // Returns the indices of the points whose distances are smaller than the threshold
00155       inliers[nr_p] = (*indices_)[i];
00156       nr_p++;
00157     }
00158   }
00159   inliers.resize (nr_p);
00160 }
00161 
00163 template <typename PointT> int
00164 pcl::SampleConsensusModelCircle2D<PointT>::countWithinDistance (
00165     const Eigen::VectorXf &model_coefficients, const double threshold)
00166 {
00167   // Check if the model is valid given the user constraints
00168   if (!isModelValid (model_coefficients))
00169     return (0);
00170   int nr_p = 0;
00171 
00172   // Iterate through the 3d points and calculate the distances from them to the sphere
00173   for (size_t i = 0; i < indices_->size (); ++i)
00174   {
00175     // Calculate the distance from the point to the sphere as the difference between
00176     // dist(point,sphere_origin) and sphere_radius
00177     float distance = fabs (sqrt (
00178                                  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
00179                                  ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
00180 
00181                                  ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
00182                                  ( input_->points[(*indices_)[i]].y - model_coefficients[1] )
00183                                 ) - model_coefficients[2]);
00184     if (distance < threshold)
00185       nr_p++;
00186   }
00187   return (nr_p);
00188 }
00189 
00191 template <typename PointT> void
00192 pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
00193       const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
00194 {
00195   boost::mutex::scoped_lock lock (tmp_mutex_);
00196 
00197   const int n_unknowns = 3;      // 3 unknowns
00198   // Needs a set of valid model coefficients
00199   if (model_coefficients.size () != n_unknowns)
00200   {
00201     PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
00202     optimized_coefficients = model_coefficients;
00203     return;
00204   }
00205 
00206   // Need at least 3 samples
00207   if (inliers.size () <= 3)
00208   {
00209     PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ());
00210     optimized_coefficients = model_coefficients;
00211     return;
00212   }
00213 
00214   tmp_inliers_ = &inliers;
00215 
00216   int m = inliers.size ();
00217   Eigen::VectorXd x(n_unknowns);
00218   for(int d = 0; d < n_unknowns; d++)
00219     x[d] = model_coefficients[d];
00220 
00221   OptimizationFunctor functor(n_unknowns, m, this);
00222   Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
00223   Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm(num_diff);
00224   int info = lm.minimize (x);
00225 
00226   // Compute the L2 norm of the residuals
00227   PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g \nFinal solution: %g %g %g\n",
00228              info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], x[0], x[1], x[2]);
00229 
00230   optimized_coefficients = Eigen::Vector3f (x[0], x[1], x[2]);
00231 }
00232 
00234 template <typename PointT> void
00235 pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
00236       const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
00237       PointCloud &projected_points, bool copy_data_fields)
00238 {
00239   // Needs a valid set of model coefficients
00240   if (model_coefficients.size () != 3)
00241   {
00242     PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
00243     return;
00244   }
00245 
00246   projected_points.header   = input_->header;
00247   projected_points.is_dense = input_->is_dense;
00248 
00249   // Copy all the data fields from the input cloud to the projected one?
00250   if (copy_data_fields)
00251   {
00252     // Allocate enough space and copy the basics
00253     projected_points.points.resize (input_->points.size ());
00254     projected_points.width    = input_->width;
00255     projected_points.height   = input_->height;
00256 
00257     typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00258     // Iterate over each point
00259     for (size_t i = 0; i < projected_points.points.size (); ++i)
00260       // Iterate over each dimension
00261       pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
00262 
00263     // Iterate through the 3d points and calculate the distances from them to the plane
00264     for (size_t i = 0; i < inliers.size (); ++i)
00265     {
00266       float dx = input_->points[inliers[i]].x - model_coefficients[0];
00267       float dy = input_->points[inliers[i]].y - model_coefficients[1];
00268       float a = sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
00269 
00270       projected_points.points[inliers[i]].x = a * dx + model_coefficients[0];
00271       projected_points.points[inliers[i]].y = a * dy + model_coefficients[1];
00272     }
00273   }
00274   else
00275   {
00276     // Allocate enough space and copy the basics
00277     projected_points.points.resize (inliers.size ());
00278     projected_points.width    = inliers.size ();
00279     projected_points.height   = 1;
00280 
00281     typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00282     // Iterate over each point
00283     for (size_t i = 0; i < inliers.size (); ++i)
00284       // Iterate over each dimension
00285       pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
00286 
00287     // Iterate through the 3d points and calculate the distances from them to the plane
00288     for (size_t i = 0; i < inliers.size (); ++i)
00289     {
00290       float dx = input_->points[inliers[i]].x - model_coefficients[0];
00291       float dy = input_->points[inliers[i]].y - model_coefficients[1];
00292       float a = sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
00293 
00294       projected_points.points[i].x = a * dx + model_coefficients[0];
00295       projected_points.points[i].y = a * dy + model_coefficients[1];
00296     }
00297   }
00298 }
00299 
00301 template <typename PointT> bool
00302 pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel (
00303       const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
00304 {
00305   // Needs a valid model coefficients
00306   if (model_coefficients.size () != 3)
00307   {
00308     PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
00309     return (false);
00310   }
00311 
00312   for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
00313     // Calculate the distance from the point to the sphere as the difference between
00314     //dist(point,sphere_origin) and sphere_radius
00315     if (fabs (sqrt (
00316                     ( input_->points[*it].x - model_coefficients[0] ) *
00317                     ( input_->points[*it].x - model_coefficients[0] ) +
00318                     ( input_->points[*it].y - model_coefficients[1] ) *
00319                     ( input_->points[*it].y - model_coefficients[1] )
00320                    ) - model_coefficients[2]) > threshold)
00321       return (false);
00322 
00323   return (true);
00324 }
00325 
00327 template <typename PointT> bool 
00328 pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
00329 {
00330   // Needs a valid model coefficients
00331   if (model_coefficients.size () != 3)
00332   {
00333     PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isModelValid] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
00334     return (false);
00335   }
00336 
00337   if (radius_min_ != -DBL_MAX && model_coefficients[2] < radius_min_)
00338     return (false);
00339   if (radius_max_ != DBL_MAX && model_coefficients[2] > radius_max_)
00340     return (false);
00341 
00342   return (true);
00343 }
00344 
00345 #define PCL_INSTANTIATE_SampleConsensusModelCircle2D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle2D<T>;
00346 
00347 #endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
00348 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines