Point Cloud Library (PCL)  1.12.1-dev
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 
48 namespace 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 
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)
102  , use_depth_as_distance_(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  {
120  max_edge_length_a_ = a;
121  max_edge_length_b_ = b;
122  max_edge_length_c_ = c;
123  if ((max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits<float>::min())
124  max_edge_length_set_ = true;
125  else
126  max_edge_length_set_ = false;
127  };
128 
129  inline void
131  {
132  max_edge_length_set_ = false;
133  }
134 
135  /** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
136  * \param[in] triangle_size edge length in pixels
137  * (Default: 1 = neighboring pixels are connected)
138  */
139  inline void
140  setTrianglePixelSize (int triangle_size)
141  {
142  setTrianglePixelSizeRows (triangle_size);
143  setTrianglePixelSizeColumns (triangle_size);
144  }
145 
146  /** \brief Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh.
147  * \param[in] triangle_size edge length in pixels
148  * (Default: 1 = neighboring pixels are connected)
149  */
150  inline void
151  setTrianglePixelSizeRows (int triangle_size)
152  {
153  triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1));
154  }
155 
156  /** \brief Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh.
157  * \param[in] triangle_size edge length in pixels
158  * (Default: 1 = neighboring pixels are connected)
159  */
160  inline void
161  setTrianglePixelSizeColumns (int triangle_size)
162  {
163  triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1));
164  }
165 
166  /** \brief Set the triangulation type (see \a TriangulationType)
167  * \param[in] type quad mesh, triangle mesh with fixed left, right cut,
168  * or adaptive cut (splits a quad w.r.t. the depth (z) of the points)
169  */
170  inline void
172  {
173  triangulation_type_ = type;
174  }
175 
176  /** \brief Set the viewpoint from where the input point cloud has been acquired.
177  * \param[in] viewpoint Vector containing the viewpoint coordinates (in the coordinate system of the data)
178  */
179  inline void setViewpoint (const Eigen::Vector3f& viewpoint)
180  {
181  viewpoint_ = viewpoint;
182  }
183 
184  /** \brief Get the viewpoint from where the input point cloud has been acquired. */
185  const inline Eigen::Vector3f& getViewpoint () const
186  {
187  return viewpoint_;
188  }
189 
190  /** \brief Store shadowed faces or not.
191  * \param[in] enable set to true to store shadowed faces
192  */
193  inline void
194  storeShadowedFaces (bool enable)
195  {
196  store_shadowed_faces_ = enable;
197  }
198 
199  /** \brief Set the angle tolerance used for checking whether or not an edge is occluded.
200  * Standard values are 5deg to 15deg (input in rad!). Set a value smaller than zero to
201  * disable the check for shadowed edges.
202  * \param[in] angle_tolerance Angle tolerance (in rad). Set a value <0 to disable.
203  */
204  inline void
205  setAngleTolerance(float angle_tolerance)
206  {
207  if (angle_tolerance > 0)
208  cos_angle_tolerance_ = std::abs (std::cos (angle_tolerance));
209  else
210  cos_angle_tolerance_ = -1.0f;
211  }
212 
213 
214  inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false)
215  {
216  distance_tolerance_ = distance_tolerance;
217  if (distance_tolerance_ < 0)
218  return;
219 
220  distance_dependent_ = depth_dependent;
221  if (!distance_dependent_)
223  }
224 
225  /** \brief Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpoint).
226  * \param[in] enable Set to true skips comptations and further speeds up computation by using depth instead of computing distance. false to disable. */
227  inline void useDepthAsDistance(bool enable)
228  {
229  use_depth_as_distance_ = enable;
230  }
231 
232  protected:
233  /** \brief max length of edge, scalar component */
235  /** \brief max length of edge, scalar component */
237  /** \brief max length of edge, scalar component */
239  /** \brief flag whether or not edges are limited in length */
241 
242  /** \brief flag whether or not max edge length is distance dependent. */
244 
245  /** \brief size of triangle edges (in pixels) for iterating over rows. */
247 
248  /** \brief size of triangle edges (in pixels) for iterating over columns*/
250 
251  /** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */
253 
254  /** \brief Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data). */
255  Eigen::Vector3f viewpoint_;
256 
257  /** \brief Whether or not shadowed faces are stored, e.g., for exploration */
259 
260  /** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */
262 
263  /** \brief distance tolerance for filtering out shadowed/occluded edges */
265 
266  /** \brief flag whether or not \a distance_tolerance_ is distance dependent (multiplied by the squared distance to the point) or not. */
268 
269  /** \brief flag whether or not the points' depths are used instead of measured distances (points' distances to the viewpoint).
270  This flag may be set using useDepthAsDistance(true) for (RGB-)Depth cameras to skip computations and gain additional speed up. */
272 
273 
274  /** \brief Perform the actual polygonal reconstruction.
275  * \param[out] polygons the resultant polygons
276  */
277  void
278  reconstructPolygons (std::vector<pcl::Vertices>& polygons);
279 
280  /** \brief Create the surface.
281  * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
282  */
283  void
284  performReconstruction (std::vector<pcl::Vertices> &polygons) override;
285 
286  /** \brief Create the surface.
287  *
288  * Simply uses image indices to create an initial polygonal mesh for organized point clouds.
289  * \a indices_ are ignored!
290  *
291  * \param[out] output the resultant polygonal mesh
292  */
293  void
294  performReconstruction (pcl::PolygonMesh &output) override;
295 
296  /** \brief Add a new triangle to the current polygon mesh
297  * \param[in] a index of the first vertex
298  * \param[in] b index of the second vertex
299  * \param[in] c index of the third vertex
300  * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
301  * \param[out] polygons the polygon mesh to be updated
302  */
303  inline void
304  addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
305  {
306  assert (idx < static_cast<int> (polygons.size ()));
307  polygons[idx].vertices.resize (3);
308  polygons[idx].vertices[0] = a;
309  polygons[idx].vertices[1] = b;
310  polygons[idx].vertices[2] = c;
311  }
312 
313  /** \brief Add a new quad to the current polygon mesh
314  * \param[in] a index of the first vertex
315  * \param[in] b index of the second vertex
316  * \param[in] c index of the third vertex
317  * \param[in] d index of the fourth vertex
318  * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
319  * \param[out] polygons the polygon mesh to be updated
320  */
321  inline void
322  addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
323  {
324  assert (idx < static_cast<int> (polygons.size ()));
325  polygons[idx].vertices.resize (4);
326  polygons[idx].vertices[0] = a;
327  polygons[idx].vertices[1] = b;
328  polygons[idx].vertices[2] = c;
329  polygons[idx].vertices[3] = d;
330  }
331 
332  /** \brief Set (all) coordinates of a particular point to the specified value
333  * \param[in] point_index index of point
334  * \param[out] mesh to modify
335  * \param[in] value value to use when re-setting
336  * \param[in] field_x_idx the X coordinate of the point
337  * \param[in] field_y_idx the Y coordinate of the point
338  * \param[in] field_z_idx the Z coordinate of the point
339  */
340  inline void
341  resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
342  int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
343  {
344  float new_value = value;
345  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
346  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
347  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
348  }
349 
350  /** \brief Check if a point is shadowed by another point
351  * \param[in] point_a the first point
352  * \param[in] point_b the second point
353  */
354  inline bool
355  isShadowed (const PointInT& point_a, const PointInT& point_b)
356  {
357  bool valid = true;
358 
359  Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap ();
360  Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
361  float distance_to_points = dir_a.norm ();
362  float distance_between_points = dir_b.norm ();
363 
364  if (cos_angle_tolerance_ > 0)
365  {
366  float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
367  if (std::isnan(cos_angle))
368  cos_angle = 1.0f;
369  bool check_angle = std::fabs (cos_angle) >= cos_angle_tolerance_;
370 
371  bool check_distance = true;
372  if (check_angle && (distance_tolerance_ > 0))
373  {
374  float dist_thresh = distance_tolerance_;
376  {
377  float d = distance_to_points;
379  d = std::max(point_a.z, point_b.z);
380  dist_thresh *= d*d;
381  dist_thresh *= dist_thresh; // distance_tolerance_ is already squared if distance_dependent_ is false.
382  }
383  check_distance = (distance_between_points > dist_thresh);
384  }
385  valid = !(check_angle && check_distance);
386  }
387 
388  // check if max. edge length is not exceeded
390  {
391  float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points);
392  float dist_thresh = max_edge_length_a_;
393  if (std::fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
394  dist_thresh += max_edge_length_b_ * dist;
395  if (std::fabs(max_edge_length_c_) > std::numeric_limits<float>::min())
396  dist_thresh += max_edge_length_c_ * dist * dist;
397  valid = (distance_between_points <= dist_thresh);
398  }
399 
400  return !valid;
401  }
402 
403  /** \brief Check if a triangle is valid.
404  * \param[in] a index of the first vertex
405  * \param[in] b index of the second vertex
406  * \param[in] c index of the third vertex
407  */
408  inline bool
409  isValidTriangle (const int& a, const int& b, const int& c)
410  {
411  if (!pcl::isFinite ((*input_)[a])) return (false);
412  if (!pcl::isFinite ((*input_)[b])) return (false);
413  if (!pcl::isFinite ((*input_)[c])) return (false);
414  return (true);
415  }
416 
417  /** \brief Check if a triangle is shadowed.
418  * \param[in] a index of the first vertex
419  * \param[in] b index of the second vertex
420  * \param[in] c index of the third vertex
421  */
422  inline bool
423  isShadowedTriangle (const int& a, const int& b, const int& c)
424  {
425  if (isShadowed ((*input_)[a], (*input_)[b])) return (true);
426  if (isShadowed ((*input_)[b], (*input_)[c])) return (true);
427  if (isShadowed ((*input_)[c], (*input_)[a])) return (true);
428  return (false);
429  }
430 
431  /** \brief Check if a quad is valid.
432  * \param[in] a index of the first vertex
433  * \param[in] b index of the second vertex
434  * \param[in] c index of the third vertex
435  * \param[in] d index of the fourth vertex
436  */
437  inline bool
438  isValidQuad (const int& a, const int& b, const int& c, const int& d)
439  {
440  if (!pcl::isFinite ((*input_)[a])) return (false);
441  if (!pcl::isFinite ((*input_)[b])) return (false);
442  if (!pcl::isFinite ((*input_)[c])) return (false);
443  if (!pcl::isFinite ((*input_)[d])) return (false);
444  return (true);
445  }
446 
447  /** \brief Check if a triangle is shadowed.
448  * \param[in] a index of the first vertex
449  * \param[in] b index of the second vertex
450  * \param[in] c index of the third vertex
451  * \param[in] d index of the fourth vertex
452  */
453  inline bool
454  isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
455  {
456  if (isShadowed ((*input_)[a], (*input_)[b])) return (true);
457  if (isShadowed ((*input_)[b], (*input_)[c])) return (true);
458  if (isShadowed ((*input_)[c], (*input_)[d])) return (true);
459  if (isShadowed ((*input_)[d], (*input_)[a])) return (true);
460  return (false);
461  }
462 
463  /** \brief Create a quad mesh.
464  * \param[out] polygons the resultant mesh
465  */
466  void
467  makeQuadMesh (std::vector<pcl::Vertices>& polygons);
468 
469  /** \brief Create a right cut mesh.
470  * \param[out] polygons the resultant mesh
471  */
472  void
473  makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
474 
475  /** \brief Create a left cut mesh.
476  * \param[out] polygons the resultant mesh
477  */
478  void
479  makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
480 
481  /** \brief Create an adaptive cut mesh.
482  * \param[out] polygons the resultant mesh
483  */
484  void
485  makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
486  };
487 }
488 
489 #ifdef PCL_NO_PRECOMPILE
490 #include <pcl/surface/impl/organized_fast_mesh.hpp>
491 #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.
const Eigen::Vector3f & getViewpoint() const
Get the viewpoint from where the input point cloud has been acquired.
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
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
Definition: point_cloud.h:413
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