Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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/////////////////////////////////////////////////////////////////////////////////////////////
48template <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/////////////////////////////////////////////////////////////////////////////////////////////
73template <typename PointInT> void
74pcl::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/////////////////////////////////////////////////////////////////////////////////////////////
84template <typename PointInT> void
85pcl::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/////////////////////////////////////////////////////////////////////////////////////////////
98template <typename PointInT> void
99pcl::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/////////////////////////////////////////////////////////////////////////////////////////////
135template <typename PointInT> void
136pcl::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/////////////////////////////////////////////////////////////////////////////////////////////
176template <typename PointInT> void
177pcl::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/////////////////////////////////////////////////////////////////////////////////////////////
217template <typename PointInT> void
218pcl::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