Point Cloud Library (PCL)  1.3.1
organized_fast_mesh.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Dirk Holz, University of Bonn.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: organized_fast_mesh.h 3031 2011-11-01 04:25:15Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_H_
00039 #define PCL_SURFACE_ORGANIZED_FAST_MESH_H_
00040 
00041 #include <pcl/common/angles.h>
00042 #include <pcl/surface/reconstruction.h>
00043 
00044 namespace pcl
00045 {
00046 
00053   template <typename PointInT>
00054   class OrganizedFastMesh : public SurfaceReconstruction<PointInT>
00055   {
00056     public:
00057       using SurfaceReconstruction<PointInT>::input_;
00058       using SurfaceReconstruction<PointInT>::check_tree_;
00059 
00060       typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
00061 
00062       typedef std::vector<pcl::Vertices> Polygons;
00063 
00064       enum TriangulationType
00065       {
00066         TRIANGLE_RIGHT_CUT,     // _always_ "cuts" a quad from top left to bottom right
00067         TRIANGLE_LEFT_CUT,      // _always_ "cuts" a quad from top right to bottom left
00068         TRIANGLE_ADAPTIVE_CUT,  // "cuts" where possible and prefers larger differences in 'z' direction
00069         QUAD_MESH               // create a simple quad mesh
00070       };
00071 
00072       OrganizedFastMesh()
00073       : max_edge_length_squared_ (0.025f)
00074       , triangle_pixel_size_ (1)
00075       , triangulation_type_ (TRIANGLE_RIGHT_CUT)
00076       , store_shadowed_faces_(false)
00077       {
00078         check_tree_ = false;
00079         cos_angle_tolerance_ = fabs(cos(pcl::deg2rad(12.5f)));
00080       };
00081       ~OrganizedFastMesh(){};
00082 
00090       void
00091       performReconstruction (pcl::PolygonMesh &output);
00092 
00093       void
00094       reconstructPolygons (std::vector<pcl::Vertices>& polygons);
00095 
00097       inline void
00098       setMaxEdgeLength(float max_edge_length)
00099       {
00100         max_edge_length_squared_ = max_edge_length*max_edge_length;
00101       };
00102 
00108       inline void
00109       setTrianglePixelSize(int triangle_size)
00110       {
00111         triangle_pixel_size_ = std::max(1, (triangle_size - 1));
00112       }
00113 
00119       inline void
00120       setTriangulationType(TriangulationType type)
00121       {
00122         triangulation_type_ = type;
00123       }
00124 
00125       inline void
00126       storeShadowedFaces(bool enable)
00127       {
00128         store_shadowed_faces_ = enable;
00129       }
00130 
00131     protected:
00132 
00134       pcl::Vertices triangle_;
00135 
00137       pcl::Vertices quad_;
00138 
00140       float max_edge_length_squared_;
00141 
00143       int triangle_pixel_size_;
00144 
00146       TriangulationType triangulation_type_;
00147 
00149       bool store_shadowed_faces_;
00150 
00151       float cos_angle_tolerance_;
00152 
00159       inline void
00160       addTriangle (int a, int b, int c, std::vector<pcl::Vertices>& polygons)
00161       {
00162         if (isShadowedTriangle(a, b, c))
00163           return;
00164 
00165         triangle_.vertices.clear ();
00166         triangle_.vertices.push_back (a);
00167         triangle_.vertices.push_back (b);
00168         triangle_.vertices.push_back (c);
00169         polygons.push_back (triangle_);
00170       }
00171 
00172       inline void
00173       addQuad(int a, int b, int c, int d, std::vector<pcl::Vertices>& polygons)
00174       {
00175         quad_.vertices.clear ();
00176         quad_.vertices.push_back (a);
00177         quad_.vertices.push_back (b);
00178         quad_.vertices.push_back (c);
00179         quad_.vertices.push_back (d);
00180         polygons.push_back (quad_);
00181       }
00182 
00183 
00189       inline void
00190       resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
00191                       int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
00192       {
00193         float new_value = value;
00194         memcpy(&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof(float));
00195         memcpy(&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof(float));
00196         memcpy(&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof(float));
00197       }
00198 
00199       inline bool
00200       isShadowed(const PointInT& point_a, const PointInT& point_b)
00201       {
00202         Eigen::Vector3f viewpoint(0.0f, 0.0f, 0.0f); // TODO: allow for passing viewpoint information
00203         Eigen::Vector3f dir_a = viewpoint - point_a.getVector3fMap();
00204         Eigen::Vector3f dir_b = point_b.getVector3fMap() - point_a.getVector3fMap();
00205         float distance_to_points = dir_a.norm();
00206         float distance_between_points = dir_b.norm();
00207         float cos_angle = dir_a.dot(dir_b) / (distance_to_points*distance_between_points);
00208         if (cos_angle != cos_angle) cos_angle = 1.0f;
00209         return ( fabs(cos_angle) >= cos_angle_tolerance_ );
00210         // TODO: check for both: angle almost 0/180 _and_ distance between points larger than noise level
00211       }
00212 
00213       inline bool
00214       isValidTriangle(const int& a, const int& b, const int& c)
00215       {
00216         if ( !pcl::hasValidXYZ(input_->points[a]) ) return false;
00217         if ( !pcl::hasValidXYZ(input_->points[b]) ) return false;
00218         if ( !pcl::hasValidXYZ(input_->points[c]) ) return false;
00219         return true;
00220       }
00221 
00222       inline bool
00223       isShadowedTriangle(const int& a, const int& b, const int& c)
00224       {
00225         if ( isShadowed(input_->points[a], input_->points[b]) ) return true;
00226         if ( isShadowed(input_->points[b], input_->points[c]) ) return true;
00227         if ( isShadowed(input_->points[c], input_->points[a]) ) return true;
00228         return false;
00229       }
00230 
00231       inline bool
00232       isValidQuad(const int& a, const int& b, const int& c, const int& d)
00233       {
00234         if ( !pcl::hasValidXYZ(input_->points[a]) ) return false;
00235         if ( !pcl::hasValidXYZ(input_->points[b]) ) return false;
00236         if ( !pcl::hasValidXYZ(input_->points[c]) ) return false;
00237         if ( !pcl::hasValidXYZ(input_->points[d]) ) return false;
00238         return true;
00239       }
00240 
00241       inline bool
00242       isShadowedQuad(const int& a, const int& b, const int& c, const int& d)
00243       {
00244         if ( isShadowed(input_->points[a], input_->points[b]) ) return true;
00245         if ( isShadowed(input_->points[b], input_->points[c]) ) return true;
00246         if ( isShadowed(input_->points[c], input_->points[d]) ) return true;
00247         if ( isShadowed(input_->points[d], input_->points[a]) ) return true;
00248         return false;
00249       }
00250 
00251       inline int
00252       getIndex(int x, int y)
00253       {
00254         return (int)(y * input_->width + x);
00255       }
00256 
00257       void
00258       makeQuadMesh (std::vector<pcl::Vertices>& polygons);
00259 
00260       void
00261       makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
00262 
00263       void
00264       makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
00265 
00266       void
00267       makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
00268 
00269   };
00270 
00271 }
00272 
00273 #endif  // PCL_SURFACE_ORGANIZED_FAST_MESH_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines