Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
grid_projection.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: grid_projection.h 5036 2012-03-12 08:54:15Z rusu $
35  *
36  */
37 
38 #ifndef PCL_SURFACE_GRID_PROJECTION_H_
39 #define PCL_SURFACE_GRID_PROJECTION_H_
40 
41 #include <pcl/surface/reconstruction.h>
42 #include <boost/dynamic_bitset/dynamic_bitset.hpp>
43 #include <boost/unordered_map.hpp>
44 
45 namespace pcl
46 {
48  const int I_SHIFT_EP[12][2] = {
49  {0, 4}, {1, 5}, {2, 6}, {3, 7},
50  {0, 1}, {1, 2}, {2, 3}, {3, 0},
51  {4, 5}, {5, 6}, {6, 7}, {7, 4}
52  };
53 
54  const int I_SHIFT_PT[4] = {
55  0, 4, 5, 7
56  };
57 
58  const int I_SHIFT_EDGE[3][2] = {
59  {0,1}, {1,3}, {1,2}
60  };
61 
62 
72  template <typename PointNT>
73  class GridProjection : public SurfaceReconstruction<PointNT>
74  {
75  public:
76  using SurfaceReconstruction<PointNT>::input_;
77  using SurfaceReconstruction<PointNT>::tree_;
78 
79  typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudPtr;
80 
81  typedef typename pcl::KdTree<PointNT> KdTree;
82  typedef typename pcl::KdTree<PointNT>::Ptr KdTreePtr;
83 
85  struct Leaf
86  {
87  Leaf () : data_indices (), pt_on_surface (), vect_at_grid_pt () {}
88 
89  std::vector<int> data_indices;
90  Eigen::Vector4f pt_on_surface;
91  Eigen::Vector3f vect_at_grid_pt;
92  };
93 
94  typedef boost::unordered_map<int, Leaf, boost::hash<int>, std::equal_to<int>, Eigen::aligned_allocator<int> > HashMap;
95 
97  GridProjection ();
98 
102  GridProjection (double in_resolution);
103 
105  ~GridProjection ();
106 
110  inline void
111  setResolution (double resolution)
112  {
113  leaf_size_ = resolution;
114  }
115 
116  inline double
117  getResolution () const
118  {
119  return (leaf_size_);
120  }
121 
131  inline void
132  setPaddingSize (int padding_size)
133  {
134  padding_size_ = padding_size;
135  }
136  inline int
137  getPaddingSize () const
138  {
139  return (padding_size_);
140  }
141 
146  inline void
147  setNearestNeighborNum (int k)
148  {
149  k_ = k;
150  }
151  inline int
152  getNearestNeighborNum () const
153  {
154  return (k_);
155  }
156 
161  inline void
162  setMaxBinarySearchLevel (int max_binary_search_level)
163  {
164  max_binary_search_level_ = max_binary_search_level;
165  }
166  inline int
167  getMaxBinarySearchLevel () const
168  {
169  return (max_binary_search_level_);
170  }
171 
173  inline const HashMap&
174  getCellHashMap () const
175  {
176  return (cell_hash_map_);
177  }
178 
179  inline const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >&
180  getVectorAtDataPoint () const
181  {
182  return (vector_at_data_point_);
183  }
184 
185  inline const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&
186  getSurface () const
187  {
188  return (surface_);
189  }
190 
191  protected:
195  void
196  getBoundingBox ();
197 
201  bool
202  reconstructPolygons (std::vector<pcl::Vertices> &polygons);
203 
213  void
214  performReconstruction (pcl::PolygonMesh &output);
215 
226  void
227  performReconstruction (pcl::PointCloud<PointNT> &points,
228  std::vector<pcl::Vertices> &polygons);
229 
235  void
236  scaleInputDataPoint (double scale_factor);
237 
243  inline void
244  getCellIndex (const Eigen::Vector4f &p, Eigen::Vector3i& index) const
245  {
246  for (int i = 0; i < 3; ++i)
247  index[i] = static_cast<int> ((p[i] - min_p_(i)) / leaf_size_);
248  }
249 
255  inline void
256  getCellCenterFromIndex (const Eigen::Vector3i &index, Eigen::Vector4f &center) const
257  {
258  for (int i = 0; i < 3; ++i)
259  center[i] =
260  min_p_[i] + static_cast<float> (index[i]) *
261  static_cast<float> (leaf_size_) +
262  static_cast<float> (leaf_size_) / 2.0f;
263  }
264 
269  void
270  getVertexFromCellCenter (const Eigen::Vector4f &cell_center,
271  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const;
272 
277  inline int
278  getIndexIn1D (const Eigen::Vector3i &index) const
279  {
280  //assert(data_size_ > 0);
281  return (index[0] * data_size_ * data_size_ +
282  index[1] * data_size_ + index[2]);
283  }
284 
290  inline void
291  getIndexIn3D (int index_1d, Eigen::Vector3i& index_3d) const
292  {
293  //assert(data_size_ > 0);
294  index_3d[0] = index_1d / (data_size_ * data_size_);
295  index_1d -= index_3d[0] * data_size_ * data_size_;
296  index_3d[1] = index_1d / data_size_;
297  index_1d -= index_3d[1] * data_size_;
298  index_3d[2] = index_1d;
299  }
300 
305  void
306  fillPad (const Eigen::Vector3i &index);
307 
312  void
313  getDataPtsUnion (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices);
314 
321  void
322  createSurfaceForCell (const Eigen::Vector3i &index, std::vector <int> &pt_union_indices);
323 
324 
332  void
333  getProjection (const Eigen::Vector4f &p, std::vector<int> &pt_union_indices, Eigen::Vector4f &projection);
334 
342  void
343  getProjectionWithPlaneFit (const Eigen::Vector4f &p,
344  std::vector<int> &pt_union_indices,
345  Eigen::Vector4f &projection);
346 
347 
353  void
354  getVectorAtPoint (const Eigen::Vector4f &p,
355  std::vector <int> &pt_union_indices, Eigen::Vector3f &vo);
356 
364  void
365  getVectorAtPointKNN (const Eigen::Vector4f &p,
366  std::vector<int> &k_indices,
367  std::vector<float> &k_squared_distances,
368  Eigen::Vector3f &vo);
369 
374  double
375  getMagAtPoint (const Eigen::Vector4f &p, const std::vector <int> &pt_union_indices);
376 
382  double
383  getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
384  const std::vector <int> &pt_union_indices);
385 
391  double
392  getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
393  const std::vector <int> &pt_union_indices);
394 
403  bool
404  isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
405  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
406  std::vector <int> &pt_union_indices);
407 
416  void
417  findIntersection (int level,
418  const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
419  const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
420  const Eigen::Vector4f &start_pt,
421  std::vector<int> &pt_union_indices,
422  Eigen::Vector4f &intersection);
423 
438  void
439  storeVectAndSurfacePoint (int index_1d, const Eigen::Vector3i &index_3d,
440  std::vector<int> &pt_union_indices, const Leaf &cell_data);
441 
455  void
456  storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &index_3d, const Leaf &cell_data);
457 
458  private:
460  HashMap cell_hash_map_;
461 
463  Eigen::Vector4f min_p_, max_p_;
464 
466  double leaf_size_;
467 
469  double gaussian_scale_;
470 
472  int data_size_;
473 
475  int max_binary_search_level_;
476 
478  int k_;
479 
481  int padding_size_;
482 
484  PointCloudPtr data_;
485 
487  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vector_at_data_point_;
488 
490  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > surface_;
491 
493  boost::dynamic_bitset<> occupied_cell_list_;
494 
496  std::string getClassName () const { return ("GridProjection"); }
497 
498  public:
499  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
500  };
501 }
502 
503 #endif // PCL_SURFACE_GRID_PROJECTION_H_
504 
Generated on Sat Aug 24 2013 17:28:41 for Point Cloud Library (PCL) by   doxygen 1.8.4