Point Cloud Library (PCL)
1.7.0
|
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_