Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2012, 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  */
00038 
00039 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
00040 #define PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
00041 
00042 #include <pcl/sample_consensus/sac_model.h>
00043 #include <pcl/sample_consensus/model_types.h>
00044 #include <pcl/common/common.h>
00045 #include <pcl/common/distances.h>
00046 #include <limits.h>
00047 
00048 namespace pcl
00049 {
00050   /** \brief @b SampleConsensusModelCone defines a model for 3D cone segmentation.
00051     * The model coefficients are defined as:
00052     * <ul>
00053     * <li><b>apex.x</b>  : the X coordinate of cone's apex
00054     * <li><b>apex.y</b>  : the Y coordinate of cone's apex
00055     * <li><b>apex.z</b>  : the Z coordinate of cone's apex
00056     * <li><b>axis_direction.x</b> : the X coordinate of the cone's axis direction
00057     * <li><b>axis_direction.y</b> : the Y coordinate of the cone's axis direction
00058     * <li><b>axis_direction.z</b> : the Z coordinate of the cone's axis direction
00059     * <li><b>opening_angle</b>    : the cone's opening angle
00060     * </ul>
00061     * \author Stefan Schrandt
00062     * \ingroup sample_consensus
00063     */
00064   template <typename PointT, typename PointNT>
00065   class SampleConsensusModelCone : public SampleConsensusModel<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
00066   {
00067     public:
00068       using SampleConsensusModel<PointT>::input_;
00069       using SampleConsensusModel<PointT>::indices_;
00070       using SampleConsensusModel<PointT>::radius_min_;
00071       using SampleConsensusModel<PointT>::radius_max_;
00072       using SampleConsensusModelFromNormals<PointT, PointNT>::normals_;
00073       using SampleConsensusModelFromNormals<PointT, PointNT>::normal_distance_weight_;
00074       using SampleConsensusModel<PointT>::error_sqr_dists_;
00075 
00076       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
00077       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
00078       typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
00079 
00080       typedef boost::shared_ptr<SampleConsensusModelCone> Ptr;
00081 
00082       /** \brief Constructor for base SampleConsensusModelCone.
00083         * \param[in] cloud the input point cloud dataset
00084         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00085         */
00086       SampleConsensusModelCone (const PointCloudConstPtr &cloud, bool random = false) 
00087         : SampleConsensusModel<PointT> (cloud, random)
00088         , SampleConsensusModelFromNormals<PointT, PointNT> ()
00089         , axis_ (Eigen::Vector3f::Zero ())
00090         , eps_angle_ (0)
00091         , min_angle_ (-std::numeric_limits<double>::max ())
00092         , max_angle_ (std::numeric_limits<double>::max ())
00093         , tmp_inliers_ ()
00094       {
00095       }
00096 
00097       /** \brief Constructor for base SampleConsensusModelCone.
00098         * \param[in] cloud the input point cloud dataset
00099         * \param[in] indices a vector of point indices to be used from \a cloud
00100         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00101         */
00102       SampleConsensusModelCone (const PointCloudConstPtr &cloud, 
00103                                 const std::vector<int> &indices,
00104                                 bool random = false) 
00105         : SampleConsensusModel<PointT> (cloud, indices, random)
00106         , SampleConsensusModelFromNormals<PointT, PointNT> ()
00107         , axis_ (Eigen::Vector3f::Zero ())
00108         , eps_angle_ (0)
00109         , min_angle_ (-std::numeric_limits<double>::max ())
00110         , max_angle_ (std::numeric_limits<double>::max ())
00111         , tmp_inliers_ ()
00112       {
00113       }
00114 
00115       /** \brief Copy constructor.
00116         * \param[in] source the model to copy into this
00117         */
00118       SampleConsensusModelCone (const SampleConsensusModelCone &source) :
00119         SampleConsensusModel<PointT> (), 
00120         SampleConsensusModelFromNormals<PointT, PointNT> (),
00121         axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ ()
00122       {
00123         *this = source;
00124       }
00125       
00126       /** \brief Empty destructor */
00127       virtual ~SampleConsensusModelCone () {}
00128 
00129       /** \brief Copy constructor.
00130         * \param[in] source the model to copy into this
00131         */
00132       inline SampleConsensusModelCone&
00133       operator = (const SampleConsensusModelCone &source)
00134       {
00135         SampleConsensusModel<PointT>::operator=(source);
00136         axis_ = source.axis_;
00137         eps_angle_ = source.eps_angle_;
00138         min_angle_ = source.min_angle_;
00139         max_angle_ = source.max_angle_;
00140         tmp_inliers_ = source.tmp_inliers_;
00141         return (*this);
00142       }
00143 
00144       /** \brief Set the angle epsilon (delta) threshold.
00145         * \param[in] ea the maximum allowed difference between the cone's axis and the given axis.
00146         */
00147       inline void 
00148       setEpsAngle (double ea) { eps_angle_ = ea; }
00149 
00150       /** \brief Get the angle epsilon (delta) threshold. */
00151       inline double 
00152       getEpsAngle () const { return (eps_angle_); }
00153 
00154       /** \brief Set the axis along which we need to search for a cone direction.
00155         * \param[in] ax the axis along which we need to search for a cone direction
00156         */
00157       inline void 
00158       setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
00159 
00160       /** \brief Get the axis along which we need to search for a cone direction. */
00161       inline Eigen::Vector3f 
00162       getAxis () const { return (axis_); }
00163 
00164       /** \brief Set the minimum and maximum allowable opening angle for a cone model
00165         * given from a user.
00166         * \param[in] min_angle the minimum allwoable opening angle of a cone model
00167         * \param[in] max_angle the maximum allwoable opening angle of a cone model
00168         */
00169       inline void
00170       setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
00171       {
00172         min_angle_ = min_angle;
00173         max_angle_ = max_angle;
00174       }
00175 
00176       /** \brief Get the opening angle which we need minumum to validate a cone model.
00177         * \param[out] min_angle the minimum allwoable opening angle of a cone model
00178         * \param[out] max_angle the maximum allwoable opening angle of a cone model
00179         */
00180       inline void
00181       getMinMaxOpeningAngle (double &min_angle, double &max_angle) const
00182       {
00183         min_angle = min_angle_;
00184         max_angle = max_angle_;
00185       }
00186 
00187       /** \brief Check whether the given index samples can form a valid cone model, compute the model coefficients
00188         * from these samples and store them in model_coefficients. The cone coefficients are: apex,
00189         * axis_direction, opening_angle.
00190         * \param[in] samples the point indices found as possible good candidates for creating a valid model
00191         * \param[out] model_coefficients the resultant model coefficients
00192         */
00193       bool 
00194       computeModelCoefficients (const std::vector<int> &samples, 
00195                                 Eigen::VectorXf &model_coefficients);
00196 
00197       /** \brief Compute all distances from the cloud data to a given cone model.
00198         * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
00199         * \param[out] distances the resultant estimated distances
00200         */
00201       void 
00202       getDistancesToModel (const Eigen::VectorXf &model_coefficients,  
00203                            std::vector<double> &distances);
00204 
00205       /** \brief Select all the points which respect the given model coefficients as inliers.
00206         * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
00207         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00208         * \param[out] inliers the resultant model inliers
00209         */
00210       void 
00211       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00212                             const double threshold, 
00213                             std::vector<int> &inliers);
00214 
00215       /** \brief Count all the points which respect the given model coefficients as inliers. 
00216         * 
00217         * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
00218         * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
00219         * \return the resultant number of inliers
00220         */
00221       virtual int
00222       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00223                            const double threshold);
00224 
00225 
00226       /** \brief Recompute the cone coefficients using the given inlier set and return them to the user.
00227         * @note: these are the coefficients of the cone model after refinement (eg. after SVD)
00228         * \param[in] inliers the data inliers found as supporting the model
00229         * \param[in] model_coefficients the initial guess for the optimization
00230         * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
00231         */
00232       void 
00233       optimizeModelCoefficients (const std::vector<int> &inliers, 
00234                                  const Eigen::VectorXf &model_coefficients, 
00235                                  Eigen::VectorXf &optimized_coefficients);
00236 
00237 
00238       /** \brief Create a new point cloud with inliers projected onto the cone model.
00239         * \param[in] inliers the data inliers that we want to project on the cone model
00240         * \param[in] model_coefficients the coefficients of a cone model
00241         * \param[out] projected_points the resultant projected points
00242         * \param[in] copy_data_fields set to true if we need to copy the other data fields
00243         */
00244       void 
00245       projectPoints (const std::vector<int> &inliers, 
00246                      const Eigen::VectorXf &model_coefficients, 
00247                      PointCloud &projected_points, 
00248                      bool copy_data_fields = true);
00249 
00250       /** \brief Verify whether a subset of indices verifies the given cone model coefficients.
00251         * \param[in] indices the data indices that need to be tested against the cone model
00252         * \param[in] model_coefficients the cone model coefficients
00253         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00254         */
00255       bool 
00256       doSamplesVerifyModel (const std::set<int> &indices, 
00257                             const Eigen::VectorXf &model_coefficients, 
00258                             const double threshold);
00259 
00260       /** \brief Return an unique id for this model (SACMODEL_CONE). */
00261       inline pcl::SacModel 
00262       getModelType () const { return (SACMODEL_CONE); }
00263 
00264     protected:
00265       /** \brief Get the distance from a point to a line (represented by a point and a direction)
00266         * \param[in] pt a point
00267         * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
00268         */
00269       double 
00270       pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients);
00271 
00272       /** \brief Get a string representation of the name of this class. */
00273       std::string 
00274       getName () const { return ("SampleConsensusModelCone"); }
00275 
00276     protected:
00277       /** \brief Check whether a model is valid given the user constraints.
00278         * \param[in] model_coefficients the set of model coefficients
00279         */
00280       bool 
00281       isModelValid (const Eigen::VectorXf &model_coefficients);
00282 
00283       /** \brief Check if a sample of indices results in a good sample of points
00284         * indices. Pure virtual.
00285         * \param[in] samples the resultant index samples
00286         */
00287       bool
00288       isSampleGood (const std::vector<int> &samples) const;
00289 
00290     private:
00291       /** \brief The axis along which we need to search for a plane perpendicular to. */
00292       Eigen::Vector3f axis_;
00293     
00294       /** \brief The maximum allowed difference between the plane normal and the given axis. */
00295       double eps_angle_;
00296 
00297       /** \brief The minimum and maximum allowed opening angles of valid cone model. */
00298       double min_angle_;
00299       double max_angle_;
00300 
00301       /** \brief temporary pointer to a list of given indices for optimizeModelCoefficients () */
00302       const std::vector<int> *tmp_inliers_;
00303 
00304 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
00305 #pragma GCC diagnostic ignored "-Weffc++"
00306 #endif
00307       /** \brief Functor for the optimization function */
00308       struct OptimizationFunctor : pcl::Functor<float>
00309       {
00310         /** Functor constructor
00311           * \param[in] m_data_points the number of data points to evaluate
00312           * \param[in] estimator pointer to the estimator object
00313           * \param[in] distance distance computation function pointer
00314           */
00315         OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCone<PointT, PointNT> *model) : 
00316           pcl::Functor<float> (m_data_points), model_ (model) {}
00317 
00318         /** Cost function to be minimized
00319           * \param[in] x variables array
00320           * \param[out] fvec resultant functions evaluations
00321           * \return 0
00322           */
00323         int 
00324         operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
00325         {
00326           Eigen::Vector4f apex  (x[0], x[1], x[2], 0);
00327           Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
00328           float opening_angle = x[6];
00329 
00330           float apexdotdir = apex.dot (axis_dir);
00331           float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
00332 
00333           for (int i = 0; i < values (); ++i)
00334           {
00335             // dist = f - r
00336             Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x,
00337                                 model_->input_->points[(*model_->tmp_inliers_)[i]].y,
00338                                 model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0);
00339 
00340             // Calculate the point's projection on the cone axis
00341             float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
00342             Eigen::Vector4f pt_proj = apex + k * axis_dir;
00343 
00344             // Calculate the actual radius of the cone at the level of the projected point
00345             Eigen::Vector4f height = apex-pt_proj;
00346             float actual_cone_radius = tanf (opening_angle) * height.norm ();
00347 
00348             fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, apex, axis_dir) - actual_cone_radius * actual_cone_radius);
00349           }
00350           return (0);
00351         }
00352 
00353         pcl::SampleConsensusModelCone<PointT, PointNT> *model_;
00354       };
00355 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
00356 #pragma GCC diagnostic warning "-Weffc++"
00357 #endif
00358   };
00359 }
00360 
00361 #ifdef PCL_NO_PRECOMPILE
00362 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
00363 #endif
00364 
00365 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_