Point Cloud Library (PCL)  1.7.1
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 #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 
50  /** \brief Simple triangulation/surface reconstruction for organized point
51  * clouds. Neighboring points (pixels in image space) are connected to
52  * construct a triangular (or quad) mesh.
53  *
54  * \note If you use this code in any academic work, please cite:
55  * D. Holz and S. Behnke.
56  * Fast Range Image Segmentation and Smoothing using Approximate Surface Reconstruction and Region Growing.
57  * In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS),
58  * Jeju Island, Korea, June 26-29 2012.
59  * <a href="http://purl.org/holz/papers/holz_2012_ias.pdf">http://purl.org/holz/papers/holz_2012_ias.pdf</a>
60  *
61  * \author Dirk Holz, Radu B. Rusu
62  * \ingroup surface
63  */
64  template <typename PointInT>
65  class OrganizedFastMesh : public MeshConstruction<PointInT>
66  {
67  public:
68  typedef boost::shared_ptr<OrganizedFastMesh<PointInT> > Ptr;
69  typedef boost::shared_ptr<const OrganizedFastMesh<PointInT> > ConstPtr;
70 
73 
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 
86  /** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */
88  : max_edge_length_squared_ (0.025f)
92  , store_shadowed_faces_ (false)
93  , cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f))))
94  {
95  check_tree_ = false;
96  };
97 
98  /** \brief Destructor. */
99  virtual ~OrganizedFastMesh () {};
100 
101  /** \brief Set a maximum edge length. TODO: Implement!
102  * \param[in] max_edge_length the maximum edge length
103  */
104  inline void
105  setMaxEdgeLength (float max_edge_length)
106  {
107  max_edge_length_squared_ = max_edge_length * max_edge_length;
108  };
109 
110  /** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
111  * \param[in] triangle_size edge length in pixels
112  * (Default: 1 = neighboring pixels are connected)
113  */
114  inline void
115  setTrianglePixelSize (int triangle_size)
116  {
117  setTrianglePixelSizeRows (triangle_size);
118  setTrianglePixelSizeColumns (triangle_size);
119  }
120 
121  /** \brief Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh.
122  * \param[in] triangle_size edge length in pixels
123  * (Default: 1 = neighboring pixels are connected)
124  */
125  inline void
126  setTrianglePixelSizeRows (int triangle_size)
127  {
128  triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1));
129  }
130 
131  /** \brief Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh.
132  * \param[in] triangle_size edge length in pixels
133  * (Default: 1 = neighboring pixels are connected)
134  */
135  inline void
136  setTrianglePixelSizeColumns (int triangle_size)
137  {
138  triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1));
139  }
140 
141  /** \brief Set the triangulation type (see \a TriangulationType)
142  * \param[in] type quad mesh, triangle mesh with fixed left, right cut,
143  * or adaptive cut (splits a quad w.r.t. the depth (z) of the points)
144  */
145  inline void
147  {
148  triangulation_type_ = type;
149  }
150 
151  /** \brief Store shadowed faces or not.
152  * \param[in] enable set to true to store shadowed faces
153  */
154  inline void
155  storeShadowedFaces (bool enable)
156  {
157  store_shadowed_faces_ = enable;
158  }
159 
160  protected:
161  /** \brief max (squared) length of edge */
163 
164  /** \brief size of triangle edges (in pixels) for iterating over rows. */
166 
167  /** \brief size of triangle edges (in pixels) for iterating over columns*/
169 
170  /** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */
172 
173  /** \brief Whether or not shadowed faces are stored, e.g., for exploration */
175 
176  /** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */
178 
179  /** \brief Perform the actual polygonal reconstruction.
180  * \param[out] polygons the resultant polygons
181  */
182  void
183  reconstructPolygons (std::vector<pcl::Vertices>& polygons);
184 
185  /** \brief Create the surface.
186  * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
187  */
188  virtual void
189  performReconstruction (std::vector<pcl::Vertices> &polygons);
190 
191  /** \brief Create the surface.
192  *
193  * Simply uses image indices to create an initial polygonal mesh for organized point clouds.
194  * \a indices_ are ignored!
195  *
196  * \param[out] output the resultant polygonal mesh
197  */
198  void
200 
201  /** \brief Add a new triangle to the current polygon mesh
202  * \param[in] a index of the first vertex
203  * \param[in] b index of the second vertex
204  * \param[in] c index of the third vertex
205  * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
206  * \param[out] polygons the polygon mesh to be updated
207  */
208  inline void
209  addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
210  {
211  assert (idx < static_cast<int> (polygons.size ()));
212  polygons[idx].vertices.resize (3);
213  polygons[idx].vertices[0] = a;
214  polygons[idx].vertices[1] = b;
215  polygons[idx].vertices[2] = c;
216  }
217 
218  /** \brief Add a new quad to the current polygon mesh
219  * \param[in] a index of the first vertex
220  * \param[in] b index of the second vertex
221  * \param[in] c index of the third vertex
222  * \param[in] d index of the fourth vertex
223  * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
224  * \param[out] polygons the polygon mesh to be updated
225  */
226  inline void
227  addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
228  {
229  assert (idx < static_cast<int> (polygons.size ()));
230  polygons[idx].vertices.resize (4);
231  polygons[idx].vertices[0] = a;
232  polygons[idx].vertices[1] = b;
233  polygons[idx].vertices[2] = c;
234  polygons[idx].vertices[3] = d;
235  }
236 
237  /** \brief Set (all) coordinates of a particular point to the specified value
238  * \param[in] point_index index of point
239  * \param[out] mesh to modify
240  * \param[in] value value to use when re-setting
241  * \param[in] field_x_idx the X coordinate of the point
242  * \param[in] field_y_idx the Y coordinate of the point
243  * \param[in] field_z_idx the Z coordinate of the point
244  */
245  inline void
246  resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
247  int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
248  {
249  float new_value = value;
250  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
251  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
252  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
253  }
254 
255  /** \brief Check if a point is shadowed by another point
256  * \param[in] point_a the first point
257  * \param[in] point_b the second point
258  */
259  inline bool
260  isShadowed (const PointInT& point_a, const PointInT& point_b)
261  {
262  Eigen::Vector3f viewpoint = Eigen::Vector3f::Zero (); // TODO: allow for passing viewpoint information
263  Eigen::Vector3f dir_a = viewpoint - point_a.getVector3fMap ();
264  Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
265  float distance_to_points = dir_a.norm ();
266  float distance_between_points = dir_b.norm ();
267  float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
268  if (cos_angle != cos_angle)
269  cos_angle = 1.0f;
270  return (fabs (cos_angle) >= cos_angle_tolerance_);
271  // TODO: check for both: angle almost 0/180 _and_ distance between points larger than noise level
272  }
273 
274  /** \brief Check if a triangle is valid.
275  * \param[in] a index of the first vertex
276  * \param[in] b index of the second vertex
277  * \param[in] c index of the third vertex
278  */
279  inline bool
280  isValidTriangle (const int& a, const int& b, const int& c)
281  {
282  if (!pcl::isFinite (input_->points[a])) return (false);
283  if (!pcl::isFinite (input_->points[b])) return (false);
284  if (!pcl::isFinite (input_->points[c])) return (false);
285  return (true);
286  }
287 
288  /** \brief Check if a triangle is shadowed.
289  * \param[in] a index of the first vertex
290  * \param[in] b index of the second vertex
291  * \param[in] c index of the third vertex
292  */
293  inline bool
294  isShadowedTriangle (const int& a, const int& b, const int& c)
295  {
296  if (isShadowed (input_->points[a], input_->points[b])) return (true);
297  if (isShadowed (input_->points[b], input_->points[c])) return (true);
298  if (isShadowed (input_->points[c], input_->points[a])) return (true);
299  return (false);
300  }
301 
302  /** \brief Check if a quad is valid.
303  * \param[in] a index of the first vertex
304  * \param[in] b index of the second vertex
305  * \param[in] c index of the third vertex
306  * \param[in] d index of the fourth vertex
307  */
308  inline bool
309  isValidQuad (const int& a, const int& b, const int& c, const int& d)
310  {
311  if (!pcl::isFinite (input_->points[a])) return (false);
312  if (!pcl::isFinite (input_->points[b])) return (false);
313  if (!pcl::isFinite (input_->points[c])) return (false);
314  if (!pcl::isFinite (input_->points[d])) return (false);
315  return (true);
316  }
317 
318  /** \brief Check if a triangle is shadowed.
319  * \param[in] a index of the first vertex
320  * \param[in] b index of the second vertex
321  * \param[in] c index of the third vertex
322  * \param[in] d index of the fourth vertex
323  */
324  inline bool
325  isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
326  {
327  if (isShadowed (input_->points[a], input_->points[b])) return (true);
328  if (isShadowed (input_->points[b], input_->points[c])) return (true);
329  if (isShadowed (input_->points[c], input_->points[d])) return (true);
330  if (isShadowed (input_->points[d], input_->points[a])) return (true);
331  return (false);
332  }
333 
334  /** \brief Create a quad mesh.
335  * \param[out] polygons the resultant mesh
336  */
337  void
338  makeQuadMesh (std::vector<pcl::Vertices>& polygons);
339 
340  /** \brief Create a right cut mesh.
341  * \param[out] polygons the resultant mesh
342  */
343  void
344  makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
345 
346  /** \brief Create a left cut mesh.
347  * \param[out] polygons the resultant mesh
348  */
349  void
350  makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
351 
352  /** \brief Create an adaptive cut mesh.
353  * \param[out] polygons the resultant mesh
354  */
355  void
356  makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
357  };
358 }
359 
360 #ifdef PCL_NO_PRECOMPILE
361 #include <pcl/surface/impl/organized_fast_mesh.hpp>
362 #endif
363 
364 #endif // PCL_SURFACE_ORGANIZED_FAST_MESH_H_
pcl::PointCloud< PointInT >::Ptr PointCloudPtr
boost::shared_ptr< const OrganizedFastMesh< PointInT > > ConstPtr
void setTrianglePixelSizeColumns(int triangle_size)
Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh...
void storeShadowedFaces(bool enable)
Store shadowed faces or not.
virtual ~OrganizedFastMesh()
Destructor.
bool isValidQuad(const int &a, const int &b, const int &c, const int &d)
Check if a quad is valid.
bool isShadowed(const PointInT &point_a, const PointInT &point_b)
Check if a point is shadowed by another point.
void makeLeftCutMesh(std::vector< pcl::Vertices > &polygons)
Create a left cut mesh.
void makeQuadMesh(std::vector< pcl::Vertices > &polygons)
Create a quad mesh.
float cos_angle_tolerance_
(Cosine of the) angle tolerance used when checking whether or not an edge between two points is shado...
std::vector< pcl::uint8_t > data
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:146
std::vector< ::pcl::PCLPointField > fields
void setMaxEdgeLength(float max_edge_length)
Set a maximum edge length.
boost::shared_ptr< OrganizedFastMesh< PointInT > > Ptr
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
Definition: point_tests.h:53
void reconstructPolygons(std::vector< pcl::Vertices > &polygons)
Perform the actual polygonal reconstruction.
void makeAdaptiveCutMesh(std::vector< pcl::Vertices > &polygons)
Create an adaptive cut mesh.
pcl::uint32_t point_step
bool store_shadowed_faces_
Whether or not shadowed faces are stored, e.g., for exploration.
OrganizedFastMesh()
Constructor.
float max_edge_length_squared_
max (squared) length of edge
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.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
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 setTrianglePixelSizeRows(int triangle_size)
Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh...
MeshConstruction represents a base surface reconstruction class.
void makeRightCutMesh(std::vector< pcl::Vertices > &polygons)
Create a right cut mesh.
void addTriangle(int a, int b, int c, int idx, std::vector< pcl::Vertices > &polygons)
Add a new triangle to the current polygon mesh.
Simple triangulation/surface reconstruction for organized point clouds.
bool isValidTriangle(const int &a, const int &b, const int &c)
Check if a triangle is valid.
int triangle_pixel_size_columns_
size of triangle edges (in pixels) for iterating over columns
virtual void performReconstruction(std::vector< pcl::Vertices > &polygons)
Create the surface.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void setTrianglePixelSize(int triangle_size)
Set the edge length (in pixels) used for constructing the fixed mesh.
void setTriangulationType(TriangulationType type)
Set the triangulation type (see TriangulationType)
bool isShadowedQuad(const int &a, const int &b, const int &c, const int &d)
Check if a triangle is shadowed.
bool isShadowedTriangle(const int &a, const int &b, const int &c)
Check if a triangle is shadowed.
int triangle_pixel_size_rows_
size of triangle edges (in pixels) for iterating over rows.
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:22
std::vector< pcl::Vertices > Polygons
bool check_tree_
A flag specifying whether or not the derived reconstruction algorithm needs the search object tree...
TriangulationType triangulation_type_
Type of meshing scheme (quads vs.