Point Cloud Library (PCL)  1.14.1-dev
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 #include <pcl/common/io.h> // for getFieldIndex
46 
47 /////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointInT> void
50 {
51  if (!input_->isOrganized()) {
52  PCL_ERROR("[OrganizedFastMesh::performReconstruction] Input point cloud must be organized but isn't!\n");
53  return;
54  }
55  reconstructPolygons (output.polygons);
56 
57  // Get the field names
58  int x_idx = pcl::getFieldIndex (output.cloud, "x");
59  int y_idx = pcl::getFieldIndex (output.cloud, "y");
60  int z_idx = pcl::getFieldIndex (output.cloud, "z");
61  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
62  return;
63  // correct all measurements,
64  // (running over complete image since some rows and columns are left out
65  // depending on triangle_pixel_size)
66  // avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files
67  for (std::size_t i = 0; i < input_->size (); ++i)
68  if (!isFinite ((*input_)[i]))
69  resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx);
70 }
71 
72 /////////////////////////////////////////////////////////////////////////////////////////////
73 template <typename PointInT> void
74 pcl::OrganizedFastMesh<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
75 {
76  if (!input_->isOrganized()) {
77  PCL_ERROR("[OrganizedFastMesh::performReconstruction] Input point cloud must be organized but isn't!\n");
78  return;
79  }
80  reconstructPolygons (polygons);
81 }
82 
83 /////////////////////////////////////////////////////////////////////////////////////////////
84 template <typename PointInT> void
85 pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
86 {
87  if (triangulation_type_ == TRIANGLE_RIGHT_CUT)
88  makeRightCutMesh (polygons);
89  else if (triangulation_type_ == TRIANGLE_LEFT_CUT)
90  makeLeftCutMesh (polygons);
91  else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT)
92  makeAdaptiveCutMesh (polygons);
93  else if (triangulation_type_ == QUAD_MESH)
94  makeQuadMesh (polygons);
95 }
96 
97 /////////////////////////////////////////////////////////////////////////////////////////////
98 template <typename PointInT> void
99 pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons)
100 {
101  int last_column = input_->width - triangle_pixel_size_columns_;
102  int last_row = input_->height - triangle_pixel_size_rows_;
103 
104  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
105  int y_big_incr = triangle_pixel_size_rows_ * input_->width,
106  x_big_incr = y_big_incr + triangle_pixel_size_columns_;
107  // Reserve enough space
108  polygons.resize (input_->width * input_->height);
109 
110  // Go over the rows first
111  for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
112  {
113  // Initialize a new row
114  i = y * input_->width;
115  index_right = i + triangle_pixel_size_columns_;
116  index_down = i + y_big_incr;
117  index_down_right = i + x_big_incr;
118 
119  // Go over the columns
120  for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
121  i += triangle_pixel_size_columns_,
122  index_right += triangle_pixel_size_columns_,
123  index_down += triangle_pixel_size_columns_,
124  index_down_right += triangle_pixel_size_columns_)
125  {
126  if (isValidQuad (i, index_right, index_down_right, index_down))
127  if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down))
128  addQuad (i, index_right, index_down_right, index_down, idx++, polygons);
129  }
130  }
131  polygons.resize (idx);
132 }
133 
134 /////////////////////////////////////////////////////////////////////////////////////////////
135 template <typename PointInT> void
136 pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons)
137 {
138  int last_column = input_->width - triangle_pixel_size_columns_;
139  int last_row = input_->height - triangle_pixel_size_rows_;
140 
141  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
142  int y_big_incr = triangle_pixel_size_rows_ * input_->width,
143  x_big_incr = y_big_incr + triangle_pixel_size_columns_;
144  // Reserve enough space
145  polygons.resize (input_->width * input_->height * 2);
146 
147  // Go over the rows first
148  for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
149  {
150  // Initialize a new row
151  i = y * input_->width;
152  index_right = i + triangle_pixel_size_columns_;
153  index_down = i + y_big_incr;
154  index_down_right = i + x_big_incr;
155 
156  // Go over the columns
157  for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
158  i += triangle_pixel_size_columns_,
159  index_right += triangle_pixel_size_columns_,
160  index_down += triangle_pixel_size_columns_,
161  index_down_right += triangle_pixel_size_columns_)
162  {
163  if (isValidTriangle (i, index_down_right, index_right))
164  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
165  addTriangle (i, index_down_right, index_right, idx++, polygons);
166 
167  if (isValidTriangle (i, index_down, index_down_right))
168  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
169  addTriangle (i, index_down, index_down_right, idx++, polygons);
170  }
171  }
172  polygons.resize (idx);
173 }
174 
175 /////////////////////////////////////////////////////////////////////////////////////////////
176 template <typename PointInT> void
177 pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons)
178 {
179  int last_column = input_->width - triangle_pixel_size_columns_;
180  int last_row = input_->height - triangle_pixel_size_rows_;
181 
182  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
183  int y_big_incr = triangle_pixel_size_rows_ * input_->width,
184  x_big_incr = y_big_incr + triangle_pixel_size_columns_;
185  // Reserve enough space
186  polygons.resize (input_->width * input_->height * 2);
187 
188  // Go over the rows first
189  for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
190  {
191  // Initialize a new row
192  i = y * input_->width;
193  index_right = i + triangle_pixel_size_columns_;
194  index_down = i + y_big_incr;
195  index_down_right = i + x_big_incr;
196 
197  // Go over the columns
198  for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
199  i += triangle_pixel_size_columns_,
200  index_right += triangle_pixel_size_columns_,
201  index_down += triangle_pixel_size_columns_,
202  index_down_right += triangle_pixel_size_columns_)
203  {
204  if (isValidTriangle (i, index_down, index_right))
205  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
206  addTriangle (i, index_down, index_right, idx++, polygons);
207 
208  if (isValidTriangle (index_right, index_down, index_down_right))
209  if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
210  addTriangle (index_right, index_down, index_down_right, idx++, polygons);
211  }
212  }
213  polygons.resize (idx);
214 }
215 
216 /////////////////////////////////////////////////////////////////////////////////////////////
217 template <typename PointInT> void
218 pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons)
219 {
220  int last_column = input_->width - triangle_pixel_size_columns_;
221  int last_row = input_->height - triangle_pixel_size_rows_;
222 
223  int idx = 0;
224  int y_big_incr = triangle_pixel_size_rows_ * input_->width,
225  x_big_incr = y_big_incr + triangle_pixel_size_columns_;
226  // Reserve enough space
227  polygons.resize (input_->width * input_->height * 2);
228 
229  // Go over the rows first
230  for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
231  {
232  // Initialize a new row
233  int i = y * input_->width;
234  int index_right = i + triangle_pixel_size_columns_;
235  int index_down = i + y_big_incr;
236  int index_down_right = i + x_big_incr;
237 
238  // Go over the columns
239  for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
240  i += triangle_pixel_size_columns_,
241  index_right += triangle_pixel_size_columns_,
242  index_down += triangle_pixel_size_columns_,
243  index_down_right += triangle_pixel_size_columns_)
244  {
245  const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right);
246  const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right);
247  const bool left_cut_upper = isValidTriangle (i, index_down, index_right);
248  const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right);
249 
250  if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower)
251  {
252  float dist_right_cut = std::abs ((*input_)[index_down].z - (*input_)[index_right].z);
253  float dist_left_cut = std::abs ((*input_)[i].z - (*input_)[index_down_right].z);
254  if (dist_right_cut >= dist_left_cut)
255  {
256  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
257  addTriangle (i, index_down_right, index_right, idx++, polygons);
258  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
259  addTriangle (i, index_down, index_down_right, idx++, polygons);
260  }
261  else
262  {
263  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
264  addTriangle (i, index_down, index_right, idx++, polygons);
265  if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
266  addTriangle (index_right, index_down, index_down_right, idx++, polygons);
267  }
268  }
269  else
270  {
271  if (right_cut_upper)
272  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
273  addTriangle (i, index_down_right, index_right, idx++, polygons);
274  if (right_cut_lower)
275  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
276  addTriangle (i, index_down, index_down_right, idx++, polygons);
277  if (left_cut_upper)
278  if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
279  addTriangle (i, index_down, index_right, idx++, polygons);
280  if (left_cut_lower)
281  if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
282  addTriangle (index_right, index_down, index_down_right, idx++, polygons);
283  }
284  }
285  }
286  polygons.resize (idx);
287 }
288 
289 #define PCL_INSTANTIATE_OrganizedFastMesh(T) \
290  template class PCL_EXPORTS pcl::OrganizedFastMesh<T>;
291 
292 #endif // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
void makeRightCutMesh(std::vector< pcl::Vertices > &polygons)
Create a right cut mesh.
void makeQuadMesh(std::vector< pcl::Vertices > &polygons)
Create a quad mesh.
void reconstructPolygons(std::vector< pcl::Vertices > &polygons)
Perform the actual polygonal reconstruction.
void performReconstruction(std::vector< pcl::Vertices > &polygons) override
Create the surface.
void makeAdaptiveCutMesh(std::vector< pcl::Vertices > &polygons)
Create an adaptive cut mesh.
void makeLeftCutMesh(std::vector< pcl::Vertices > &polygons)
Create a left cut mesh.
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition: io.hpp:52
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:22
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:20