organized_fast_mesh.h
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_H_
42 #define PCL_SURFACE_ORGANIZED_FAST_MESH_H_
43 
44 #include <pcl/common/angles.h>
45 #include <pcl/surface/reconstruction.h>
46 
47 namespace pcl
48 {
49 
64  template <typename PointInT>
65  class OrganizedFastMesh : public pcl::MeshConstruction<PointInT>
66  {
67  public:
70 
71  using pcl::MeshConstruction<PointInT>::input_;
72  using pcl::MeshConstruction<PointInT>::check_tree_;
73 
74  typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
75 
76  typedef std::vector<pcl::Vertices> Polygons;
77 
79  {
80  TRIANGLE_RIGHT_CUT, // _always_ "cuts" a quad from top left to bottom right
81  TRIANGLE_LEFT_CUT, // _always_ "cuts" a quad from top right to bottom left
82  TRIANGLE_ADAPTIVE_CUT, // "cuts" where possible and prefers larger differences in 'z' direction
83  QUAD_MESH // create a simple quad mesh
84  };
85 
88  : max_edge_length_a_ (0.0f)
89  , max_edge_length_b_ (0.0f)
90  , max_edge_length_c_ (0.0f)
96  , viewpoint_ (Eigen::Vector3f::Zero ())
98  , cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f))))
99  , distance_tolerance_ (-1.0f)
102  {
103  check_tree_ = false;
104  };
105 
107  virtual ~OrganizedFastMesh () {};
108 
116  inline void
117  setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f)
118  {
119  max_edge_length_a_ = a;
120  max_edge_length_b_ = b;
121  max_edge_length_c_ = c;
123  max_edge_length_set_ = true;
124  else
125  max_edge_length_set_ = false;
126  };
127 
128  inline void
130  {
131  max_edge_length_set_ = false;
132  }
133 
138  inline void
139  setTrianglePixelSize (int triangle_size)
140  {
141  setTrianglePixelSizeRows (triangle_size);
142  setTrianglePixelSizeColumns (triangle_size);
143  }
144 
149  inline void
150  setTrianglePixelSizeRows (int triangle_size)
151  {
152  triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1));
153  }
154 
159  inline void
160  setTrianglePixelSizeColumns (int triangle_size)
161  {
162  triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1));
163  }
164 
169  inline void
171  {
172  triangulation_type_ = type;
173  }
174 
178  inline void setViewpoint (const Eigen::Vector3f& viewpoint)
179  {
180  viewpoint_ = viewpoint;
181  }
182 
184  const inline Eigen::Vector3f& getViewpoint () const
185  {
186  return viewpoint_;
187  }
188 
192  inline void
193  storeShadowedFaces (bool enable)
194  {
195  store_shadowed_faces_ = enable;
196  }
197 
203  inline void
204  setAngleTolerance(float angle_tolerance)
205  {
206  if (angle_tolerance > 0)
207  cos_angle_tolerance_ = fabsf (cosf (angle_tolerance));
208  else
209  cos_angle_tolerance_ = -1.0f;
210  }
211 
212 
213  inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false)
214  {
215  distance_tolerance_ = distance_tolerance;
216  if (distance_tolerance_ < 0)
217  return;
218 
219  distance_dependent_ = depth_dependent;
220  if (!distance_dependent_)
222  }
223 
226  inline void useDepthAsDistance(bool enable)
227  {
228  use_depth_as_distance_ = enable;
229  }
230 
231  protected:
240 
243 
246 
249 
252 
254  Eigen::Vector3f viewpoint_;
255 
258 
261 
264 
267 
271 
272 
276  void
277  reconstructPolygons (std::vector<pcl::Vertices>& polygons);
278 
282  virtual void
283  performReconstruction (std::vector<pcl::Vertices> &polygons);
284 
292  void
293  performReconstruction (pcl::PolygonMesh &output);
294 
302  inline void
303  addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
304  {
305  assert (idx < static_cast<int> (polygons.size ()));
306  polygons[idx].vertices.resize (3);
307  polygons[idx].vertices[0] = a;
308  polygons[idx].vertices[1] = b;
309  polygons[idx].vertices[2] = c;
310  }
311 
320  inline void
321  addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
322  {
323  assert (idx < static_cast<int> (polygons.size ()));
324  polygons[idx].vertices.resize (4);
325  polygons[idx].vertices[0] = a;
326  polygons[idx].vertices[1] = b;
327  polygons[idx].vertices[2] = c;
328  polygons[idx].vertices[3] = d;
329  }
330 
339  inline void
340  resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
341  int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
342  {
343  float new_value = value;
344  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
345  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
346  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
347  }
348 
353  inline bool
354  isShadowed (const PointInT& point_a, const PointInT& point_b)
355  {
356  bool valid = true;
357 
358  Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap ();
359  Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
360  float distance_to_points = dir_a.norm ();
361  float distance_between_points = dir_b.norm ();
362 
363  if (cos_angle_tolerance_ > 0)
364  {
365  float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
366  if (cos_angle != cos_angle)
367  cos_angle = 1.0f;
368  bool check_angle = fabs (cos_angle) >= cos_angle_tolerance_;
369 
370  bool check_distance = true;
371  if (check_angle && (distance_tolerance_ > 0))
372  {
373  float dist_thresh = distance_tolerance_;
374  if (distance_dependent_)
375  {
376  float d = distance_to_points;
377  if (use_depth_as_distance_)
378  d = std::max(point_a.z, point_b.z);
379  dist_thresh *= d*d;
380  dist_thresh *= dist_thresh; // distance_tolerance_ is already squared if distance_dependent_ is false.
381  }
382  check_distance = (distance_between_points > dist_thresh);
383  }
384  valid = !(check_angle && check_distance);
385  }
386 
387  // check if max. edge length is not exceeded
388  if (max_edge_length_set_)
389  {
390  float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points);
391  float dist_thresh = max_edge_length_a_;
392  if (fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
393  dist_thresh += max_edge_length_b_ * dist;
394  if (fabs(max_edge_length_c_) > std::numeric_limits<float>::min())
395  dist_thresh += max_edge_length_c_ * dist * dist;
396  valid = (distance_between_points <= dist_thresh);
397  }
398 
399  return !valid;
400  }
401 
407  inline bool
408  isValidTriangle (const int& a, const int& b, const int& c)
409  {
410  if (!pcl::isFinite (input_->points[a])) return (false);
411  if (!pcl::isFinite (input_->points[b])) return (false);
412  if (!pcl::isFinite (input_->points[c])) return (false);
413  return (true);
414  }
415 
421  inline bool
422  isShadowedTriangle (const int& a, const int& b, const int& c)
423  {
424  if (isShadowed (input_->points[a], input_->points[b])) return (true);
425  if (isShadowed (input_->points[b], input_->points[c])) return (true);
426  if (isShadowed (input_->points[c], input_->points[a])) return (true);
427  return (false);
428  }
429 
436  inline bool
437  isValidQuad (const int& a, const int& b, const int& c, const int& d)
438  {
439  if (!pcl::isFinite (input_->points[a])) return (false);
440  if (!pcl::isFinite (input_->points[b])) return (false);
441  if (!pcl::isFinite (input_->points[c])) return (false);
442  if (!pcl::isFinite (input_->points[d])) return (false);
443  return (true);
444  }
445 
452  inline bool
453  isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
454  {
455  if (isShadowed (input_->points[a], input_->points[b])) return (true);
456  if (isShadowed (input_->points[b], input_->points[c])) return (true);
457  if (isShadowed (input_->points[c], input_->points[d])) return (true);
458  if (isShadowed (input_->points[d], input_->points[a])) return (true);
459  return (false);
460  }
461 
465  void
466  makeQuadMesh (std::vector<pcl::Vertices>& polygons);
467 
471  void
472  makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
473 
477  void
478  makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
479 
483  void
484  makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
485  };
486 }
487 
489 
490 #endif // PCL_SURFACE_ORGANIZED_FAST_MESH_H_
d
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3108
void addTriangle(int a, int b, int c, int idx, std::vector< pcl::Vertices > &polygons)
Add a new triangle to the current polygon mesh.
void setViewpoint(const Eigen::Vector3f &viewpoint)
Set the viewpoint from where the input point cloud has been acquired.
virtual ~OrganizedFastMesh()
Destructor.
void reconstructPolygons(std::vector< pcl::Vertices > &polygons)
Perform the actual polygonal reconstruction.
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
f
void setTriangulationType(TriangulationType type)
Set the triangulation type (see TriangulationType)
Simple triangulation/surface reconstruction for organized point clouds. Neighboring points (pixels in...
float max_edge_length_b_
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. Standard values are 5de...
int triangle_pixel_size_rows_
size of triangle edges (in pixels) for iterating over rows.
void setTrianglePixelSizeRows(int triangle_size)
Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh...
float cos_angle_tolerance_
(Cosine of the) angle tolerance used when checking whether or not an edge between two points is shado...
bool use_depth_as_distance_
flag whether or not the points&#39; depths are used instead of measured distances (points&#39; distances to t...
const Eigen::Vector3f & getViewpoint() const
Get the viewpoint from where the input point cloud has been acquired.
Eigen::Vector3f viewpoint_
Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data)...
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.
bool store_shadowed_faces_
Whether or not shadowed faces are stored, e.g., for exploration.
bool isShadowedQuad(const int &a, const int &b, const int &c, const int &d)
Check if a triangle is shadowed.
OrganizedFastMesh()
Constructor. Triangulation type defaults to QUAD_MESH.
std::vector< pcl::Vertices > Polygons
void setTrianglePixelSize(int triangle_size)
Set the edge length (in pixels) used for constructing the fixed mesh.
void storeShadowedFaces(bool enable)
Store shadowed faces or not.
pcl::PointCloud< PointInT >::Ptr PointCloudPtr
void useDepthAsDistance(bool enable)
Use the points&#39; depths (z-coordinates) instead of measured distances (points&#39; distances to the viewpo...
bool isShadowedTriangle(const int &a, const int &b, const int &c)
Check if a triangle is shadowed.
void makeRightCutMesh(std::vector< pcl::Vertices > &polygons)
Create a right cut mesh.
bool isValidQuad(const int &a, const int &b, const int &c, const int &d)
Check if a quad is valid.
bool isValidTriangle(const int &a, const int &b, const int &c)
Check if a triangle is valid.
void makeQuadMesh(std::vector< pcl::Vertices > &polygons)
Create a quad mesh.
int triangle_pixel_size_columns_
size of triangle edges (in pixels) for iterating over columns
float max_edge_length_c_
max length of edge, scalar component
void makeLeftCutMesh(std::vector< pcl::Vertices > &polygons)
Create a left cut mesh.
bool distance_dependent_
flag whether or not distance_tolerance_ is distance dependent (multiplied by the squared distance to ...
#define false
Definition: ConvertUTF.c:56
virtual void performReconstruction(std::vector< pcl::Vertices > &polygons)
Create the surface.
bool isShadowed(const PointInT &point_a, const PointInT &point_b)
Check if a point is shadowed by another point.
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.
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
boost::shared_ptr< OrganizedFastMesh< PointInT > > Ptr
boost::shared_ptr< const OrganizedFastMesh< PointInT > > ConstPtr
void makeAdaptiveCutMesh(std::vector< pcl::Vertices > &polygons)
Create an adaptive cut mesh.
float distance_tolerance_
distance tolerance for filtering out shadowed/occluded edges
void setDistanceTolerance(float distance_tolerance, bool depth_dependent=false)
bool max_edge_length_set_
flag whether or not edges are limited in length
bool max_edge_length_dist_dependent_
flag whether or not max edge length is distance dependent.
float max_edge_length_a_
max length of edge, scalar component
void setMaxEdgeLength(float a, float b=0.0f, float c=0.0f)
Set a maximum edge length. Using not only the scalar a, but also b and c, allows for using a distance...
TriangulationType triangulation_type_
Type of meshing scheme (quads vs. triangles, left cut vs. right cut ...


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59