sac_model_circle.h
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) 2010-2011, 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  * $Id: sac_model_circle.h 6144 2012-07-04 22:06:28Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_
00041 #define PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_
00042 
00043 #include <pcl/sample_consensus/sac_model.h>
00044 #include <pcl/sample_consensus/model_types.h>
00045 
00046 namespace pcl
00047 {
00058   template <typename PointT>
00059   class SampleConsensusModelCircle2D : public SampleConsensusModel<PointT>
00060   {
00061     using SampleConsensusModel<PointT>::input_;
00062     using SampleConsensusModel<PointT>::indices_;
00063     using SampleConsensusModel<PointT>::radius_min_;
00064     using SampleConsensusModel<PointT>::radius_max_;
00065 
00066     public:
00067       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
00068       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
00069       typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
00070 
00071       typedef boost::shared_ptr<SampleConsensusModelCircle2D> Ptr;
00072 
00076       SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud) : 
00077         SampleConsensusModel<PointT> (cloud), tmp_inliers_ () 
00078       {};
00079 
00084       SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, const std::vector<int> &indices) : 
00085         SampleConsensusModel<PointT> (cloud, indices), tmp_inliers_ ()
00086       {};
00087 
00091       SampleConsensusModelCircle2D (const SampleConsensusModelCircle2D &source) :
00092         SampleConsensusModel<PointT> (), tmp_inliers_ () 
00093       {
00094         *this = source;
00095       }
00096 
00100       inline SampleConsensusModelCircle2D&
00101       operator = (const SampleConsensusModelCircle2D &source)
00102       {
00103         SampleConsensusModel<PointT>::operator=(source);
00104         tmp_inliers_ = source.tmp_inliers_;
00105         return (*this);
00106       }
00107 
00113       bool 
00114       computeModelCoefficients (const std::vector<int> &samples, 
00115                                 Eigen::VectorXf &model_coefficients);
00116 
00121       void 
00122       getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
00123                            std::vector<double> &distances);
00124 
00130       void 
00131       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00132                             const double threshold, 
00133                             std::vector<int> &inliers);
00134 
00141       virtual int
00142       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00143                            const double threshold);
00144 
00151       void 
00152       optimizeModelCoefficients (const std::vector<int> &inliers, 
00153                                  const Eigen::VectorXf &model_coefficients, 
00154                                  Eigen::VectorXf &optimized_coefficients);
00155 
00162       void 
00163       projectPoints (const std::vector<int> &inliers, 
00164                      const Eigen::VectorXf &model_coefficients, 
00165                      PointCloud &projected_points, 
00166                      bool copy_data_fields = true);
00167 
00173       bool 
00174       doSamplesVerifyModel (const std::set<int> &indices, 
00175                             const Eigen::VectorXf &model_coefficients, 
00176                             const double threshold);
00177 
00179       inline pcl::SacModel 
00180       getModelType () const { return (SACMODEL_CIRCLE2D); }
00181 
00182     protected:
00186       bool 
00187       isModelValid (const Eigen::VectorXf &model_coefficients);
00188 
00192       bool
00193       isSampleGood(const std::vector<int> &samples) const;
00194 
00195     private:
00197       const std::vector<int> *tmp_inliers_;
00198 
00199 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
00200 #pragma GCC diagnostic ignored "-Weffc++"
00201 #endif
00202 
00203       struct OptimizationFunctor : pcl::Functor<float>
00204       {
00210         OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCircle2D<PointT> *model) : 
00211           pcl::Functor<float>(m_data_points), model_ (model) {}
00212 
00218         int 
00219         operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
00220         {
00221           for (int i = 0; i < values (); ++i)
00222           {
00223             // Compute the difference between the center of the circle and the datapoint X_i
00224             float xt = model_->input_->points[(*model_->tmp_inliers_)[i]].x - x[0];
00225             float yt = model_->input_->points[(*model_->tmp_inliers_)[i]].y - x[1];
00226             
00227             // g = sqrt ((x-a)^2 + (y-b)^2) - R
00228             fvec[i] = sqrtf (xt * xt + yt * yt) - x[2];
00229           }
00230           return (0);
00231         }
00232 
00233         pcl::SampleConsensusModelCircle2D<PointT> *model_;
00234       };
00235 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
00236 #pragma GCC diagnostic warning "-Weffc++"
00237 #endif
00238   };
00239 }
00240 
00241 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_


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