Point Cloud Library (PCL)  1.7.0
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 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)
91  , store_shadowed_faces_ (false)
92  , cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f))))
93  {
94  check_tree_ = false;
95  };
96 
97  /** \brief Destructor. */
98  virtual ~OrganizedFastMesh () {};
99 
100  /** \brief Set a maximum edge length. TODO: Implement!
101  * \param[in] max_edge_length the maximum edge length
102  */
103  inline void
104  setMaxEdgeLength (float max_edge_length)
105  {
106  max_edge_length_squared_ = max_edge_length * max_edge_length;
107  };
108 
109  /** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
110  * \param[in] triangle_size edge length in pixels
111  * (Default: 1 = neighboring pixels are connected)
112  */
113  inline void
114  setTrianglePixelSize (int triangle_size)
115  {
116  triangle_pixel_size_ = std::max (1, (triangle_size - 1));
117  }
118 
119  /** \brief Set the triangulation type (see \a TriangulationType)
120  * \param[in] type quad mesh, triangle mesh with fixed left, right cut,
121  * or adaptive cut (splits a quad wrt. the depth (z) of the points)
122  */
123  inline void
125  {
126  triangulation_type_ = type;
127  }
128 
129  /** \brief Store shadowed faces or not.
130  * \param[in] enable set to true to store shadowed faces
131  */
132  inline void
133  storeShadowedFaces (bool enable)
134  {
135  store_shadowed_faces_ = enable;
136  }
137 
138  protected:
139  /** \brief max (squared) length of edge */
141 
142  /** \brief size of triangle endges (in pixels) */
144 
145  /** \brief Type of meshin scheme (quads vs. triangles, left cut vs. right cut ... */
147 
148  /** \brief Whether or not shadowed faces are stored, e.g., for exploration */
150 
152 
153  /** \brief Perform the actual polygonal reconstruction.
154  * \param[out] polygons the resultant polygons
155  */
156  void
157  reconstructPolygons (std::vector<pcl::Vertices>& polygons);
158 
159  /** \brief Create the surface.
160  * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
161  */
162  virtual void
163  performReconstruction (std::vector<pcl::Vertices> &polygons);
164 
165  /** \brief Create the surface.
166  *
167  * Simply uses image indices to create an initial polygonal mesh for organized point clouds.
168  * \a indices_ are ignored!
169  *
170  * \param[out] output the resultant polygonal mesh
171  */
172  void
174 
175  /** \brief Add a new triangle to the current polygon mesh
176  * \param[in] a index of the first vertex
177  * \param[in] b index of the second vertex
178  * \param[in] c index of the third vertex
179  * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
180  * \param[out] polygons the polygon mesh to be updated
181  */
182  inline void
183  addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
184  {
185  assert (idx < static_cast<int> (polygons.size ()));
186  polygons[idx].vertices.resize (3);
187  polygons[idx].vertices[0] = a;
188  polygons[idx].vertices[1] = b;
189  polygons[idx].vertices[2] = c;
190  }
191 
192  /** \brief Add a new quad to the current polygon mesh
193  * \param[in] a index of the first vertex
194  * \param[in] b index of the second vertex
195  * \param[in] c index of the third vertex
196  * \param[in] d index of the fourth vertex
197  * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
198  * \param[out] polygons the polygon mesh to be updated
199  */
200  inline void
201  addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
202  {
203  assert (idx < static_cast<int> (polygons.size ()));
204  polygons[idx].vertices.resize (4);
205  polygons[idx].vertices[0] = a;
206  polygons[idx].vertices[1] = b;
207  polygons[idx].vertices[2] = c;
208  polygons[idx].vertices[3] = d;
209  }
210 
211  /** \brief Set (all) coordinates of a particular point to the specified value
212  * \param[in] point_index index of point
213  * \param[out] mesh to modify
214  * \param[in] value value to use when re-setting
215  * \param[in] field_x_idx the X coordinate of the point
216  * \param[in] field_y_idx the Y coordinate of the point
217  * \param[in] field_z_idx the Z coordinate of the point
218  */
219  inline void
220  resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
221  int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
222  {
223  float new_value = value;
224  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
225  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
226  memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
227  }
228 
229  /** \brief Check if a point is shadowed by another point
230  * \param[in] point_a the first point
231  * \param[in] point_b the second point
232  */
233  inline bool
234  isShadowed (const PointInT& point_a, const PointInT& point_b)
235  {
236  Eigen::Vector3f viewpoint = Eigen::Vector3f::Zero (); // TODO: allow for passing viewpoint information
237  Eigen::Vector3f dir_a = viewpoint - point_a.getVector3fMap ();
238  Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
239  float distance_to_points = dir_a.norm ();
240  float distance_between_points = dir_b.norm ();
241  float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
242  if (cos_angle != cos_angle)
243  cos_angle = 1.0f;
244  return (fabs (cos_angle) >= cos_angle_tolerance_);
245  // TODO: check for both: angle almost 0/180 _and_ distance between points larger than noise level
246  }
247 
248  /** \brief Check if a triangle is valid.
249  * \param[in] a index of the first vertex
250  * \param[in] b index of the second vertex
251  * \param[in] c index of the third vertex
252  */
253  inline bool
254  isValidTriangle (const int& a, const int& b, const int& c)
255  {
256  if (!pcl::isFinite (input_->points[a])) return (false);
257  if (!pcl::isFinite (input_->points[b])) return (false);
258  if (!pcl::isFinite (input_->points[c])) return (false);
259  return (true);
260  }
261 
262  /** \brief Check if a triangle is shadowed.
263  * \param[in] a index of the first vertex
264  * \param[in] b index of the second vertex
265  * \param[in] c index of the third vertex
266  */
267  inline bool
268  isShadowedTriangle (const int& a, const int& b, const int& c)
269  {
270  if (isShadowed (input_->points[a], input_->points[b])) return (true);
271  if (isShadowed (input_->points[b], input_->points[c])) return (true);
272  if (isShadowed (input_->points[c], input_->points[a])) return (true);
273  return (false);
274  }
275 
276  /** \brief Check if a quad is valid.
277  * \param[in] a index of the first vertex
278  * \param[in] b index of the second vertex
279  * \param[in] c index of the third vertex
280  * \param[in] d index of the fourth vertex
281  */
282  inline bool
283  isValidQuad (const int& a, const int& b, const int& c, const int& d)
284  {
285  if (!pcl::isFinite (input_->points[a])) return (false);
286  if (!pcl::isFinite (input_->points[b])) return (false);
287  if (!pcl::isFinite (input_->points[c])) return (false);
288  if (!pcl::isFinite (input_->points[d])) return (false);
289  return (true);
290  }
291 
292  /** \brief Check if a triangle is shadowed.
293  * \param[in] a index of the first vertex
294  * \param[in] b index of the second vertex
295  * \param[in] c index of the third vertex
296  * \param[in] d index of the fourth vertex
297  */
298  inline bool
299  isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
300  {
301  if (isShadowed (input_->points[a], input_->points[b])) return (true);
302  if (isShadowed (input_->points[b], input_->points[c])) return (true);
303  if (isShadowed (input_->points[c], input_->points[d])) return (true);
304  if (isShadowed (input_->points[d], input_->points[a])) return (true);
305  return (false);
306  }
307 
308  /** \brief Create a quad mesh.
309  * \param[out] polygons the resultant mesh
310  */
311  void
312  makeQuadMesh (std::vector<pcl::Vertices>& polygons);
313 
314  /** \brief Create a right cut mesh.
315  * \param[out] polygons the resultant mesh
316  */
317  void
318  makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
319 
320  /** \brief Create a left cut mesh.
321  * \param[out] polygons the resultant mesh
322  */
323  void
324  makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
325 
326  /** \brief Create an adaptive cut mesh.
327  * \param[out] polygons the resultant mesh
328  */
329  void
330  makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
331  };
332 }
333 
334 #ifdef PCL_NO_PRECOMPILE
335 #include <pcl/surface/impl/organized_fast_mesh.hpp>
336 #endif
337 
338 #endif // PCL_SURFACE_ORGANIZED_FAST_MESH_H_