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 3027 2011-11-01 04:04:27Z 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 #include <boost/thread/mutex.hpp>
00046 
00047 namespace pcl
00048 {
00060   template <typename PointT>
00061   class SampleConsensusModelCircle2D : public SampleConsensusModel<PointT>
00062   {
00063     using SampleConsensusModel<PointT>::input_;
00064     using SampleConsensusModel<PointT>::indices_;
00065     using SampleConsensusModel<PointT>::radius_min_;
00066     using SampleConsensusModel<PointT>::radius_max_;
00067 
00068     public:
00069       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
00070       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
00071       typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
00072 
00073       typedef boost::shared_ptr<SampleConsensusModelCircle2D> Ptr;
00074 
00078       SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud) : SampleConsensusModel<PointT> (cloud) {};
00079 
00084       SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, const std::vector<int> &indices) : SampleConsensusModel<PointT> (cloud, indices) {};
00085 
00091       void 
00092       getSamples (int &iterations, std::vector<int> &samples);
00093 
00099       bool 
00100       computeModelCoefficients (const std::vector<int> &samples, 
00101                                 Eigen::VectorXf &model_coefficients);
00102 
00107       void 
00108       getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
00109                            std::vector<double> &distances);
00110 
00116       void 
00117       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00118                             const double threshold, 
00119                             std::vector<int> &inliers);
00120 
00127       virtual int
00128       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00129                            const double threshold);
00130 
00137       void 
00138       optimizeModelCoefficients (const std::vector<int> &inliers, 
00139                                  const Eigen::VectorXf &model_coefficients, 
00140                                  Eigen::VectorXf &optimized_coefficients);
00141 
00148       void 
00149       projectPoints (const std::vector<int> &inliers, 
00150                      const Eigen::VectorXf &model_coefficients, 
00151                      PointCloud &projected_points, 
00152                      bool copy_data_fields = true);
00153 
00159       bool 
00160       doSamplesVerifyModel (const std::set<int> &indices, 
00161                             const Eigen::VectorXf &model_coefficients, 
00162                             const double threshold);
00163 
00165       inline pcl::SacModel 
00166       getModelType () const { return (SACMODEL_CIRCLE2D); }
00167 
00168     protected:
00172       bool 
00173       isModelValid (const Eigen::VectorXf &model_coefficients);
00174 
00178       bool
00179       isSampleGood(const std::vector<int> &samples) const;
00180 
00181     private:
00183       boost::mutex tmp_mutex_;
00184 
00186       const std::vector<int> *tmp_inliers_;
00187 
00189       struct OptimizationFunctor : pcl::Functor<double>
00190       {
00197         OptimizationFunctor(int n, int m, pcl::SampleConsensusModelCircle2D<PointT> *model) : 
00198           pcl::Functor<double>(m,n), model_(model) {}
00204         int operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
00205         {
00206           for (int i = 0; i < m_values; ++i)
00207           {
00208             // Compute the difference between the center of the circle and the datapoint X_i
00209             double xt = model_->input_->points[(*model_->tmp_inliers_)[i]].x - x[0];
00210             double yt = model_->input_->points[(*model_->tmp_inliers_)[i]].y - x[1];
00211             
00212             // g = sqrt ((x-a)^2 + (y-b)^2) - R
00213             fvec[i] = sqrt (xt * xt + yt * yt) - x[2];
00214           }
00215           return (0);
00216         }
00217 
00218         pcl::SampleConsensusModelCircle2D<PointT> *model_;
00219       };
00220   };
00221 }
00222 
00223 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_