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_HPP_ 00042 #define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_ 00043 00044 #include <pcl/surface/organized_fast_mesh.h> 00045 00046 ///////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointInT> void 00048 pcl::OrganizedFastMesh<PointInT>::performReconstruction (pcl::PolygonMesh &output) 00049 { 00050 reconstructPolygons (output.polygons); 00051 00052 // Get the field names 00053 int x_idx = pcl::getFieldIndex (output.cloud, "x"); 00054 int y_idx = pcl::getFieldIndex (output.cloud, "y"); 00055 int z_idx = pcl::getFieldIndex (output.cloud, "z"); 00056 if (x_idx == -1 || y_idx == -1 || z_idx == -1) 00057 return; 00058 // correct all measurements, 00059 // (running over complete image since some rows and columns are left out 00060 // depending on triangle_pixel_size) 00061 // avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files 00062 for (unsigned int i = 0; i < input_->points.size (); ++i) 00063 if (!isFinite (input_->points[i])) 00064 resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx); 00065 } 00066 00067 ///////////////////////////////////////////////////////////////////////////////////////////// 00068 template <typename PointInT> void 00069 pcl::OrganizedFastMesh<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons) 00070 { 00071 reconstructPolygons (polygons); 00072 } 00073 00074 ///////////////////////////////////////////////////////////////////////////////////////////// 00075 template <typename PointInT> void 00076 pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons) 00077 { 00078 if (triangulation_type_ == TRIANGLE_RIGHT_CUT) 00079 makeRightCutMesh (polygons); 00080 else if (triangulation_type_ == TRIANGLE_LEFT_CUT) 00081 makeLeftCutMesh (polygons); 00082 else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT) 00083 makeAdaptiveCutMesh (polygons); 00084 else if (triangulation_type_ == QUAD_MESH) 00085 makeQuadMesh (polygons); 00086 } 00087 00088 ///////////////////////////////////////////////////////////////////////////////////////////// 00089 template <typename PointInT> void 00090 pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons) 00091 { 00092 int last_column = input_->width - triangle_pixel_size_; 00093 int last_row = input_->height - triangle_pixel_size_; 00094 00095 int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0; 00096 int y_big_incr = triangle_pixel_size_ * input_->width, 00097 x_big_incr = y_big_incr + triangle_pixel_size_; 00098 // Reserve enough space 00099 polygons.resize (input_->width * input_->height); 00100 00101 // Go over the rows first 00102 for (int y = 0; y < last_row; y += triangle_pixel_size_) 00103 { 00104 // Initialize a new row 00105 i = y * input_->width; 00106 index_right = i + triangle_pixel_size_; 00107 index_down = i + y_big_incr; 00108 index_down_right = i + x_big_incr; 00109 00110 // Go over the columns 00111 for (int x = 0; x < last_column; x += triangle_pixel_size_, 00112 i += triangle_pixel_size_, 00113 index_right += triangle_pixel_size_, 00114 index_down += triangle_pixel_size_, 00115 index_down_right += triangle_pixel_size_) 00116 { 00117 if (isValidQuad (i, index_down, index_right, index_down_right)) 00118 if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down)) 00119 addQuad (i, index_down, index_right, index_down_right, idx++, polygons); 00120 } 00121 } 00122 polygons.resize (idx); 00123 } 00124 00125 ///////////////////////////////////////////////////////////////////////////////////////////// 00126 template <typename PointInT> void 00127 pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons) 00128 { 00129 int last_column = input_->width - triangle_pixel_size_; 00130 int last_row = input_->height - triangle_pixel_size_; 00131 00132 int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0; 00133 int y_big_incr = triangle_pixel_size_ * input_->width, 00134 x_big_incr = y_big_incr + triangle_pixel_size_; 00135 // Reserve enough space 00136 polygons.resize (input_->width * input_->height * 2); 00137 00138 // Go over the rows first 00139 for (int y = 0; y < last_row; y += triangle_pixel_size_) 00140 { 00141 // Initialize a new row 00142 i = y * input_->width; 00143 index_right = i + triangle_pixel_size_; 00144 index_down = i + y_big_incr; 00145 index_down_right = i + x_big_incr; 00146 00147 // Go over the columns 00148 for (int x = 0; x < last_column; x += triangle_pixel_size_, 00149 i += triangle_pixel_size_, 00150 index_right += triangle_pixel_size_, 00151 index_down += triangle_pixel_size_, 00152 index_down_right += triangle_pixel_size_) 00153 { 00154 if (isValidTriangle (i, index_down_right, index_right)) 00155 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right)) 00156 addTriangle (i, index_down_right, index_right, idx++, polygons); 00157 00158 if (isValidTriangle (i, index_down, index_down_right)) 00159 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right)) 00160 addTriangle (i, index_down, index_down_right, idx++, polygons); 00161 } 00162 } 00163 polygons.resize (idx); 00164 } 00165 00166 ///////////////////////////////////////////////////////////////////////////////////////////// 00167 template <typename PointInT> void 00168 pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons) 00169 { 00170 int last_column = input_->width - triangle_pixel_size_; 00171 int last_row = input_->height - triangle_pixel_size_; 00172 00173 int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0; 00174 int y_big_incr = triangle_pixel_size_ * input_->width, 00175 x_big_incr = y_big_incr + triangle_pixel_size_; 00176 // Reserve enough space 00177 polygons.resize (input_->width * input_->height * 2); 00178 00179 // Go over the rows first 00180 for (int y = 0; y < last_row; y += triangle_pixel_size_) 00181 { 00182 // Initialize a new row 00183 i = y * input_->width; 00184 index_right = i + triangle_pixel_size_; 00185 index_down = i + y_big_incr; 00186 index_down_right = i + x_big_incr; 00187 00188 // Go over the columns 00189 for (int x = 0; x < last_column; x += triangle_pixel_size_, 00190 i += triangle_pixel_size_, 00191 index_right += triangle_pixel_size_, 00192 index_down += triangle_pixel_size_, 00193 index_down_right += triangle_pixel_size_) 00194 { 00195 if (isValidTriangle (i, index_down, index_right)) 00196 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right)) 00197 addTriangle (i, index_down, index_right, idx++, polygons); 00198 00199 if (isValidTriangle (index_right, index_down, index_down_right)) 00200 if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right)) 00201 addTriangle (index_right, index_down, index_down_right, idx++, polygons); 00202 } 00203 } 00204 polygons.resize (idx); 00205 } 00206 00207 ///////////////////////////////////////////////////////////////////////////////////////////// 00208 template <typename PointInT> void 00209 pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons) 00210 { 00211 int last_column = input_->width - triangle_pixel_size_; 00212 int last_row = input_->height - triangle_pixel_size_; 00213 00214 int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0; 00215 int y_big_incr = triangle_pixel_size_ * input_->width, 00216 x_big_incr = y_big_incr + triangle_pixel_size_; 00217 // Reserve enough space 00218 polygons.resize (input_->width * input_->height * 4); 00219 00220 // Go over the rows first 00221 for (int y = 0; y < last_row; y += triangle_pixel_size_) 00222 { 00223 // Initialize a new row 00224 i = y * input_->width; 00225 index_right = i + triangle_pixel_size_; 00226 index_down = i + y_big_incr; 00227 index_down_right = i + x_big_incr; 00228 00229 // Go over the columns 00230 for (int x = 0; x < last_column; x += triangle_pixel_size_, 00231 i += triangle_pixel_size_, 00232 index_right += triangle_pixel_size_, 00233 index_down += triangle_pixel_size_, 00234 index_down_right += triangle_pixel_size_) 00235 { 00236 const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right); 00237 const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right); 00238 const bool left_cut_upper = isValidTriangle (i, index_down, index_right); 00239 const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right); 00240 00241 if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower) 00242 { 00243 float dist_right_cut = fabsf (input_->points[index_down].z - input_->points[index_right].z); 00244 float dist_left_cut = fabsf (input_->points[i].z - input_->points[index_down_right].z); 00245 if (dist_right_cut >= dist_left_cut) 00246 { 00247 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right)) 00248 addTriangle (i, index_down_right, index_right, idx++, polygons); 00249 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right)) 00250 addTriangle (i, index_down, index_down_right, idx++, polygons); 00251 } 00252 else 00253 { 00254 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right)) 00255 addTriangle (i, index_down, index_right, idx++, polygons); 00256 if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right)) 00257 addTriangle (index_right, index_down, index_down_right, idx++, polygons); 00258 } 00259 } 00260 else 00261 { 00262 if (right_cut_upper) 00263 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right)) 00264 addTriangle (i, index_down_right, index_right, idx++, polygons); 00265 if (right_cut_lower) 00266 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right)) 00267 addTriangle (i, index_down, index_down_right, idx++, polygons); 00268 if (left_cut_upper) 00269 if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right)) 00270 addTriangle (i, index_down, index_right, idx++, polygons); 00271 if (left_cut_lower) 00272 if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right)) 00273 addTriangle (index_right, index_down, index_down_right, idx++, polygons); 00274 } 00275 } 00276 } 00277 polygons.resize (idx); 00278 } 00279 00280 #define PCL_INSTANTIATE_OrganizedFastMesh(T) \ 00281 template class PCL_EXPORTS pcl::OrganizedFastMesh<T>; 00282 00283 #endif // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_