Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.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_CYLINDER_H_
00042 #define PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_
00043 
00044 #include <pcl/sample_consensus/sac_model.h>
00045 #include <pcl/sample_consensus/model_types.h>
00046 #include <pcl/common/common.h>
00047 #include <pcl/common/distances.h>
00048 
00049 namespace pcl
00050 {
00051   /** \brief @b SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
00052     * The model coefficients are defined as:
00053     *   - \b point_on_axis.x  : the X coordinate of a point located on the cylinder axis
00054     *   - \b point_on_axis.y  : the Y coordinate of a point located on the cylinder axis
00055     *   - \b point_on_axis.z  : the Z coordinate of a point located on the cylinder axis
00056     *   - \b axis_direction.x : the X coordinate of the cylinder's axis direction
00057     *   - \b axis_direction.y : the Y coordinate of the cylinder's axis direction
00058     *   - \b axis_direction.z : the Z coordinate of the cylinder's axis direction
00059     *   - \b radius           : the cylinder's radius
00060     * 
00061     * \author Radu Bogdan Rusu
00062     * \ingroup sample_consensus
00063     */
00064   template <typename PointT, typename PointNT>
00065   class SampleConsensusModelCylinder : 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<SampleConsensusModelCylinder> Ptr;
00081 
00082       /** \brief Constructor for base SampleConsensusModelCylinder.
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       SampleConsensusModelCylinder (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         , tmp_inliers_ ()
00092       {
00093       }
00094 
00095       /** \brief Constructor for base SampleConsensusModelCylinder.
00096         * \param[in] cloud the input point cloud dataset
00097         * \param[in] indices a vector of point indices to be used from \a cloud
00098         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00099         */
00100       SampleConsensusModelCylinder (const PointCloudConstPtr &cloud, 
00101                                     const std::vector<int> &indices,
00102                                     bool random = false) 
00103         : SampleConsensusModel<PointT> (cloud, indices, random)
00104         , SampleConsensusModelFromNormals<PointT, PointNT> ()
00105         , axis_ (Eigen::Vector3f::Zero ())
00106         , eps_angle_ (0)
00107         , tmp_inliers_ ()
00108       {
00109       }
00110 
00111       /** \brief Copy constructor.
00112         * \param[in] source the model to copy into this
00113         */
00114       SampleConsensusModelCylinder (const SampleConsensusModelCylinder &source) :
00115         SampleConsensusModel<PointT> (),
00116         SampleConsensusModelFromNormals<PointT, PointNT> (), 
00117         axis_ (Eigen::Vector3f::Zero ()),
00118         eps_angle_ (0),
00119         tmp_inliers_ ()
00120       {
00121         *this = source;
00122       }
00123       
00124       /** \brief Empty destructor */
00125       virtual ~SampleConsensusModelCylinder () {}
00126 
00127       /** \brief Copy constructor.
00128         * \param[in] source the model to copy into this
00129         */
00130       inline SampleConsensusModelCylinder&
00131       operator = (const SampleConsensusModelCylinder &source)
00132       {
00133         SampleConsensusModel<PointT>::operator=(source);
00134         axis_ = source.axis_;
00135         eps_angle_ = source.eps_angle_;
00136         tmp_inliers_ = source.tmp_inliers_;
00137         return (*this);
00138       }
00139 
00140       /** \brief Set the angle epsilon (delta) threshold.
00141         * \param[in] ea the maximum allowed difference between the cyilinder axis and the given axis.
00142         */
00143       inline void 
00144       setEpsAngle (const double ea) { eps_angle_ = ea; }
00145 
00146       /** \brief Get the angle epsilon (delta) threshold. */
00147       inline double 
00148       getEpsAngle () { return (eps_angle_); }
00149 
00150       /** \brief Set the axis along which we need to search for a cylinder direction.
00151         * \param[in] ax the axis along which we need to search for a cylinder direction
00152         */
00153       inline void 
00154       setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
00155 
00156       /** \brief Get the axis along which we need to search for a cylinder direction. */
00157       inline Eigen::Vector3f 
00158       getAxis ()  { return (axis_); }
00159 
00160       /** \brief Check whether the given index samples can form a valid cylinder model, compute the model coefficients
00161         * from these samples and store them in model_coefficients. The cylinder coefficients are: point_on_axis,
00162         * axis_direction, cylinder_radius_R
00163         * \param[in] samples the point indices found as possible good candidates for creating a valid model
00164         * \param[out] model_coefficients the resultant model coefficients
00165         */
00166       bool 
00167       computeModelCoefficients (const std::vector<int> &samples, 
00168                                 Eigen::VectorXf &model_coefficients);
00169 
00170       /** \brief Compute all distances from the cloud data to a given cylinder model.
00171         * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to
00172         * \param[out] distances the resultant estimated distances
00173         */
00174       void 
00175       getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
00176                            std::vector<double> &distances);
00177 
00178       /** \brief Select all the points which respect the given model coefficients as inliers.
00179         * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to
00180         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00181         * \param[out] inliers the resultant model inliers
00182         */
00183       void 
00184       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00185                             const double threshold, 
00186                             std::vector<int> &inliers);
00187 
00188       /** \brief Count all the points which respect the given model coefficients as inliers. 
00189         * 
00190         * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
00191         * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
00192         * \return the resultant number of inliers
00193         */
00194       virtual int
00195       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00196                            const double threshold);
00197 
00198       /** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user.
00199         * @note: these are the coefficients of the cylinder model after refinement (eg. after SVD)
00200         * \param[in] inliers the data inliers found as supporting the model
00201         * \param[in] model_coefficients the initial guess for the optimization
00202         * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
00203         */
00204       void 
00205       optimizeModelCoefficients (const std::vector<int> &inliers, 
00206                                  const Eigen::VectorXf &model_coefficients, 
00207                                  Eigen::VectorXf &optimized_coefficients);
00208 
00209 
00210       /** \brief Create a new point cloud with inliers projected onto the cylinder model.
00211         * \param[in] inliers the data inliers that we want to project on the cylinder model
00212         * \param[in] model_coefficients the coefficients of a cylinder model
00213         * \param[out] projected_points the resultant projected points
00214         * \param[in] copy_data_fields set to true if we need to copy the other data fields
00215         */
00216       void 
00217       projectPoints (const std::vector<int> &inliers, 
00218                      const Eigen::VectorXf &model_coefficients, 
00219                      PointCloud &projected_points, 
00220                      bool copy_data_fields = true);
00221 
00222       /** \brief Verify whether a subset of indices verifies the given cylinder model coefficients.
00223         * \param[in] indices the data indices that need to be tested against the cylinder model
00224         * \param[in] model_coefficients the cylinder model coefficients
00225         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00226         */
00227       bool 
00228       doSamplesVerifyModel (const std::set<int> &indices, 
00229                             const Eigen::VectorXf &model_coefficients, 
00230                             const double threshold);
00231 
00232       /** \brief Return an unique id for this model (SACMODEL_CYLINDER). */
00233       inline pcl::SacModel 
00234       getModelType () const { return (SACMODEL_CYLINDER); }
00235 
00236     protected:
00237       /** \brief Get the distance from a point to a line (represented by a point and a direction)
00238         * \param[in] pt a point
00239         * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
00240         */
00241       double 
00242       pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients);
00243 
00244       /** \brief Project a point onto a line given by a point and a direction vector
00245         * \param[in] pt the input point to project
00246         * \param[in] line_pt the point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
00247         * \param[in] line_dir the direction of the line (make sure that line_dir[3] = 0 as there are no internal checks!)
00248         * \param[out] pt_proj the resultant projected point
00249         */
00250       inline void
00251       projectPointToLine (const Eigen::Vector4f &pt, 
00252                           const Eigen::Vector4f &line_pt, 
00253                           const Eigen::Vector4f &line_dir,
00254                           Eigen::Vector4f &pt_proj)
00255       {
00256         float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
00257         // Calculate the projection of the point on the line
00258         pt_proj = line_pt + k * line_dir;
00259       }
00260 
00261       /** \brief Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction,
00262         * cylinder_radius_R)
00263         * \param[in] pt the input point to project
00264         * \param[in] model_coefficients the coefficients of the cylinder (point_on_axis, axis_direction, cylinder_radius_R)
00265         * \param[out] pt_proj the resultant projected point
00266         */
00267       void 
00268       projectPointToCylinder (const Eigen::Vector4f &pt, 
00269                               const Eigen::VectorXf &model_coefficients, 
00270                               Eigen::Vector4f &pt_proj);
00271 
00272       /** \brief Get a string representation of the name of this class. */
00273       std::string 
00274       getName () const { return ("SampleConsensusModelCylinder"); }
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 temporary pointer to a list of given indices for optimizeModelCoefficients () */
00298       const std::vector<int> *tmp_inliers_;
00299 
00300 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
00301 #pragma GCC diagnostic ignored "-Weffc++"
00302 #endif
00303       /** \brief Functor for the optimization function */
00304       struct OptimizationFunctor : pcl::Functor<float>
00305       {
00306         /** Functor constructor
00307           * \param[in] m_data_points the number of data points to evaluate
00308           * \param[in] estimator pointer to the estimator object
00309           * \param[in] distance distance computation function pointer
00310           */
00311         OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCylinder<PointT, PointNT> *model) : 
00312           pcl::Functor<float> (m_data_points), model_ (model) {}
00313 
00314         /** Cost function to be minimized
00315           * \param[in] x variables array
00316           * \param[out] fvec resultant functions evaluations
00317           * \return 0
00318           */
00319         int 
00320         operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
00321         {
00322           Eigen::Vector4f line_pt  (x[0], x[1], x[2], 0);
00323           Eigen::Vector4f line_dir (x[3], x[4], x[5], 0);
00324           
00325           for (int i = 0; i < values (); ++i)
00326           {
00327             // dist = f - r
00328             Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x,
00329                                 model_->input_->points[(*model_->tmp_inliers_)[i]].y,
00330                                 model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0);
00331 
00332             fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]);
00333           }
00334           return (0);
00335         }
00336 
00337         pcl::SampleConsensusModelCylinder<PointT, PointNT> *model_;
00338       };
00339 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
00340 #pragma GCC diagnostic warning "-Weffc++"
00341 #endif
00342   };
00343 }
00344 
00345 #ifdef PCL_NO_PRECOMPILE
00346 #include <pcl/sample_consensus/impl/sac_model_cylinder.hpp>
00347 #endif
00348 
00349 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_