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.hpp 2138 2011-08-18 13:33:59Z marton $ 00035 * 00036 */ 00037 00038 #ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_ 00039 #define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_ 00040 00041 #include "pcl/surface/organized_fast_mesh.h" 00042 00043 00044 00046 template <typename PointInT> void 00047 pcl::OrganizedFastMesh<PointInT>::performReconstruction (pcl::PolygonMesh &output) 00048 { 00049 this->reconstructPolygons(output.polygons); 00050 00051 // correct all measurements, 00052 // (running over complete image since some rows and columns are left out 00053 // depending on triangle_pixel_size) 00054 // avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files 00055 for ( unsigned int i = 0; i < input_->points.size(); ++i ) 00056 if ( !hasValidXYZ(input_->points[i]) ) 00057 resetPointData(i, output, 0.0f); 00058 } 00059 00060 template <typename PointInT> void 00061 pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices>& polygons) 00062 { 00063 if ( triangulation_type_ == TRIANGLE_RIGHT_CUT ) 00064 makeRightCutMesh(polygons); 00065 else if ( triangulation_type_ == TRIANGLE_LEFT_CUT ) 00066 makeLeftCutMesh(polygons); 00067 else if ( triangulation_type_ == TRIANGLE_ADAPTIVE_CUT ) 00068 makeAdaptiveCutMesh(polygons); 00069 else if ( triangulation_type_ == QUAD_MESH ) 00070 makeQuadMesh(polygons); 00071 } 00072 00073 00075 template <typename PointInT> void 00076 pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons) 00077 { 00078 unsigned int last_column = input_->width - triangle_pixel_size_; 00079 unsigned int last_row = input_->height - triangle_pixel_size_; 00080 for ( unsigned int x = 0; x < last_column; x+=triangle_pixel_size_ ) 00081 { 00082 for ( unsigned int y = 0; y < last_row; y+=triangle_pixel_size_ ) 00083 { 00084 int i = getIndex(x, y); 00085 int index_right = getIndex(x+triangle_pixel_size_, y); 00086 int index_down = getIndex(x, y+triangle_pixel_size_); 00087 int index_down_right = getIndex(x+triangle_pixel_size_, y+triangle_pixel_size_); 00088 if ( isValidQuad(i, index_right, index_down_right, index_down) ) 00089 if ( !isShadowedQuad(i, index_right, index_down_right, index_down) ) 00090 addQuad(i, index_right, index_down_right, index_down, polygons); 00091 } 00092 } 00093 } 00094 00095 00097 template <typename PointInT> void 00098 pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons) 00099 { 00100 unsigned int last_column = input_->width - triangle_pixel_size_; 00101 unsigned int last_row = input_->height - triangle_pixel_size_; 00102 for ( unsigned int x = 0; x < last_column; x+=triangle_pixel_size_ ) 00103 { 00104 for ( unsigned int y = 0; y < last_row; y+=triangle_pixel_size_ ) 00105 { 00106 int i = getIndex(x, y); 00107 int index_right = getIndex(x+triangle_pixel_size_, y); 00108 int index_down = getIndex(x, y+triangle_pixel_size_); 00109 int index_down_right = getIndex(x+triangle_pixel_size_, y+triangle_pixel_size_); 00110 if ( isValidTriangle(i, index_right, index_down_right) ) 00111 addTriangle(i, index_right, index_down_right, polygons); 00112 if ( isValidTriangle(i, index_down, index_down_right) ) 00113 addTriangle(i, index_down, index_down_right, polygons); 00114 } 00115 } 00116 } 00117 00118 00120 template <typename PointInT> void 00121 pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons) 00122 { 00123 unsigned int last_column = input_->width - triangle_pixel_size_; 00124 unsigned int last_row = input_->height - triangle_pixel_size_; 00125 for ( unsigned int x = 0; x < last_column; x+=triangle_pixel_size_ ) 00126 { 00127 for ( unsigned int y = 0; y < last_row; y+=triangle_pixel_size_ ) 00128 { 00129 int i = getIndex(x, y); 00130 int index_right = getIndex(x+triangle_pixel_size_, y); 00131 int index_down = getIndex(x, y+triangle_pixel_size_); 00132 int index_down_right = getIndex(x+triangle_pixel_size_, y+triangle_pixel_size_); 00133 if ( isValidTriangle(i, index_right, index_down) ) 00134 addTriangle(i, index_right, index_down, polygons); 00135 if ( isValidTriangle(index_right, index_down, index_down_right) ) 00136 addTriangle(index_right, index_down, index_down_right, polygons); 00137 } 00138 } 00139 } 00140 00141 00143 template <typename PointInT> void 00144 pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons) 00145 { 00146 unsigned int last_column = input_->width - triangle_pixel_size_; 00147 unsigned int last_row = input_->height - triangle_pixel_size_; 00148 for ( unsigned int x = 0; x < last_column; x+=triangle_pixel_size_ ) 00149 { 00150 for ( unsigned int y = 0; y < last_row; y+=triangle_pixel_size_ ) 00151 { 00152 int i = getIndex(x, y); 00153 int index_right = getIndex(x+triangle_pixel_size_, y); 00154 int index_down = getIndex(x, y+triangle_pixel_size_); 00155 int index_down_right = getIndex(x+triangle_pixel_size_, y+triangle_pixel_size_); 00156 00157 const bool right_cut_upper = isValidTriangle(i, index_right, index_down_right); 00158 const bool right_cut_lower = isValidTriangle(i, index_down, index_down_right); 00159 const bool left_cut_upper = isValidTriangle(i, index_right, index_down); 00160 const bool left_cut_lower = isValidTriangle(index_right, index_down, index_down_right); 00161 00162 if ( right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower ) 00163 { 00164 float dist_right_cut = fabs (input_->points[index_down].z - input_->points[index_right].z); 00165 float dist_left_cut = fabs (input_->points[i].z - input_->points[index_down_right].z); 00166 if ( dist_right_cut >= dist_left_cut ) 00167 { 00168 addTriangle(i, index_right, index_down_right, polygons); 00169 addTriangle(i, index_down, index_down_right, polygons); 00170 } 00171 else 00172 { 00173 addTriangle(i, index_right, index_down, polygons); 00174 addTriangle(index_right, index_down, index_down_right, polygons); 00175 } 00176 } 00177 else 00178 { 00179 if (right_cut_upper) 00180 addTriangle(i, index_right, index_down_right, polygons); 00181 if (right_cut_lower) 00182 addTriangle(i, index_down, index_down_right, polygons); 00183 if (left_cut_upper) 00184 addTriangle(i, index_right, index_down, polygons); 00185 if (left_cut_lower) 00186 addTriangle(index_right, index_down, index_down_right, polygons); 00187 } 00188 } 00189 } 00190 } 00191 00192 00193 #define PCL_INSTANTIATE_OrganizedFastMesh(T) \ 00194 template class PCL_EXPORTS pcl::OrganizedFastMesh<T>; 00195 00196 #endif // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_