Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/surface/include/pcl/surface/convex_hull.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 #include <pcl/pcl_config.h>
00041 #ifdef HAVE_QHULL
00042 
00043 #ifndef PCL_CONVEX_HULL_2D_H_
00044 #define PCL_CONVEX_HULL_2D_H_
00045 
00046 // PCL includes
00047 #include <pcl/surface/reconstruction.h>
00048 #include <pcl/ModelCoefficients.h>
00049 #include <pcl/PolygonMesh.h>
00050 
00051 namespace pcl
00052 {
00053   /** \brief Sort 2D points in a vector structure
00054     * \param p1 the first point
00055     * \param p2 the second point
00056     * \ingroup surface
00057     */
00058   inline bool
00059   comparePoints2D (const std::pair<int, Eigen::Vector4f> & p1, const std::pair<int, Eigen::Vector4f> & p2)
00060   {
00061     double angle1 = atan2 (p1.second[1], p1.second[0]) + M_PI;
00062     double angle2 = atan2 (p2.second[1], p2.second[0]) + M_PI;
00063     return (angle1 > angle2);
00064   }
00065 
00066   ////////////////////////////////////////////////////////////////////////////////////////////
00067   /** \brief @b ConvexHull using libqhull library.
00068     * \author Aitor Aldoma, Alex Trevor
00069     * \ingroup surface
00070     */
00071   template<typename PointInT>
00072   class ConvexHull : public MeshConstruction<PointInT>
00073   {
00074     protected:
00075       using PCLBase<PointInT>::input_;
00076       using PCLBase<PointInT>::indices_;
00077       using PCLBase<PointInT>::initCompute;
00078       using PCLBase<PointInT>::deinitCompute;
00079 
00080     public:
00081       typedef boost::shared_ptr<ConvexHull<PointInT> > Ptr;
00082       typedef boost::shared_ptr<const ConvexHull<PointInT> > ConstPtr;
00083 
00084       using MeshConstruction<PointInT>::reconstruct;
00085 
00086       typedef pcl::PointCloud<PointInT> PointCloud;
00087       typedef typename PointCloud::Ptr PointCloudPtr;
00088       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00089 
00090       /** \brief Empty constructor. */
00091       ConvexHull () : compute_area_ (false), total_area_ (0), total_volume_ (0), dimension_ (0), 
00092                       projection_angle_thresh_ (cos (0.174532925) ), qhull_flags ("qhull "),
00093                       x_axis_ (1.0, 0.0, 0.0), y_axis_ (0.0, 1.0, 0.0), z_axis_ (0.0, 0.0, 1.0)
00094       {
00095       };
00096       
00097       /** \brief Empty destructor */
00098       virtual ~ConvexHull () {}
00099 
00100       /** \brief Compute a convex hull for all points given.
00101         *
00102         * \note In 2D case (i.e. if the input points belong to one plane)
00103         * the \a polygons vector will have a single item, whereas in 3D
00104         * case it will contain one item for each hull facet.
00105         *
00106         * \param[out] points the resultant points lying on the convex hull.
00107         * \param[out] polygons the resultant convex hull polygons, as a set of
00108         * vertices. The Vertices structure contains an array of point indices.
00109         */
00110       void
00111       reconstruct (PointCloud &points,
00112                    std::vector<pcl::Vertices> &polygons);
00113 
00114       /** \brief Compute a convex hull for all points given.
00115         * \param[out] points the resultant points lying on the convex hull.
00116         */
00117       void
00118       reconstruct (PointCloud &points);
00119 
00120       /** \brief If set to true, the qhull library is called to compute the total area and volume of the convex hull.
00121         * NOTE: When this option is activated, the qhull library produces output to the console.
00122         * \param[in] value wheter to compute the area and the volume, default is false
00123         */
00124       void
00125       setComputeAreaVolume (bool value)
00126       {
00127         compute_area_ = value;
00128         if (compute_area_)
00129           qhull_flags = std::string ("qhull FA");
00130         else
00131           qhull_flags = std::string ("qhull ");
00132       }
00133 
00134       /** \brief Returns the total area of the convex hull. */
00135       double
00136       getTotalArea () const
00137       {
00138         return (total_area_);
00139       }
00140 
00141       /** \brief Returns the total volume of the convex hull. Only valid for 3-dimensional sets.
00142         *  For 2D-sets volume is zero. 
00143         */
00144       double
00145       getTotalVolume () const
00146       {
00147         return (total_volume_);
00148       }
00149 
00150       /** \brief Sets the dimension on the input data, 2D or 3D.
00151         * \param[in] dimension The dimension of the input data.  If not set, this will be determined automatically.
00152         */
00153       void 
00154       setDimension (int dimension)
00155       {
00156         if ((dimension == 2) || (dimension == 3))
00157           dimension_ = dimension;
00158         else
00159           PCL_ERROR ("[pcl::%s::setDimension] Invalid input dimension specified!\n", getClassName ().c_str ());
00160       }
00161 
00162       /** \brief Returns the dimensionality (2 or 3) of the calculated hull. */
00163       inline int
00164       getDimension () const
00165       {
00166         return (dimension_);
00167       }
00168 
00169     protected:
00170       /** \brief The actual reconstruction method. 
00171         * 
00172         * \param[out] points the resultant points lying on the convex hull 
00173         * \param[out] polygons the resultant convex hull polygons, as a set of
00174         * vertices. The Vertices structure contains an array of point indices.
00175         * \param[in] fill_polygon_data true if polygons should be filled, false otherwise
00176         */
00177       void
00178       performReconstruction (PointCloud &points, 
00179                              std::vector<pcl::Vertices> &polygons, 
00180                              bool fill_polygon_data = false);
00181       
00182       /** \brief The reconstruction method for 2D data.  Does not require dimension to be set. 
00183         * 
00184         * \param[out] points the resultant points lying on the convex hull 
00185         * \param[out] polygons the resultant convex hull polygons, as a set of
00186         * vertices. The Vertices structure contains an array of point indices.
00187         * \param[in] fill_polygon_data true if polygons should be filled, false otherwise
00188         */
00189       void
00190       performReconstruction2D (PointCloud &points, 
00191                                std::vector<pcl::Vertices> &polygons, 
00192                                bool fill_polygon_data = false);
00193       
00194       /** \brief The reconstruction method for 3D data.  Does not require dimension to be set. 
00195         * 
00196         * \param[out] points the resultant points lying on the convex hull 
00197         * \param[out] polygons the resultant convex hull polygons, as a set of
00198         * vertices. The Vertices structure contains an array of point indices.
00199         * \param[in] fill_polygon_data true if polygons should be filled, false otherwise
00200         */
00201       void
00202       performReconstruction3D (PointCloud &points, 
00203                                std::vector<pcl::Vertices> &polygons, 
00204                                bool fill_polygon_data = false);
00205       
00206       /** \brief A reconstruction method that returns a polygonmesh.
00207         *
00208         * \param[out] output a PolygonMesh representing the convex hull of the input data.
00209         */
00210       virtual void
00211       performReconstruction (PolygonMesh &output);
00212       
00213       /** \brief A reconstruction method that returns the polygon of the convex hull.
00214         *
00215         * \param[out] polygons the polygon(s) representing the convex hull of the input data.
00216         */
00217       virtual void
00218       performReconstruction (std::vector<pcl::Vertices> &polygons);
00219 
00220       /** \brief Automatically determines the dimension of input data - 2D or 3D. */
00221       void 
00222       calculateInputDimension ();
00223 
00224       /** \brief Class get name method. */
00225       std::string
00226       getClassName () const
00227       {
00228         return ("ConvexHull");
00229       }
00230 
00231       /* \brief True if we should compute the area and volume of the convex hull. */
00232       bool compute_area_;
00233 
00234       /* \brief The area of the convex hull. */
00235       double total_area_;
00236 
00237       /* \brief The volume of the convex hull (only for 3D hulls, zero for 2D). */
00238       double total_volume_;
00239       
00240       /** \brief The dimensionality of the concave hull (2D or 3D). */
00241       int dimension_;
00242 
00243       /** \brief How close can a 2D plane's normal be to an axis to make projection problematic. */
00244       double projection_angle_thresh_;
00245 
00246       /** \brief Option flag string to be used calling qhull. */
00247       std::string qhull_flags;
00248 
00249       /* \brief x-axis - for checking valid projections. */
00250       const Eigen::Vector3d x_axis_;
00251 
00252       /* \brief y-axis - for checking valid projections. */
00253       const Eigen::Vector3d y_axis_;
00254 
00255       /* \brief z-axis - for checking valid projections. */
00256       const Eigen::Vector3d z_axis_;
00257 
00258       public:
00259         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00260   };
00261 }
00262 
00263 #ifdef PCL_NO_PRECOMPILE
00264 #include <pcl/surface/impl/convex_hull.hpp>
00265 #endif
00266 
00267 #endif  //#ifndef PCL_CONVEX_HULL_2D_H_
00268 #endif