Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model_plane.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_PLANE_H_
00042 #define PCL_SAMPLE_CONSENSUS_MODEL_PLANE_H_
00043 
00044 #include <pcl/sample_consensus/sac_model.h>
00045 #include <pcl/sample_consensus/model_types.h>
00046 
00047 namespace pcl
00048 {
00049 
00050   /** \brief Project a point on a planar model given by a set of normalized coefficients
00051     * \param[in] p the input point to project
00052     * \param[in] model_coefficients the coefficients of the plane (a, b, c, d) that satisfy ax+by+cz+d=0
00053     * \param[out] q the resultant projected point
00054     */
00055   template <typename Point> inline void
00056   projectPoint (const Point &p, const Eigen::Vector4f &model_coefficients, Point &q)
00057   {
00058     // Calculate the distance from the point to the plane
00059     Eigen::Vector4f pp (p.x, p.y, p.z, 1);
00060     // use normalized coefficients to calculate the scalar projection 
00061     float distance_to_plane = pp.dot(model_coefficients);
00062 
00063 
00064     //TODO: Why doesn't getVector4Map work here?
00065     //Eigen::Vector4f q_e = q.getVector4fMap ();
00066     //q_e = pp - model_coefficients * distance_to_plane;
00067     
00068     Eigen::Vector4f q_e = pp - distance_to_plane * model_coefficients;
00069     q.x = q_e[0];
00070     q.y = q_e[1];
00071     q.z = q_e[2];
00072   }
00073 
00074   /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0
00075     * \param p a point
00076     * \param a the normalized <i>a</i> coefficient of a plane
00077     * \param b the normalized <i>b</i> coefficient of a plane
00078     * \param c the normalized <i>c</i> coefficient of a plane
00079     * \param d the normalized <i>d</i> coefficient of a plane
00080     * \ingroup sample_consensus
00081     */
00082   template <typename Point> inline double
00083   pointToPlaneDistanceSigned (const Point &p, double a, double b, double c, double d)
00084   {
00085     return (a * p.x + b * p.y + c * p.z + d);
00086   }
00087 
00088   /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0
00089     * \param p a point
00090     * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane
00091     * \ingroup sample_consensus
00092     */
00093   template <typename Point> inline double
00094   pointToPlaneDistanceSigned (const Point &p, const Eigen::Vector4f &plane_coefficients)
00095   {
00096     return ( plane_coefficients[0] * p.x + plane_coefficients[1] * p.y + plane_coefficients[2] * p.z + plane_coefficients[3] );
00097   }
00098 
00099   /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0
00100     * \param p a point
00101     * \param a the normalized <i>a</i> coefficient of a plane
00102     * \param b the normalized <i>b</i> coefficient of a plane
00103     * \param c the normalized <i>c</i> coefficient of a plane
00104     * \param d the normalized <i>d</i> coefficient of a plane
00105     * \ingroup sample_consensus
00106     */
00107   template <typename Point> inline double
00108   pointToPlaneDistance (const Point &p, double a, double b, double c, double d)
00109   {
00110     return (fabs (pointToPlaneDistanceSigned (p, a, b, c, d)) );
00111   }
00112 
00113   /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0
00114     * \param p a point
00115     * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane
00116     * \ingroup sample_consensus
00117     */
00118   template <typename Point> inline double
00119   pointToPlaneDistance (const Point &p, const Eigen::Vector4f &plane_coefficients)
00120   {
00121     return ( fabs (pointToPlaneDistanceSigned (p, plane_coefficients)) );
00122   }
00123 
00124   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00125   /** \brief SampleConsensusModelPlane defines a model for 3D plane segmentation.
00126     * The model coefficients are defined as:
00127     *   - \b a : the X coordinate of the plane's normal (normalized)
00128     *   - \b b : the Y coordinate of the plane's normal (normalized)
00129     *   - \b c : the Z coordinate of the plane's normal (normalized)
00130     *   - \b d : the fourth <a href="http://mathworld.wolfram.com/HessianNormalForm.html">Hessian component</a> of the plane's equation
00131     * 
00132     * \author Radu B. Rusu
00133     * \ingroup sample_consensus
00134     */
00135   template <typename PointT>
00136   class SampleConsensusModelPlane : public SampleConsensusModel<PointT>
00137   {
00138     public:
00139       using SampleConsensusModel<PointT>::input_;
00140       using SampleConsensusModel<PointT>::indices_;
00141       using SampleConsensusModel<PointT>::error_sqr_dists_;
00142 
00143       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
00144       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
00145       typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
00146 
00147       typedef boost::shared_ptr<SampleConsensusModelPlane> Ptr;
00148 
00149       /** \brief Constructor for base SampleConsensusModelPlane.
00150         * \param[in] cloud the input point cloud dataset
00151         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00152         */
00153       SampleConsensusModelPlane (const PointCloudConstPtr &cloud, bool random = false) 
00154         : SampleConsensusModel<PointT> (cloud, random) {};
00155 
00156       /** \brief Constructor for base SampleConsensusModelPlane.
00157         * \param[in] cloud the input point cloud dataset
00158         * \param[in] indices a vector of point indices to be used from \a cloud
00159         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00160         */
00161       SampleConsensusModelPlane (const PointCloudConstPtr &cloud, 
00162                                  const std::vector<int> &indices,
00163                                  bool random = false) 
00164         : SampleConsensusModel<PointT> (cloud, indices, random) {};
00165       
00166       /** \brief Empty destructor */
00167       virtual ~SampleConsensusModelPlane () {}
00168 
00169       /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from
00170         * these samples and store them internally in model_coefficients_. The plane coefficients are:
00171         * a, b, c, d (ax+by+cz+d=0)
00172         * \param[in] samples the point indices found as possible good candidates for creating a valid model
00173         * \param[out] model_coefficients the resultant model coefficients
00174         */
00175       bool 
00176       computeModelCoefficients (const std::vector<int> &samples, 
00177                                 Eigen::VectorXf &model_coefficients);
00178 
00179       /** \brief Compute all distances from the cloud data to a given plane model.
00180         * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
00181         * \param[out] distances the resultant estimated distances
00182         */
00183       void 
00184       getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
00185                            std::vector<double> &distances);
00186 
00187       /** \brief Select all the points which respect the given model coefficients as inliers.
00188         * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
00189         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00190         * \param[out] inliers the resultant model inliers
00191         */
00192       void 
00193       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00194                             const double threshold, 
00195                             std::vector<int> &inliers);
00196 
00197       /** \brief Count all the points which respect the given model coefficients as inliers. 
00198         * 
00199         * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
00200         * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
00201         * \return the resultant number of inliers
00202         */
00203       virtual int
00204       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00205                            const double threshold);
00206 
00207       /** \brief Recompute the plane coefficients using the given inlier set and return them to the user.
00208         * @note: these are the coefficients of the plane model after refinement (eg. after SVD)
00209         * \param[in] inliers the data inliers found as supporting the model
00210         * \param[in] model_coefficients the initial guess for the model coefficients
00211         * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
00212         */
00213       void 
00214       optimizeModelCoefficients (const std::vector<int> &inliers, 
00215                                  const Eigen::VectorXf &model_coefficients, 
00216                                  Eigen::VectorXf &optimized_coefficients);
00217 
00218       /** \brief Create a new point cloud with inliers projected onto the plane model.
00219         * \param[in] inliers the data inliers that we want to project on the plane model
00220         * \param[in] model_coefficients the *normalized* coefficients of a plane model
00221         * \param[out] projected_points the resultant projected points
00222         * \param[in] copy_data_fields set to true if we need to copy the other data fields
00223         */
00224       void 
00225       projectPoints (const std::vector<int> &inliers, 
00226                      const Eigen::VectorXf &model_coefficients, 
00227                      PointCloud &projected_points, 
00228                      bool copy_data_fields = true);
00229 
00230       /** \brief Verify whether a subset of indices verifies the given plane model coefficients.
00231         * \param[in] indices the data indices that need to be tested against the plane model
00232         * \param[in] model_coefficients the plane model coefficients
00233         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00234         */
00235       bool 
00236       doSamplesVerifyModel (const std::set<int> &indices, 
00237                             const Eigen::VectorXf &model_coefficients, 
00238                             const double threshold);
00239 
00240       /** \brief Return an unique id for this model (SACMODEL_PLANE). */
00241       inline pcl::SacModel 
00242       getModelType () const { return (SACMODEL_PLANE); }
00243 
00244     protected:
00245       /** \brief Check whether a model is valid given the user constraints.
00246         * \param[in] model_coefficients the set of model coefficients
00247         */
00248       inline bool 
00249       isModelValid (const Eigen::VectorXf &model_coefficients)
00250       {
00251         // Needs a valid model coefficients
00252         if (model_coefficients.size () != 4)
00253         {
00254           PCL_ERROR ("[pcl::SampleConsensusModelPlane::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
00255           return (false);
00256         }
00257         return (true);
00258       }
00259 
00260     private:
00261       /** \brief Check if a sample of indices results in a good sample of points
00262         * indices.
00263         * \param[in] samples the resultant index samples
00264         */
00265       virtual bool
00266       isSampleGood (const std::vector<int> &samples) const;
00267   };
00268 }
00269 
00270 #ifdef PCL_NO_PRECOMPILE
00271 #include <pcl/sample_consensus/impl/sac_model_plane.hpp>
00272 #endif
00273 
00274 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_PLANE_H_