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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:14