Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
gp3.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) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id: gp3.h 5124 2012-03-16 03:09:41Z rusu $
37  *
38  */
39 
40 #ifndef PCL_GP3_H_
41 #define PCL_GP3_H_
42 
43 // PCL includes
44 #include <pcl/surface/reconstruction.h>
45 
46 #include <pcl/ros/conversions.h>
47 #include <pcl/kdtree/kdtree.h>
48 #include <pcl/kdtree/kdtree_flann.h>
49 #include <pcl/PolygonMesh.h>
50 #include <pcl/TextureMesh.h>
51 #include <boost/function.hpp>
52 
53 #include <fstream>
54 #include <iostream>
55 
56 // add by ktran to export update function
57 #include <pcl/pcl_macros.h>
58 #include <pcl/point_types.h>
59 
60 
61 namespace pcl
62 {
71  inline bool
72  isVisible (const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2,
73  const Eigen::Vector2f &R = Eigen::Vector2f::Zero ())
74  {
75  double a0 = S1[1] - S2[1];
76  double b0 = S2[0] - S1[0];
77  double c0 = S1[0]*S2[1] - S2[0]*S1[1];
78  double a1 = -X[1];
79  double b1 = X[0];
80  double c1 = 0;
81  if (R != Eigen::Vector2f::Zero())
82  {
83  a1 += R[1];
84  b1 -= R[0];
85  c1 = R[0]*X[1] - X[0]*R[1];
86  }
87  double div = a0*b1 - b0*a1;
88  double x = (b0*c1 - b1*c0) / div;
89  double y = (a1*c0 - a0*c1) / div;
90 
91  bool intersection_outside_XR;
92  if (R == Eigen::Vector2f::Zero())
93  {
94  if (X[0] > 0)
95  intersection_outside_XR = (x <= 0) || (x >= X[0]);
96  else if (X[0] < 0)
97  intersection_outside_XR = (x >= 0) || (x <= X[0]);
98  else if (X[1] > 0)
99  intersection_outside_XR = (y <= 0) || (y >= X[1]);
100  else if (X[1] < 0)
101  intersection_outside_XR = (y >= 0) || (y <= X[1]);
102  else
103  intersection_outside_XR = true;
104  }
105  else
106  {
107  if (X[0] > R[0])
108  intersection_outside_XR = (x <= R[0]) || (x >= X[0]);
109  else if (X[0] < R[0])
110  intersection_outside_XR = (x >= R[0]) || (x <= X[0]);
111  else if (X[1] > R[1])
112  intersection_outside_XR = (y <= R[1]) || (y >= X[1]);
113  else if (X[1] < R[1])
114  intersection_outside_XR = (y >= R[1]) || (y <= X[1]);
115  else
116  intersection_outside_XR = true;
117  }
118  if (intersection_outside_XR)
119  return true;
120  else
121  {
122  if (S1[0] > S2[0])
123  return (x <= S2[0]) || (x >= S1[0]);
124  else if (S1[0] < S2[0])
125  return (x >= S2[0]) || (x <= S1[0]);
126  else if (S1[1] > S2[1])
127  return (y <= S2[1]) || (y >= S1[1]);
128  else if (S1[1] < S2[1])
129  return (y >= S2[1]) || (y <= S1[1]);
130  else
131  return false;
132  }
133  }
134 
141  template <typename PointInT>
142  class GreedyProjectionTriangulation : public MeshConstruction<PointInT>
143  {
144  public:
145  using MeshConstruction<PointInT>::tree_;
146  using MeshConstruction<PointInT>::input_;
147  using MeshConstruction<PointInT>::indices_;
148 
149  typedef typename pcl::KdTree<PointInT> KdTree;
150  typedef typename pcl::KdTree<PointInT>::Ptr KdTreePtr;
151 
152  typedef pcl::PointCloud<PointInT> PointCloudIn;
153  typedef typename PointCloudIn::Ptr PointCloudInPtr;
154  typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
155 
156  // FIXME this enum should have a type. Not be anonymous.
157  // Otherplaces where consts are used probably should be fixed.
158  enum
159  {
160  NONE = -1, // not-defined
161  FREE = 0,
162  FRINGE = 1,
163  BOUNDARY = 2,
164  COMPLETED = 3
165  };
166 
168  GreedyProjectionTriangulation () :
169  mu_ (0),
170  search_radius_ (0), // must be set by user
171  nnn_ (100),
172  minimum_angle_ (M_PI/18), // 10 degrees
173  maximum_angle_ (2*M_PI/3), // 120 degrees
174  eps_angle_(M_PI/4), //45 degrees,
175  consistent_(false),
176  consistent_ordering_ (false),
177  triangle_ (),
178  coords_ (),
179  angles_ (),
180  R_ (),
181  state_ (),
182  source_ (),
183  ffn_ (),
184  sfn_ (),
185  part_ (),
186  fringe_queue_ (),
187  is_current_free_ (false),
188  current_index_ (),
189  prev_is_ffn_ (false),
190  prev_is_sfn_ (false),
191  next_is_ffn_ (false),
192  next_is_sfn_ (false),
193  changed_1st_fn_ (false),
194  changed_2nd_fn_ (false),
195  new2boundary_ (),
196  already_connected_ (false),
197  proj_qp_ (),
198  u_ (),
199  v_ (),
200  uvn_ffn_ (),
201  uvn_sfn_ (),
202  uvn_next_ffn_ (),
203  uvn_next_sfn_ (),
204  tmp_ ()
205  {};
206 
211  inline void
212  setMu (double mu) { mu_ = mu; }
213 
215  inline double
216  getMu () { return (mu_); }
217 
221  inline void
222  setMaximumNearestNeighbors (int nnn) { nnn_ = nnn; }
223 
225  inline int
226  getMaximumNearestNeighbors () { return (nnn_); }
227 
232  inline void
233  setSearchRadius (double radius) { search_radius_ = radius; }
234 
236  inline double
237  getSearchRadius () { return (search_radius_); }
238 
243  inline void
244  setMinimumAngle (double minimum_angle) { minimum_angle_ = minimum_angle; }
245 
247  inline double
248  getMinimumAngle () { return (minimum_angle_); }
249 
254  inline void
255  setMaximumAngle (double maximum_angle) { maximum_angle_ = maximum_angle; }
256 
258  inline double
259  getMaximumAngle () { return (maximum_angle_); }
260 
266  inline void
267  setMaximumSurfaceAngle (double eps_angle) { eps_angle_ = eps_angle; }
268 
270  inline double
271  getMaximumSurfaceAngle () { return (eps_angle_); }
272 
276  inline void
277  setNormalConsistency (bool consistent) { consistent_ = consistent; }
278 
280  inline bool
281  getNormalConsistency () { return (consistent_); }
282 
287  inline void
288  setConsistentVertexOrdering (bool consistent_ordering) { consistent_ordering_ = consistent_ordering; }
289 
291  inline bool
292  getConsistentVertexOrdering () { return (consistent_ordering_); }
293 
297  inline std::vector<int>
298  getPointStates () { return (state_); }
299 
303  inline std::vector<int>
304  getPartIDs () { return (part_); }
305 
306 
308  inline std::vector<int>
309  getSFN () { return (sfn_); }
310 
312  inline std::vector<int>
313  getFFN () { return (ffn_); }
314 
315  protected:
317  double mu_;
318 
320  double search_radius_;
321 
323  int nnn_;
324 
326  double minimum_angle_;
327 
329  double maximum_angle_;
330 
332  double eps_angle_;
333 
335  bool consistent_;
336 
338  bool consistent_ordering_;
339 
340  private:
342  struct nnAngle
343  {
344  double angle;
345  int index;
346  int nnIndex;
347  bool visible;
348  };
349 
351  struct doubleEdge
352  {
353  doubleEdge () : index (0), first (), second () {}
354  int index;
355  Eigen::Vector2f first;
356  Eigen::Vector2f second;
357  };
358 
359  // Variables made global to decrease the number of parameters to helper functions
360 
362  pcl::Vertices triangle_;
364  std::vector<Eigen::Vector3f> coords_;
365 
367  std::vector<nnAngle> angles_;
369  int R_;
371  std::vector<int> state_;
373  std::vector<int> source_;
375  std::vector<int> ffn_;
377  std::vector<int> sfn_;
379  std::vector<int> part_;
381  std::vector<int> fringe_queue_;
382 
384  bool is_current_free_;
386  int current_index_;
388  bool prev_is_ffn_;
390  bool prev_is_sfn_;
392  bool next_is_ffn_;
394  bool next_is_sfn_;
396  bool changed_1st_fn_;
398  bool changed_2nd_fn_;
400  int new2boundary_;
401 
405  bool already_connected_;
406 
408  Eigen::Vector3f proj_qp_;
410  Eigen::Vector3f u_;
412  Eigen::Vector3f v_;
414  Eigen::Vector2f uvn_ffn_;
416  Eigen::Vector2f uvn_sfn_;
418  Eigen::Vector2f uvn_next_ffn_;
420  Eigen::Vector2f uvn_next_sfn_;
421 
423  Eigen::Vector3f tmp_;
424 
428  void
429  performReconstruction (pcl::PolygonMesh &output);
430 
434  void
435  performReconstruction (std::vector<pcl::Vertices> &polygons);
436 
440  bool
441  reconstructPolygons (std::vector<pcl::Vertices> &polygons);
442 
444  std::string
445  getClassName () const { return ("GreedyProjectionTriangulation"); }
446 
457  void
458  connectPoint (std::vector<pcl::Vertices> &polygons,
459  const int prev_index,
460  const int next_index,
461  const int next_next_index,
462  const Eigen::Vector2f &uvn_current,
463  const Eigen::Vector2f &uvn_prev,
464  const Eigen::Vector2f &uvn_next);
465 
470  void
471  closeTriangle (std::vector<pcl::Vertices> &polygons);
472 
476  std::vector<std::vector<size_t> >
477  getTriangleList (const pcl::PolygonMesh &input);
478 
485  inline void
486  addTriangle (int a, int b, int c, std::vector<pcl::Vertices> &polygons)
487  {
488  triangle_.vertices.resize (3);
489  if (consistent_ordering_)
490  {
491  const PointInT p = input_->at (indices_->at (a));
492  const Eigen::Vector3f pv = p.getVector3fMap ();
493  if (p.getNormalVector3fMap ().dot (
494  (pv - input_->at (indices_->at (b)).getVector3fMap ()).cross (
495  pv - input_->at (indices_->at (c)).getVector3fMap ()) ) > 0)
496  {
497  triangle_.vertices[0] = a;
498  triangle_.vertices[1] = b;
499  triangle_.vertices[2] = c;
500  }
501  else
502  {
503  triangle_.vertices[0] = a;
504  triangle_.vertices[1] = c;
505  triangle_.vertices[2] = b;
506  }
507  }
508  else
509  {
510  triangle_.vertices[0] = a;
511  triangle_.vertices[1] = b;
512  triangle_.vertices[2] = c;
513  }
514  polygons.push_back (triangle_);
515  }
516 
521  inline void
522  addFringePoint (int v, int s)
523  {
524  source_[v] = s;
525  part_[v] = part_[s];
526  fringe_queue_.push_back(v);
527  }
528 
534  static inline bool
535  nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2)
536  {
537  if (a1.visible == a2.visible)
538  return (a1.angle < a2.angle);
539  else
540  return a1.visible;
541  }
542  };
543 
544 } // namespace pcl
545 
546 #endif //#ifndef PCL_GP3_H_
547 
Generated on Fri Mar 8 2013 12:50:58 for Point Cloud Library (PCL) by   doxygen 1.8.3.1