Point Cloud Library (PCL)  1.7.0
organized_fast_mesh.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Dirk Holz (University of Bonn)
6  * Copyright (c) 2010-2011, Willow Garage, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of Willow Garage, Inc. nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
42 #define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
43 
44 #include <pcl/surface/organized_fast_mesh.h>
45 
46 /////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointInT> void
49 {
50  reconstructPolygons (output.polygons);
51 
52  // Get the field names
53  int x_idx = pcl::getFieldIndex (output.cloud, "x");
54  int y_idx = pcl::getFieldIndex (output.cloud, "y");
55  int z_idx = pcl::getFieldIndex (output.cloud, "z");
56  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
57  return;
58  // correct all measurements,
59  // (running over complete image since some rows and columns are left out
60  // depending on triangle_pixel_size)
61  // avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files
62  for (unsigned int i = 0; i < input_->points.size (); ++i)
63  if (!isFinite (input_->points[i]))
64  resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx);
65 }
66 
67 /////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointInT> void
69 pcl::OrganizedFastMesh<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
70 {
71  reconstructPolygons (polygons);
72 }
73 
74 /////////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointInT> void
76 pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
77 {
78  if (triangulation_type_ == TRIANGLE_RIGHT_CUT)
79  makeRightCutMesh (polygons);
80  else if (triangulation_type_ == TRIANGLE_LEFT_CUT)
81  makeLeftCutMesh (polygons);
82  else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT)
83  makeAdaptiveCutMesh (polygons);
84  else if (triangulation_type_ == QUAD_MESH)
85  makeQuadMesh (polygons);
86 }
87 
88 /////////////////////////////////////////////////////////////////////////////////////////////
89 template <typename PointInT> void
90 pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons)
91 {
92  int last_column = input_->width - triangle_pixel_size_;
93  int last_row = input_->height - triangle_pixel_size_;
94 
95  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
96  int y_big_incr = triangle_pixel_size_ * input_->width,
97  x_big_incr = y_big_incr + triangle_pixel_size_;
98  // Reserve enough space
99  polygons.resize (input_->width * input_->height);
100 
101  // Go over the rows first
102  for (int y = 0; y < last_row; y += triangle_pixel_size_)
103  {
104  // Initialize a new row
105  i = y * input_->width;
106  index_right = i + triangle_pixel_size_;
107  index_down = i + y_big_incr;
108  index_down_right = i + x_big_incr;
109 
110  // Go over the columns
111  for (int x = 0; x < last_column; x += triangle_pixel_size_,
112  i += triangle_pixel_size_,
113  index_right += triangle_pixel_size_,
114  index_down += triangle_pixel_size_,
115  index_down_right += triangle_pixel_size_)
116  {
117  if (isValidQuad (i, index_down, index_right, index_down_right))
118  if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down))
119  addQuad (i, index_down, index_right, index_down_right, idx++, polygons);
120  }
121  }
122  polygons.resize (idx);
123 }
124 
125 /////////////////////////////////////////////////////////////////////////////////////////////
126 template <typename PointInT> void
127 pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons)
128 {
129  int last_column = input_->width - triangle_pixel_size_;
130  int last_row = input_->height - triangle_pixel_size_;
131 
132  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
133  int y_big_incr = triangle_pixel_size_ * input_->width,
134  x_big_incr = y_big_incr + triangle_pixel_size_;
135  // Reserve enough space
136  polygons.resize (input_->width * input_->height * 2);
137 
138  // Go over the rows first
139  for (int y = 0; y < last_row; y += triangle_pixel_size_)
140  {
141  // Initialize a new row
142  i = y * input_->width;
143  index_right = i + triangle_pixel_size_;
144  index_down = i + y_big_incr;
145  index_down_right = i + x_big_incr;
146 
147  // Go over the columns
148  for (int x = 0; x < last_column; x += triangle_pixel_size_,
149  i += triangle_pixel_size_,
150  index_right += triangle_pixel_size_,
151  index_down += triangle_pixel_size_,
152  index_down_right += triangle_pixel_size_)
153  {
154  if (isValidTriangle (i, index_down_right, index_right))
155  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
156  addTriangle (i, index_down_right, index_right, idx++, polygons);
157 
158  if (isValidTriangle (i, index_down, index_down_right))
159  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
160  addTriangle (i, index_down, index_down_right, idx++, polygons);
161  }
162  }
163  polygons.resize (idx);
164 }
165 
166 /////////////////////////////////////////////////////////////////////////////////////////////
167 template <typename PointInT> void
168 pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons)
169 {
170  int last_column = input_->width - triangle_pixel_size_;
171  int last_row = input_->height - triangle_pixel_size_;
172 
173  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
174  int y_big_incr = triangle_pixel_size_ * input_->width,
175  x_big_incr = y_big_incr + triangle_pixel_size_;
176  // Reserve enough space
177  polygons.resize (input_->width * input_->height * 2);
178 
179  // Go over the rows first
180  for (int y = 0; y < last_row; y += triangle_pixel_size_)
181  {
182  // Initialize a new row
183  i = y * input_->width;
184  index_right = i + triangle_pixel_size_;
185  index_down = i + y_big_incr;
186  index_down_right = i + x_big_incr;
187 
188  // Go over the columns
189  for (int x = 0; x < last_column; x += triangle_pixel_size_,
190  i += triangle_pixel_size_,
191  index_right += triangle_pixel_size_,
192  index_down += triangle_pixel_size_,
193  index_down_right += triangle_pixel_size_)
194  {
195  if (isValidTriangle (i, index_down, index_right))
196  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
197  addTriangle (i, index_down, index_right, idx++, polygons);
198 
199  if (isValidTriangle (index_right, index_down, index_down_right))
200  if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
201  addTriangle (index_right, index_down, index_down_right, idx++, polygons);
202  }
203  }
204  polygons.resize (idx);
205 }
206 
207 /////////////////////////////////////////////////////////////////////////////////////////////
208 template <typename PointInT> void
209 pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons)
210 {
211  int last_column = input_->width - triangle_pixel_size_;
212  int last_row = input_->height - triangle_pixel_size_;
213 
214  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
215  int y_big_incr = triangle_pixel_size_ * input_->width,
216  x_big_incr = y_big_incr + triangle_pixel_size_;
217  // Reserve enough space
218  polygons.resize (input_->width * input_->height * 4);
219 
220  // Go over the rows first
221  for (int y = 0; y < last_row; y += triangle_pixel_size_)
222  {
223  // Initialize a new row
224  i = y * input_->width;
225  index_right = i + triangle_pixel_size_;
226  index_down = i + y_big_incr;
227  index_down_right = i + x_big_incr;
228 
229  // Go over the columns
230  for (int x = 0; x < last_column; x += triangle_pixel_size_,
231  i += triangle_pixel_size_,
232  index_right += triangle_pixel_size_,
233  index_down += triangle_pixel_size_,
234  index_down_right += triangle_pixel_size_)
235  {
236  const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right);
237  const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right);
238  const bool left_cut_upper = isValidTriangle (i, index_down, index_right);
239  const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right);
240 
241  if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower)
242  {
243  float dist_right_cut = fabsf (input_->points[index_down].z - input_->points[index_right].z);
244  float dist_left_cut = fabsf (input_->points[i].z - input_->points[index_down_right].z);
245  if (dist_right_cut >= dist_left_cut)
246  {
247  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
248  addTriangle (i, index_down_right, index_right, idx++, polygons);
249  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
250  addTriangle (i, index_down, index_down_right, idx++, polygons);
251  }
252  else
253  {
254  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
255  addTriangle (i, index_down, index_right, idx++, polygons);
256  if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
257  addTriangle (index_right, index_down, index_down_right, idx++, polygons);
258  }
259  }
260  else
261  {
262  if (right_cut_upper)
263  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
264  addTriangle (i, index_down_right, index_right, idx++, polygons);
265  if (right_cut_lower)
266  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
267  addTriangle (i, index_down, index_down_right, idx++, polygons);
268  if (left_cut_upper)
269  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
270  addTriangle (i, index_down, index_right, idx++, polygons);
271  if (left_cut_lower)
272  if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
273  addTriangle (index_right, index_down, index_down_right, idx++, polygons);
274  }
275  }
276  }
277  polygons.resize (idx);
278 }
279 
280 #define PCL_INSTANTIATE_OrganizedFastMesh(T) \
281  template class PCL_EXPORTS pcl::OrganizedFastMesh<T>;
282 
283 #endif // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_