Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/surface/include/pcl/surface/organized_fast_mesh.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2011, Dirk Holz, University of Bonn.
00006  *  Copyright (c) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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_SURFACE_ORGANIZED_FAST_MESH_H_
00042 #define PCL_SURFACE_ORGANIZED_FAST_MESH_H_
00043 
00044 #include <pcl/common/angles.h>
00045 #include <pcl/surface/reconstruction.h>
00046 
00047 namespace pcl
00048 {
00049 
00050   /** \brief Simple triangulation/surface reconstruction for organized point
00051     * clouds. Neighboring points (pixels in image space) are connected to
00052     * construct a triangular mesh.
00053     *
00054     * \note If you use this code in any academic work, please cite:
00055     *   D. Holz and S. Behnke.
00056     *   Fast Range Image Segmentation and Smoothing using Approximate Surface Reconstruction and Region Growing.
00057     *   In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS),
00058     *   Jeju Island, Korea, June 26-29 2012.
00059     *   <a href="http://purl.org/holz/papers/holz_2012_ias.pdf">http://purl.org/holz/papers/holz_2012_ias.pdf</a>
00060     * 
00061     * \author Dirk Holz, Radu B. Rusu
00062     * \ingroup surface
00063     */
00064   template <typename PointInT>
00065   class OrganizedFastMesh : public MeshConstruction<PointInT>
00066   {
00067     public:
00068       typedef boost::shared_ptr<OrganizedFastMesh<PointInT> > Ptr;
00069       typedef boost::shared_ptr<const OrganizedFastMesh<PointInT> > ConstPtr;
00070 
00071       using MeshConstruction<PointInT>::input_;
00072       using MeshConstruction<PointInT>::check_tree_;
00073 
00074       typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
00075 
00076       typedef std::vector<pcl::Vertices> Polygons;
00077 
00078       enum TriangulationType
00079       {
00080         TRIANGLE_RIGHT_CUT,     // _always_ "cuts" a quad from top left to bottom right
00081         TRIANGLE_LEFT_CUT,      // _always_ "cuts" a quad from top right to bottom left
00082         TRIANGLE_ADAPTIVE_CUT,  // "cuts" where possible and prefers larger differences in 'z' direction
00083         QUAD_MESH               // create a simple quad mesh
00084       };
00085 
00086       /** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */
00087       OrganizedFastMesh ()
00088       : max_edge_length_squared_ (0.025f)
00089       , triangle_pixel_size_ (1)
00090       , triangulation_type_ (QUAD_MESH)
00091       , store_shadowed_faces_ (false)
00092       , cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f))))
00093       {
00094         check_tree_ = false;
00095       };
00096 
00097       /** \brief Destructor. */
00098       virtual ~OrganizedFastMesh () {};
00099 
00100       /** \brief Set a maximum edge length. TODO: Implement!
00101         * \param[in] max_edge_length the maximum edge length
00102         */
00103       inline void
00104       setMaxEdgeLength (float max_edge_length)
00105       {
00106         max_edge_length_squared_ = max_edge_length * max_edge_length;
00107       };
00108 
00109       /** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
00110         * \param[in] triangle_size edge length in pixels
00111         * (Default: 1 = neighboring pixels are connected)
00112         */
00113       inline void
00114       setTrianglePixelSize (int triangle_size)
00115       {
00116         triangle_pixel_size_ = std::max (1, (triangle_size - 1));
00117       }
00118 
00119       /** \brief Set the triangulation type (see \a TriangulationType)
00120         * \param[in] type quad mesh, triangle mesh with fixed left, right cut,
00121         * or adaptive cut (splits a quad wrt. the depth (z) of the points)
00122         */
00123       inline void
00124       setTriangulationType (TriangulationType type)
00125       {
00126         triangulation_type_ = type;
00127       }
00128 
00129       /** \brief Store shadowed faces or not.
00130         * \param[in] enable set to true to store shadowed faces
00131         */
00132       inline void
00133       storeShadowedFaces (bool enable)
00134       {
00135         store_shadowed_faces_ = enable;
00136       }
00137 
00138     protected:
00139       /** \brief max (squared) length of edge */
00140       float max_edge_length_squared_;
00141 
00142       /** \brief size of triangle endges (in pixels) */
00143       int triangle_pixel_size_;
00144 
00145       /** \brief Type of meshin scheme (quads vs. triangles, left cut vs. right cut ... */
00146       TriangulationType triangulation_type_;
00147 
00148       /** \brief Whether or not shadowed faces are stored, e.g., for exploration */
00149       bool store_shadowed_faces_;
00150 
00151       float cos_angle_tolerance_;
00152 
00153       /** \brief Perform the actual polygonal reconstruction.
00154         * \param[out] polygons the resultant polygons
00155         */
00156       void
00157       reconstructPolygons (std::vector<pcl::Vertices>& polygons);
00158 
00159       /** \brief Create the surface.
00160         * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
00161         */
00162       virtual void
00163       performReconstruction (std::vector<pcl::Vertices> &polygons);
00164 
00165       /** \brief Create the surface.
00166         *
00167         * Simply uses image indices to create an initial polygonal mesh for organized point clouds.
00168         * \a indices_ are ignored!
00169         *
00170         * \param[out] output the resultant polygonal mesh
00171         */
00172       void
00173       performReconstruction (pcl::PolygonMesh &output);
00174 
00175       /** \brief Add a new triangle to the current polygon mesh
00176         * \param[in] a index of the first vertex
00177         * \param[in] b index of the second vertex
00178         * \param[in] c index of the third vertex
00179         * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
00180         * \param[out] polygons the polygon mesh to be updated
00181         */
00182       inline void
00183       addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
00184       {
00185         assert (idx < static_cast<int> (polygons.size ()));
00186         polygons[idx].vertices.resize (3);
00187         polygons[idx].vertices[0] = a;
00188         polygons[idx].vertices[1] = b;
00189         polygons[idx].vertices[2] = c;
00190       }
00191 
00192       /** \brief Add a new quad to the current polygon mesh
00193         * \param[in] a index of the first vertex
00194         * \param[in] b index of the second vertex
00195         * \param[in] c index of the third vertex
00196         * \param[in] d index of the fourth vertex
00197         * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
00198         * \param[out] polygons the polygon mesh to be updated
00199         */
00200       inline void
00201       addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
00202       {
00203         assert (idx < static_cast<int> (polygons.size ()));
00204         polygons[idx].vertices.resize (4);
00205         polygons[idx].vertices[0] = a;
00206         polygons[idx].vertices[1] = b;
00207         polygons[idx].vertices[2] = c;
00208         polygons[idx].vertices[3] = d;
00209       }
00210 
00211       /** \brief Set (all) coordinates of a particular point to the specified value
00212         * \param[in] point_index index of point
00213         * \param[out] mesh to modify
00214         * \param[in] value value to use when re-setting
00215         * \param[in] field_x_idx the X coordinate of the point
00216         * \param[in] field_y_idx the Y coordinate of the point
00217         * \param[in] field_z_idx the Z coordinate of the point
00218         */
00219       inline void
00220       resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
00221                       int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
00222       {
00223         float new_value = value;
00224         memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
00225         memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
00226         memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
00227       }
00228 
00229       /** \brief Check if a point is shadowed by another point
00230         * \param[in] point_a the first point
00231         * \param[in] point_b the second point
00232         */
00233       inline bool
00234       isShadowed (const PointInT& point_a, const PointInT& point_b)
00235       {
00236         Eigen::Vector3f viewpoint = Eigen::Vector3f::Zero (); // TODO: allow for passing viewpoint information
00237         Eigen::Vector3f dir_a = viewpoint - point_a.getVector3fMap ();
00238         Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
00239         float distance_to_points = dir_a.norm ();
00240         float distance_between_points = dir_b.norm ();
00241         float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
00242         if (cos_angle != cos_angle)
00243           cos_angle = 1.0f;
00244         return (fabs (cos_angle) >= cos_angle_tolerance_);
00245         // TODO: check for both: angle almost 0/180 _and_ distance between points larger than noise level
00246       }
00247 
00248       /** \brief Check if a triangle is valid.
00249         * \param[in] a index of the first vertex
00250         * \param[in] b index of the second vertex
00251         * \param[in] c index of the third vertex
00252         */
00253       inline bool
00254       isValidTriangle (const int& a, const int& b, const int& c)
00255       {
00256         if (!pcl::isFinite (input_->points[a])) return (false);
00257         if (!pcl::isFinite (input_->points[b])) return (false);
00258         if (!pcl::isFinite (input_->points[c])) return (false);
00259         return (true);
00260       }
00261 
00262       /** \brief Check if a triangle is shadowed.
00263         * \param[in] a index of the first vertex
00264         * \param[in] b index of the second vertex
00265         * \param[in] c index of the third vertex
00266         */
00267       inline bool
00268       isShadowedTriangle (const int& a, const int& b, const int& c)
00269       {
00270         if (isShadowed (input_->points[a], input_->points[b])) return (true);
00271         if (isShadowed (input_->points[b], input_->points[c])) return (true);
00272         if (isShadowed (input_->points[c], input_->points[a])) return (true);
00273         return (false);
00274       }
00275 
00276       /** \brief Check if a quad is valid.
00277         * \param[in] a index of the first vertex
00278         * \param[in] b index of the second vertex
00279         * \param[in] c index of the third vertex
00280         * \param[in] d index of the fourth vertex
00281         */
00282       inline bool
00283       isValidQuad (const int& a, const int& b, const int& c, const int& d)
00284       {
00285         if (!pcl::isFinite (input_->points[a])) return (false);
00286         if (!pcl::isFinite (input_->points[b])) return (false);
00287         if (!pcl::isFinite (input_->points[c])) return (false);
00288         if (!pcl::isFinite (input_->points[d])) return (false);
00289         return (true);
00290       }
00291 
00292       /** \brief Check if a triangle is shadowed.
00293         * \param[in] a index of the first vertex
00294         * \param[in] b index of the second vertex
00295         * \param[in] c index of the third vertex
00296         * \param[in] d index of the fourth vertex
00297         */
00298       inline bool
00299       isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
00300       {
00301         if (isShadowed (input_->points[a], input_->points[b])) return (true);
00302         if (isShadowed (input_->points[b], input_->points[c])) return (true);
00303         if (isShadowed (input_->points[c], input_->points[d])) return (true);
00304         if (isShadowed (input_->points[d], input_->points[a])) return (true);
00305         return (false);
00306       }
00307 
00308       /** \brief Create a quad mesh.
00309         * \param[out] polygons the resultant mesh
00310         */
00311       void
00312       makeQuadMesh (std::vector<pcl::Vertices>& polygons);
00313 
00314       /** \brief Create a right cut mesh.
00315         * \param[out] polygons the resultant mesh
00316         */
00317       void
00318       makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
00319 
00320       /** \brief Create a left cut mesh.
00321         * \param[out] polygons the resultant mesh
00322         */
00323       void
00324       makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
00325 
00326       /** \brief Create an adaptive cut mesh.
00327         * \param[out] polygons the resultant mesh
00328         */
00329       void
00330       makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
00331   };
00332 }
00333 
00334 #ifdef PCL_NO_PRECOMPILE
00335 #include <pcl/surface/impl/organized_fast_mesh.hpp>
00336 #endif
00337 
00338 #endif  // PCL_SURFACE_ORGANIZED_FAST_MESH_H_