Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
organized_fast_mesh.h
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#pragma once
42
43#include <pcl/common/angles.h>
44#include <pcl/common/point_tests.h> // for pcl::isFinite
45#include <pcl/surface/reconstruction.h>
46
47
48namespace pcl
49{
50
51 /** \brief Simple triangulation/surface reconstruction for organized point
52 * clouds. Neighboring points (pixels in image space) are connected to
53 * construct a triangular (or quad) mesh.
54 *
55 * \note If you use this code in any academic work, please cite:
56 * D. Holz and S. Behnke.
57 * Fast Range Image Segmentation and Smoothing using Approximate Surface Reconstruction and Region Growing.
58 * In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS),
59 * Jeju Island, Korea, June 26-29 2012.
60 * <a href="http://purl.org/holz/papers/holz_2012_ias.pdf">http://purl.org/holz/papers/holz_2012_ias.pdf</a>
61 *
62 * \author Dirk Holz, Radu B. Rusu
63 * \ingroup surface
64 */
65 template <typename PointInT>
66 class OrganizedFastMesh : public MeshConstruction<PointInT>
67 {
68 public:
69 using Ptr = shared_ptr<OrganizedFastMesh<PointInT> >;
70 using ConstPtr = shared_ptr<const OrganizedFastMesh<PointInT> >;
71
72 using MeshConstruction<PointInT>::input_;
73 using MeshConstruction<PointInT>::check_tree_;
74
76
77 using Polygons = std::vector<pcl::Vertices>;
78
80 {
81 TRIANGLE_RIGHT_CUT, // _always_ "cuts" a quad from top left to bottom right
82 TRIANGLE_LEFT_CUT, // _always_ "cuts" a quad from top right to bottom left
83 TRIANGLE_ADAPTIVE_CUT, // "cuts" where possible and prefers larger differences in 'z' direction
84 QUAD_MESH // create a simple quad mesh
85 };
86
87 /** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */
89 : max_edge_length_a_ (0.0f)
90 , max_edge_length_b_ (0.0f)
91 , max_edge_length_c_ (0.0f)
92 , max_edge_length_set_ (false)
97 , viewpoint_ (Eigen::Vector3f::Zero ())
98 , store_shadowed_faces_ (false)
99 , cos_angle_tolerance_ (std::abs (std::cos (pcl::deg2rad (12.5f))))
100 , distance_tolerance_ (-1.0f)
101 , distance_dependent_ (false)
103 {
104 check_tree_ = false;
105 };
106
107 /** \brief Destructor. */
108 ~OrganizedFastMesh () override = default;
109
110 /** \brief Set a maximum edge length.
111 * Using not only the scalar \a a, but also \a b and \a c, allows for using a distance threshold in the form of:
112 * threshold(x) = c*x*x + b*x + a
113 * \param[in] a scalar coefficient of the (distance-dependent polynom) threshold
114 * \param[in] b linear coefficient of the (distance-dependent polynom) threshold
115 * \param[in] c quadratic coefficient of the (distance-dependent polynom) threshold
116 */
117 inline void
118 setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f)
119 {
123 max_edge_length_set_ = (max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits<float>::min();
124 };
125
126 inline void
128 {
129 max_edge_length_set_ = false;
130 }
131
132 /** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
133 * \param[in] triangle_size edge length in pixels
134 * (Default: 1 = neighboring pixels are connected)
135 */
136 inline void
137 setTrianglePixelSize (int triangle_size)
138 {
139 setTrianglePixelSizeRows (triangle_size);
140 setTrianglePixelSizeColumns (triangle_size);
141 }
142
143 /** \brief Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh.
144 * \param[in] triangle_size edge length in pixels
145 * (Default: 1 = neighboring pixels are connected)
146 */
147 inline void
148 setTrianglePixelSizeRows (int triangle_size)
149 {
150 triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1));
151 }
152
153 /** \brief Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh.
154 * \param[in] triangle_size edge length in pixels
155 * (Default: 1 = neighboring pixels are connected)
156 */
157 inline void
158 setTrianglePixelSizeColumns (int triangle_size)
159 {
160 triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1));
161 }
162
163 /** \brief Set the triangulation type (see \a TriangulationType)
164 * \param[in] type quad mesh, triangle mesh with fixed left, right cut,
165 * or adaptive cut (splits a quad w.r.t. the depth (z) of the points)
166 */
167 inline void
172
173 /** \brief Set the viewpoint from where the input point cloud has been acquired.
174 * \param[in] viewpoint Vector containing the viewpoint coordinates (in the coordinate system of the data)
175 */
176 inline void setViewpoint (const Eigen::Vector3f& viewpoint)
177 {
178 viewpoint_ = viewpoint;
179 }
180
181 /** \brief Get the viewpoint from where the input point cloud has been acquired. */
182 const inline Eigen::Vector3f& getViewpoint () const
183 {
184 return viewpoint_;
185 }
186
187 /** \brief Store shadowed faces or not.
188 * \param[in] enable set to true to store shadowed faces
189 */
190 inline void
191 storeShadowedFaces (bool enable)
192 {
193 store_shadowed_faces_ = enable;
194 }
195
196 /** \brief Set the angle tolerance used for checking whether or not an edge is occluded.
197 * Standard values are 5deg to 15deg (input in rad!). Set a value smaller than zero to
198 * disable the check for shadowed edges.
199 * \param[in] angle_tolerance Angle tolerance (in rad). Set a value <0 to disable.
200 */
201 inline void
202 setAngleTolerance(float angle_tolerance)
203 {
204 if (angle_tolerance > 0)
205 cos_angle_tolerance_ = std::abs (std::cos (angle_tolerance));
206 else
207 cos_angle_tolerance_ = -1.0f;
208 }
209
210
211 inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false)
212 {
213 distance_tolerance_ = distance_tolerance;
214 if (distance_tolerance_ < 0)
215 return;
216
217 distance_dependent_ = depth_dependent;
220 }
221
222 /** \brief Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpoint).
223 * \param[in] enable Set to true skips comptations and further speeds up computation by using depth instead of computing distance. false to disable. */
224 inline void useDepthAsDistance(bool enable)
225 {
226 use_depth_as_distance_ = enable;
227 }
228
229 protected:
230 /** \brief max length of edge, scalar component */
232 /** \brief max length of edge, scalar component */
234 /** \brief max length of edge, scalar component */
236 /** \brief flag whether or not edges are limited in length */
238
239 /** \brief flag whether or not max edge length is distance dependent. */
241
242 /** \brief size of triangle edges (in pixels) for iterating over rows. */
244
245 /** \brief size of triangle edges (in pixels) for iterating over columns*/
247
248 /** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */
250
251 /** \brief Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data). */
252 Eigen::Vector3f viewpoint_;
253
254 /** \brief Whether or not shadowed faces are stored, e.g., for exploration */
256
257 /** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */
259
260 /** \brief distance tolerance for filtering out shadowed/occluded edges */
262
263 /** \brief flag whether or not \a distance_tolerance_ is distance dependent (multiplied by the squared distance to the point) or not. */
265
266 /** \brief flag whether or not the points' depths are used instead of measured distances (points' distances to the viewpoint).
267 This flag may be set using useDepthAsDistance(true) for (RGB-)Depth cameras to skip computations and gain additional speed up. */
269
270
271 /** \brief Perform the actual polygonal reconstruction.
272 * \param[out] polygons the resultant polygons
273 */
274 void
275 reconstructPolygons (std::vector<pcl::Vertices>& polygons);
276
277 /** \brief Create the surface.
278 * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
279 */
280 void
281 performReconstruction (std::vector<pcl::Vertices> &polygons) override;
282
283 /** \brief Create the surface.
284 *
285 * Simply uses image indices to create an initial polygonal mesh for organized point clouds.
286 * \a indices_ are ignored!
287 *
288 * \param[out] output the resultant polygonal mesh
289 */
290 void
291 performReconstruction (pcl::PolygonMesh &output) override;
292
293 /** \brief Add a new triangle to the current polygon mesh
294 * \param[in] a index of the first vertex
295 * \param[in] b index of the second vertex
296 * \param[in] c index of the third vertex
297 * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
298 * \param[out] polygons the polygon mesh to be updated
299 */
300 inline void
301 addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
302 {
303 assert (idx < static_cast<int> (polygons.size ()));
304 polygons[idx].vertices.resize (3);
305 polygons[idx].vertices[0] = a;
306 polygons[idx].vertices[1] = b;
307 polygons[idx].vertices[2] = c;
308 }
309
310 /** \brief Add a new quad to the current polygon mesh
311 * \param[in] a index of the first vertex
312 * \param[in] b index of the second vertex
313 * \param[in] c index of the third vertex
314 * \param[in] d index of the fourth vertex
315 * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
316 * \param[out] polygons the polygon mesh to be updated
317 */
318 inline void
319 addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
320 {
321 assert (idx < static_cast<int> (polygons.size ()));
322 polygons[idx].vertices.resize (4);
323 polygons[idx].vertices[0] = a;
324 polygons[idx].vertices[1] = b;
325 polygons[idx].vertices[2] = c;
326 polygons[idx].vertices[3] = d;
327 }
328
329 /** \brief Set (all) coordinates of a particular point to the specified value
330 * \param[in] point_index index of point
331 * \param[out] mesh to modify
332 * \param[in] value value to use when re-setting
333 * \param[in] field_x_idx the X coordinate of the point
334 * \param[in] field_y_idx the Y coordinate of the point
335 * \param[in] field_z_idx the Z coordinate of the point
336 */
337 inline void
338 resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
339 int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
340 {
341 float new_value = value;
342 memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
343 memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
344 memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
345 }
346
347 /** \brief Check if a point is shadowed by another point
348 * \param[in] point_a the first point
349 * \param[in] point_b the second point
350 */
351 inline bool
352 isShadowed (const PointInT& point_a, const PointInT& point_b)
353 {
354 bool valid = true;
355
356 Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap ();
357 Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
358 float distance_to_points = dir_a.norm ();
359 float distance_between_points = dir_b.norm ();
360
361 if (cos_angle_tolerance_ > 0)
362 {
363 float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
364 if (std::isnan(cos_angle))
365 cos_angle = 1.0f;
366 bool check_angle = std::fabs (cos_angle) >= cos_angle_tolerance_;
367
368 bool check_distance = true;
369 if (check_angle && (distance_tolerance_ > 0))
370 {
371 float dist_thresh = distance_tolerance_;
373 {
374 float d = distance_to_points;
376 d = std::max(point_a.z, point_b.z);
377 dist_thresh *= d*d;
378 dist_thresh *= dist_thresh; // distance_tolerance_ is already squared if distance_dependent_ is false.
379 }
380 check_distance = (distance_between_points > dist_thresh);
381 }
382 valid = !(check_angle && check_distance);
383 }
384
385 // check if max. edge length is not exceeded
387 {
388 float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points);
389 float dist_thresh = max_edge_length_a_;
390 if (std::fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
391 dist_thresh += max_edge_length_b_ * dist;
392 if (std::fabs(max_edge_length_c_) > std::numeric_limits<float>::min())
393 dist_thresh += max_edge_length_c_ * dist * dist;
394 valid = (distance_between_points <= dist_thresh);
395 }
396
397 return !valid;
398 }
399
400 /** \brief Check if a triangle is valid.
401 * \param[in] a index of the first vertex
402 * \param[in] b index of the second vertex
403 * \param[in] c index of the third vertex
404 */
405 inline bool
406 isValidTriangle (const int& a, const int& b, const int& c)
407 {
408 if (!pcl::isFinite ((*input_)[a])) return (false);
409 if (!pcl::isFinite ((*input_)[b])) return (false);
410 if (!pcl::isFinite ((*input_)[c])) return (false);
411 return (true);
412 }
413
414 /** \brief Check if a triangle is shadowed.
415 * \param[in] a index of the first vertex
416 * \param[in] b index of the second vertex
417 * \param[in] c index of the third vertex
418 */
419 inline bool
420 isShadowedTriangle (const int& a, const int& b, const int& c)
421 {
422 if (isShadowed ((*input_)[a], (*input_)[b])) return (true);
423 if (isShadowed ((*input_)[b], (*input_)[c])) return (true);
424 if (isShadowed ((*input_)[c], (*input_)[a])) return (true);
425 return (false);
426 }
427
428 /** \brief Check if a quad is valid.
429 * \param[in] a index of the first vertex
430 * \param[in] b index of the second vertex
431 * \param[in] c index of the third vertex
432 * \param[in] d index of the fourth vertex
433 */
434 inline bool
435 isValidQuad (const int& a, const int& b, const int& c, const int& d)
436 {
437 if (!pcl::isFinite ((*input_)[a])) return (false);
438 if (!pcl::isFinite ((*input_)[b])) return (false);
439 if (!pcl::isFinite ((*input_)[c])) return (false);
440 if (!pcl::isFinite ((*input_)[d])) return (false);
441 return (true);
442 }
443
444 /** \brief Check if a triangle is shadowed.
445 * \param[in] a index of the first vertex
446 * \param[in] b index of the second vertex
447 * \param[in] c index of the third vertex
448 * \param[in] d index of the fourth vertex
449 */
450 inline bool
451 isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
452 {
453 if (isShadowed ((*input_)[a], (*input_)[b])) return (true);
454 if (isShadowed ((*input_)[b], (*input_)[c])) return (true);
455 if (isShadowed ((*input_)[c], (*input_)[d])) return (true);
456 if (isShadowed ((*input_)[d], (*input_)[a])) return (true);
457 return (false);
458 }
459
460 /** \brief Create a quad mesh.
461 * \param[out] polygons the resultant mesh
462 */
463 void
464 makeQuadMesh (std::vector<pcl::Vertices>& polygons);
465
466 /** \brief Create a right cut mesh.
467 * \param[out] polygons the resultant mesh
468 */
469 void
470 makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
471
472 /** \brief Create a left cut mesh.
473 * \param[out] polygons the resultant mesh
474 */
475 void
476 makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
477
478 /** \brief Create an adaptive cut mesh.
479 * \param[out] polygons the resultant mesh
480 */
481 void
482 makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
483 };
484}
485
486#ifdef PCL_NO_PRECOMPILE
487#include <pcl/surface/impl/organized_fast_mesh.hpp>
488#endif
Define standard C methods to do angle calculations.
MeshConstruction represents a base surface reconstruction class.
bool check_tree_
A flag specifying whether or not the derived reconstruction algorithm needs the search object tree.
Simple triangulation/surface reconstruction for organized point clouds.
void addTriangle(int a, int b, int c, int idx, std::vector< pcl::Vertices > &polygons)
Add a new triangle to the current polygon mesh.
bool isShadowedQuad(const int &a, const int &b, const int &c, const int &d)
Check if a triangle is shadowed.
shared_ptr< OrganizedFastMesh< PointInT > > Ptr
void makeRightCutMesh(std::vector< pcl::Vertices > &polygons)
Create a right cut mesh.
float distance_tolerance_
distance tolerance for filtering out shadowed/occluded edges
bool isValidQuad(const int &a, const int &b, const int &c, const int &d)
Check if a quad is valid.
std::vector< pcl::Vertices > Polygons
bool use_depth_as_distance_
flag whether or not the points' depths are used instead of measured distances (points' distances to t...
void useDepthAsDistance(bool enable)
Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpo...
void makeQuadMesh(std::vector< pcl::Vertices > &polygons)
Create a quad mesh.
float max_edge_length_b_
max length of edge, scalar component
Eigen::Vector3f viewpoint_
Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data).
float cos_angle_tolerance_
(Cosine of the) angle tolerance used when checking whether or not an edge between two points is shado...
int triangle_pixel_size_columns_
size of triangle edges (in pixels) for iterating over columns
void setViewpoint(const Eigen::Vector3f &viewpoint)
Set the viewpoint from where the input point cloud has been acquired.
void storeShadowedFaces(bool enable)
Store shadowed faces or not.
void addQuad(int a, int b, int c, int d, int idx, std::vector< pcl::Vertices > &polygons)
Add a new quad to the current polygon mesh.
void reconstructPolygons(std::vector< pcl::Vertices > &polygons)
Perform the actual polygonal reconstruction.
bool isValidTriangle(const int &a, const int &b, const int &c)
Check if a triangle is valid.
float max_edge_length_a_
max length of edge, scalar component
bool max_edge_length_dist_dependent_
flag whether or not max edge length is distance dependent.
void setTriangulationType(TriangulationType type)
Set the triangulation type (see TriangulationType)
shared_ptr< const OrganizedFastMesh< PointInT > > ConstPtr
void setTrianglePixelSize(int triangle_size)
Set the edge length (in pixels) used for constructing the fixed mesh.
typename pcl::PointCloud< PointInT >::Ptr PointCloudPtr
bool max_edge_length_set_
flag whether or not edges are limited in length
int triangle_pixel_size_rows_
size of triangle edges (in pixels) for iterating over rows.
void setMaxEdgeLength(float a, float b=0.0f, float c=0.0f)
Set a maximum edge length.
void setTrianglePixelSizeRows(int triangle_size)
Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh.
bool distance_dependent_
flag whether or not distance_tolerance_ is distance dependent (multiplied by the squared distance to ...
bool store_shadowed_faces_
Whether or not shadowed faces are stored, e.g., for exploration.
void setDistanceTolerance(float distance_tolerance, bool depth_dependent=false)
bool isShadowedTriangle(const int &a, const int &b, const int &c)
Check if a triangle is shadowed.
float max_edge_length_c_
max length of edge, scalar component
const Eigen::Vector3f & getViewpoint() const
Get the viewpoint from where the input point cloud has been acquired.
void setTrianglePixelSizeColumns(int triangle_size)
Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh.
void setAngleTolerance(float angle_tolerance)
Set the angle tolerance used for checking whether or not an edge is occluded.
bool isShadowed(const PointInT &point_a, const PointInT &point_b)
Check if a point is shadowed by another point.
void performReconstruction(std::vector< pcl::Vertices > &polygons) override
Create the surface.
TriangulationType triangulation_type_
Type of meshing scheme (quads vs.
void makeAdaptiveCutMesh(std::vector< pcl::Vertices > &polygons)
Create an adaptive cut mesh.
void makeLeftCutMesh(std::vector< pcl::Vertices > &polygons)
Create a left cut mesh.
~OrganizedFastMesh() override=default
Destructor.
void resetPointData(const int &point_index, pcl::PolygonMesh &mesh, const float &value=0.0f, int field_x_idx=0, int field_y_idx=1, int field_z_idx=2)
Set (all) coordinates of a particular point to the specified value.
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
shared_ptr< PointCloud< PointT > > Ptr
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition angles.hpp:67
Definition bfgs.h:10
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::PCLPointField > fields
std::vector< std::uint8_t > data
::pcl::PCLPointCloud2 cloud
Definition PolygonMesh.h:20