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