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