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_
void reconstructPolygons(std::vector< pcl::Vertices > &polygons)
Perform the actual polygonal reconstruction.
boost::shared_ptr< OrganizedFastMesh< PointInT > > Ptr
void makeAdaptiveCutMesh(std::vector< pcl::Vertices > &polygons)
Create an adaptive cut mesh.
bool isShadowedQuad(const int &a, const int &b, const int &c, const int &d)
Check if a triangle is shadowed.
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.
Simple triangulation/surface reconstruction for organized point clouds.
float max_edge_length_squared_
max (squared) length of edge
bool isShadowed(const PointInT &point_a, const PointInT &point_b)
Check if a point is shadowed by another point.
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
virtual ~OrganizedFastMesh()
Destructor.
void makeLeftCutMesh(std::vector< pcl::Vertices > &polygons)
Create a left cut mesh.
OrganizedFastMesh()
Constructor.
int triangle_pixel_size_rows_
size of triangle edges (in pixels) for iterating over rows.
MeshConstruction represents a base surface reconstruction class.
pcl::uint32_t point_step
TriangulationType triangulation_type_
Type of meshing scheme (quads vs.
std::vector< ::pcl::PCLPointField > fields
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
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 setTrianglePixelSize(int triangle_size)
Set the edge length (in pixels) used for constructing the fixed mesh.
virtual void performReconstruction(std::vector< pcl::Vertices > &polygons)
Create the surface.
void setTrianglePixelSizeColumns(int triangle_size)
Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh...
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
bool isValidQuad(const int &a, const int &b, const int &c, const int &d)
Check if a quad is valid.
void makeRightCutMesh(std::vector< pcl::Vertices > &polygons)
Create a right cut mesh.
boost::shared_ptr< const OrganizedFastMesh< PointInT > > ConstPtr
std::vector< pcl::Vertices > Polygons
pcl::PointCloud< PointInT >::Ptr PointCloudPtr
bool isValidTriangle(const int &a, const int &b, const int &c)
Check if a triangle is valid.
bool isShadowedTriangle(const int &a, const int &b, const int &c)
Check if a triangle is shadowed.
void setMaxEdgeLength(float max_edge_length)
Set a maximum edge length.
bool store_shadowed_faces_
Whether or not shadowed faces are stored, e.g., for exploration.
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:22
void storeShadowedFaces(bool enable)
Store shadowed faces or not.
float cos_angle_tolerance_
(Cosine of the) angle tolerance used when checking whether or not an edge between two points is shado...
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:146
void setTrianglePixelSizeRows(int triangle_size)
Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh...
std::vector< pcl::uint8_t > data
bool check_tree_
A flag specifying whether or not the derived reconstruction algorithm needs the search object tree...
void setTriangulationType(TriangulationType type)
Set the triangulation type (see TriangulationType)
void addTriangle(int a, int b, int c, int idx, std::vector< pcl::Vertices > &polygons)
Add a new triangle to the current polygon mesh.