Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h
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  *  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  * $Id$
00038  *
00039  */
00040 
00041 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_
00042 #define PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_
00043 
00044 #include <pcl/sample_consensus/sac_model.h>
00045 #include <pcl/sample_consensus/model_types.h>
00046 
00047 namespace pcl
00048 {
00049   /** \brief SampleConsensusModelCircle2D defines a model for 2D circle segmentation on the X-Y plane.
00050     *
00051     * The model coefficients are defined as:
00052     *   - \b center.x : the X coordinate of the circle's center
00053     *   - \b center.y : the Y coordinate of the circle's center
00054     *   - \b radius   : the circle's radius
00055     * 
00056     * \author Radu B. Rusu
00057     * \ingroup sample_consensus
00058    */
00059   template <typename PointT>
00060   class SampleConsensusModelCircle2D : public SampleConsensusModel<PointT>
00061   {
00062     public:
00063       using SampleConsensusModel<PointT>::input_;
00064       using SampleConsensusModel<PointT>::indices_;
00065       using SampleConsensusModel<PointT>::radius_min_;
00066       using SampleConsensusModel<PointT>::radius_max_;
00067       using SampleConsensusModel<PointT>::error_sqr_dists_;
00068 
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 
00075       /** \brief Constructor for base SampleConsensusModelCircle2D.
00076         * \param[in] cloud the input point cloud dataset
00077         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00078         */
00079       SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, bool random = false) 
00080         : SampleConsensusModel<PointT> (cloud, random), tmp_inliers_ () 
00081       {};
00082 
00083       /** \brief Constructor for base SampleConsensusModelCircle2D.
00084         * \param[in] cloud the input point cloud dataset
00085         * \param[in] indices a vector of point indices to be used from \a cloud
00086         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00087         */
00088       SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, 
00089                                     const std::vector<int> &indices,
00090                                     bool random = false)
00091         : SampleConsensusModel<PointT> (cloud, indices, random), tmp_inliers_ ()
00092       {};
00093 
00094       /** \brief Copy constructor.
00095         * \param[in] source the model to copy into this
00096         */
00097       SampleConsensusModelCircle2D (const SampleConsensusModelCircle2D &source) :
00098         SampleConsensusModel<PointT> (), tmp_inliers_ () 
00099       {
00100         *this = source;
00101       }
00102       
00103       /** \brief Empty destructor */
00104       virtual ~SampleConsensusModelCircle2D () {}
00105 
00106       /** \brief Copy constructor.
00107         * \param[in] source the model to copy into this
00108         */
00109       inline SampleConsensusModelCircle2D&
00110       operator = (const SampleConsensusModelCircle2D &source)
00111       {
00112         SampleConsensusModel<PointT>::operator=(source);
00113         tmp_inliers_ = source.tmp_inliers_;
00114         return (*this);
00115       }
00116 
00117       /** \brief Check whether the given index samples can form a valid 2D circle model, compute the model coefficients
00118         * from these samples and store them in model_coefficients. The circle coefficients are: x, y, R.
00119         * \param[in] samples the point indices found as possible good candidates for creating a valid model
00120         * \param[out] model_coefficients the resultant model coefficients
00121         */
00122       bool 
00123       computeModelCoefficients (const std::vector<int> &samples, 
00124                                 Eigen::VectorXf &model_coefficients);
00125 
00126       /** \brief Compute all distances from the cloud data to a given 2D circle model.
00127         * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to
00128         * \param[out] distances the resultant estimated distances
00129         */
00130       void 
00131       getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
00132                            std::vector<double> &distances);
00133 
00134       /** \brief Compute all distances from the cloud data to a given 2D circle model.
00135         * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to
00136         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00137         * \param[out] inliers the resultant model inliers
00138         */
00139       void 
00140       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00141                             const double threshold, 
00142                             std::vector<int> &inliers);
00143 
00144       /** \brief Count all the points which respect the given model coefficients as inliers. 
00145         * 
00146         * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
00147         * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
00148         * \return the resultant number of inliers
00149         */
00150       virtual int
00151       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00152                            const double threshold);
00153 
00154        /** \brief Recompute the 2d circle coefficients using the given inlier set and return them to the user.
00155         * @note: these are the coefficients of the 2d circle model after refinement (eg. after SVD)
00156         * \param[in] inliers the data inliers found as supporting the model
00157         * \param[in] model_coefficients the initial guess for the optimization
00158         * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
00159         */
00160       void 
00161       optimizeModelCoefficients (const std::vector<int> &inliers, 
00162                                  const Eigen::VectorXf &model_coefficients, 
00163                                  Eigen::VectorXf &optimized_coefficients);
00164 
00165       /** \brief Create a new point cloud with inliers projected onto the 2d circle model.
00166         * \param[in] inliers the data inliers that we want to project on the 2d circle model
00167         * \param[in] model_coefficients the coefficients of a 2d circle model
00168         * \param[out] projected_points the resultant projected points
00169         * \param[in] copy_data_fields set to true if we need to copy the other data fields
00170         */
00171       void 
00172       projectPoints (const std::vector<int> &inliers, 
00173                      const Eigen::VectorXf &model_coefficients, 
00174                      PointCloud &projected_points, 
00175                      bool copy_data_fields = true);
00176 
00177       /** \brief Verify whether a subset of indices verifies the given 2d circle model coefficients.
00178         * \param[in] indices the data indices that need to be tested against the 2d circle model
00179         * \param[in] model_coefficients the 2d circle model coefficients
00180         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00181         */
00182       bool 
00183       doSamplesVerifyModel (const std::set<int> &indices, 
00184                             const Eigen::VectorXf &model_coefficients, 
00185                             const double threshold);
00186 
00187       /** \brief Return an unique id for this model (SACMODEL_CIRCLE2D). */
00188       inline pcl::SacModel 
00189       getModelType () const { return (SACMODEL_CIRCLE2D); }
00190 
00191     protected:
00192       /** \brief Check whether a model is valid given the user constraints.
00193         * \param[in] model_coefficients the set of model coefficients
00194         */
00195       bool 
00196       isModelValid (const Eigen::VectorXf &model_coefficients);
00197 
00198       /** \brief Check if a sample of indices results in a good sample of points indices.
00199         * \param[in] samples the resultant index samples
00200         */
00201       bool
00202       isSampleGood(const std::vector<int> &samples) const;
00203 
00204     private:
00205       /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */
00206       const std::vector<int> *tmp_inliers_;
00207 
00208 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
00209 #pragma GCC diagnostic ignored "-Weffc++"
00210 #endif
00211       /** \brief Functor for the optimization function */
00212       struct OptimizationFunctor : pcl::Functor<float>
00213       {
00214         /** \brief Functor constructor
00215           * \param[in] m_data_points the number of data points to evaluate
00216           * \param[in] estimator pointer to the estimator object
00217           * \param[in] distance distance computation function pointer
00218           */
00219         OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCircle2D<PointT> *model) : 
00220           pcl::Functor<float>(m_data_points), model_ (model) {}
00221 
00222         /** Cost function to be minimized
00223           * \param[in] x the variables array
00224           * \param[out] fvec the resultant functions evaluations
00225           * \return 0
00226           */
00227         int 
00228         operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
00229         {
00230           for (int i = 0; i < values (); ++i)
00231           {
00232             // Compute the difference between the center of the circle and the datapoint X_i
00233             float xt = model_->input_->points[(*model_->tmp_inliers_)[i]].x - x[0];
00234             float yt = model_->input_->points[(*model_->tmp_inliers_)[i]].y - x[1];
00235             
00236             // g = sqrt ((x-a)^2 + (y-b)^2) - R
00237             fvec[i] = sqrtf (xt * xt + yt * yt) - x[2];
00238           }
00239           return (0);
00240         }
00241 
00242         pcl::SampleConsensusModelCircle2D<PointT> *model_;
00243       };
00244 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
00245 #pragma GCC diagnostic warning "-Weffc++"
00246 #endif
00247   };
00248 }
00249 
00250 #ifdef PCL_NO_PRECOMPILE
00251 #include <pcl/sample_consensus/impl/sac_model_circle.hpp>
00252 #endif
00253 
00254 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_