Point Cloud Library (PCL)  1.7.0
grid_projection.hpp
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$
35  *
36  */
37 
38 #ifndef PCL_SURFACE_IMPL_GRID_PROJECTION_H_
39 #define PCL_SURFACE_IMPL_GRID_PROJECTION_H_
40 
41 #include <pcl/surface/grid_projection.h>
42 #include <pcl/common/common.h>
43 #include <pcl/common/centroid.h>
44 #include <pcl/common/vector_average.h>
45 #include <pcl/Vertices.h>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointNT>
50  cell_hash_map_ (), min_p_ (), max_p_ (), leaf_size_ (0.001), gaussian_scale_ (),
51  data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ (),
52  vector_at_data_point_ (), surface_ (), occupied_cell_list_ ()
53 {}
54 
55 //////////////////////////////////////////////////////////////////////////////////////////////
56 template <typename PointNT>
58  cell_hash_map_ (), min_p_ (), max_p_ (), leaf_size_ (resolution), gaussian_scale_ (),
59  data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ (),
60  vector_at_data_point_ (), surface_ (), occupied_cell_list_ ()
61 {}
62 
63 //////////////////////////////////////////////////////////////////////////////////////////////
64 template <typename PointNT>
66 {
67  vector_at_data_point_.clear ();
68  surface_.clear ();
69  cell_hash_map_.clear ();
70  occupied_cell_list_.clear ();
71  data_.reset ();
72 }
73 
74 //////////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointNT> void
77 {
78  for (size_t i = 0; i < data_->points.size(); ++i)
79  {
80  data_->points[i].x /= static_cast<float> (scale_factor);
81  data_->points[i].y /= static_cast<float> (scale_factor);
82  data_->points[i].z /= static_cast<float> (scale_factor);
83  }
84  max_p_ /= static_cast<float> (scale_factor);
85  min_p_ /= static_cast<float> (scale_factor);
86 }
87 
88 //////////////////////////////////////////////////////////////////////////////////////////////
89 template <typename PointNT> void
91 {
92  pcl::getMinMax3D (*data_, min_p_, max_p_);
93 
94  Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
95  double scale_factor = (std::max)((std::max)(bounding_box_size.x (),
96  bounding_box_size.y ()),
97  bounding_box_size.z ());
98  if (scale_factor > 1)
99  scaleInputDataPoint (scale_factor);
100 
101  // Round the max_p_, min_p_ so that they are aligned with the cells vertices
102  int upper_right_index[3];
103  int lower_left_index[3];
104  for (size_t i = 0; i < 3; ++i)
105  {
106  upper_right_index[i] = static_cast<int> (max_p_(i) / leaf_size_ + 5);
107  lower_left_index[i] = static_cast<int> (min_p_(i) / leaf_size_ - 5);
108  max_p_(i) = static_cast<float> (upper_right_index[i] * leaf_size_);
109  min_p_(i) = static_cast<float> (lower_left_index[i] * leaf_size_);
110  }
111  bounding_box_size = max_p_ - min_p_;
112  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
113  bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
114  double max_size =
115  (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
116  bounding_box_size.z ());
117 
118  data_size_ = static_cast<int> (max_size / leaf_size_);
119  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n",
120  min_p_.x (), min_p_.y (), min_p_.z ());
121  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n",
122  max_p_.x (), max_p_.y (), max_p_.z ());
123  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_);
124  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_);
125  occupied_cell_list_.resize (data_size_ * data_size_ * data_size_);
126  gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
127 }
128 
129 //////////////////////////////////////////////////////////////////////////////////////////////
130 template <typename PointNT> void
132  const Eigen::Vector4f &cell_center,
133  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const
134 {
135  assert (pts.size () == 8);
136 
137  float sz = static_cast<float> (leaf_size_) / 2.0f;
138 
139  pts[0] = cell_center + Eigen::Vector4f (-sz, sz, -sz, 0);
140  pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0);
141  pts[2] = cell_center + Eigen::Vector4f (sz, -sz, -sz, 0);
142  pts[3] = cell_center + Eigen::Vector4f (sz, sz, -sz, 0);
143  pts[4] = cell_center + Eigen::Vector4f (-sz, sz, sz, 0);
144  pts[5] = cell_center + Eigen::Vector4f (-sz, -sz, sz, 0);
145  pts[6] = cell_center + Eigen::Vector4f (sz, -sz, sz, 0);
146  pts[7] = cell_center + Eigen::Vector4f (sz, sz, sz, 0);
147 }
148 
149 //////////////////////////////////////////////////////////////////////////////////////////////
150 template <typename PointNT> void
151 pcl::GridProjection<PointNT>::getDataPtsUnion (const Eigen::Vector3i &index,
152  std::vector <int> &pt_union_indices)
153 {
154  for (int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
155  {
156  for (int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
157  {
158  for (int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
159  {
160  Eigen::Vector3i cell_index_3d (i, j, k);
161  int cell_index_1d = getIndexIn1D (cell_index_3d);
162  if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
163  {
164  // Get the indices of the input points within the cell(i,j,k), which
165  // is stored in the hash map
166  pt_union_indices.insert (pt_union_indices.end (),
167  cell_hash_map_.at (cell_index_1d).data_indices.begin (),
168  cell_hash_map_.at (cell_index_1d).data_indices.end ());
169  }
170  }
171  }
172  }
173 }
174 
175 //////////////////////////////////////////////////////////////////////////////////////////////
176 template <typename PointNT> void
178  std::vector <int> &pt_union_indices)
179 {
180  // 8 vertices of the cell
181  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
182 
183  // 4 end points that shared by 3 edges connecting the upper left front points
184  Eigen::Vector4f pts[4];
185  Eigen::Vector3f vector_at_pts[4];
186 
187  // Given the index of cell, caluate the coordinates of the eight vertices of the cell
188  // index the index of the cell in (x,y,z) 3d format
189  Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
190  getCellCenterFromIndex (index, cell_center);
191  getVertexFromCellCenter (cell_center, vertices);
192 
193  // Get the indices of the cells which stores the 4 end points.
194  Eigen::Vector3i indices[4];
195  indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
196  indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
197  indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
198  indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
199 
200  // Get the coordinate of the 4 end points, and the corresponding vectors
201  for (size_t i = 0; i < 4; ++i)
202  {
203  pts[i] = vertices[I_SHIFT_PT[i]];
204  unsigned int index_1d = getIndexIn1D (indices[i]);
205  if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () ||
206  occupied_cell_list_[index_1d] == 0)
207  return;
208  else
209  vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
210  }
211 
212  // Go through the 3 edges, test whether they are intersected by the surface
213  for (size_t i = 0; i < 3; ++i)
214  {
215  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
216  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
217  for (size_t j = 0; j < 2; ++j)
218  {
219  end_pts[j] = pts[I_SHIFT_EDGE[i][j]];
220  vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]];
221  }
222 
223  if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
224  {
225  // Indices of cells that contains points which will be connected to
226  // create a polygon
227  Eigen::Vector3i polygon[4];
228  Eigen::Vector4f polygon_pts[4];
229  int polygon_indices_1d[4];
230  bool is_all_in_hash_map = true;
231  switch (i)
232  {
233  case 0:
234  polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
235  polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
236  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
237  polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
238  break;
239  case 1:
240  polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
241  polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
242  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
243  polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
244  break;
245  case 2:
246  polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
247  polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
248  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
249  polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
250  break;
251  default:
252  break;
253  }
254  for (size_t k = 0; k < 4; k++)
255  {
256  polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
257  if (!occupied_cell_list_[polygon_indices_1d[k]])
258  {
259  is_all_in_hash_map = false;
260  break;
261  }
262  }
263  if (is_all_in_hash_map)
264  {
265  for (size_t k = 0; k < 4; k++)
266  {
267  polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
268  surface_.push_back (polygon_pts[k]);
269  }
270  }
271  }
272  }
273 }
274 
275 //////////////////////////////////////////////////////////////////////////////////////////////
276 template <typename PointNT> void
278  std::vector <int> &pt_union_indices, Eigen::Vector4f &projection)
279 {
280  const double projection_distance = leaf_size_ * 3;
281  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
282  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
283  end_pt[0] = p;
284  getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
285  end_pt_vect[0].normalize();
286 
287  double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
288 
289  // Find another point which is projection_distance away from the p, do a
290  // binary search between these two points, to find the projected point on the
291  // surface
292  if (dSecond > 0)
293  end_pt[1] = end_pt[0] + Eigen::Vector4f (
294  end_pt_vect[0][0] * static_cast<float> (projection_distance),
295  end_pt_vect[0][1] * static_cast<float> (projection_distance),
296  end_pt_vect[0][2] * static_cast<float> (projection_distance),
297  0.0f);
298  else
299  end_pt[1] = end_pt[0] - Eigen::Vector4f (
300  end_pt_vect[0][0] * static_cast<float> (projection_distance),
301  end_pt_vect[0][1] * static_cast<float> (projection_distance),
302  end_pt_vect[0][2] * static_cast<float> (projection_distance),
303  0.0f);
304  getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
305  if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
306  {
307  Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
308  findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
309  }
310  else
311  projection = p;
312 }
313 
314 //////////////////////////////////////////////////////////////////////////////////////////////
315 template <typename PointNT> void
317  std::vector<int> (&pt_union_indices),
318  Eigen::Vector4f &projection)
319 {
320  // Compute the plane coefficients
321  Eigen::Vector4f model_coefficients;
322  /// @remark iterative weighted least squares or sac might give better results
323  Eigen::Matrix3f covariance_matrix;
324  Eigen::Vector4f xyz_centroid;
325 
326  computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid);
327 
328  // Get the plane normal
329  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
330  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
331  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
332 
333  // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
334  model_coefficients[0] = eigen_vector [0];
335  model_coefficients[1] = eigen_vector [1];
336  model_coefficients[2] = eigen_vector [2];
337  model_coefficients[3] = 0;
338  // Hessian form (D = nc . p_plane (centroid here) + p)
339  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
340 
341  // Projected point
342  Eigen::Vector3f point (p.x (), p.y (), p.z ()); //= Eigen::Vector3f::MapAligned (&output.points[cp].x, 3);
343  float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
344  point -= distance * model_coefficients.head < 3 > ();
345 
346  projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
347 }
348 
349 //////////////////////////////////////////////////////////////////////////////////////////////
350 template <typename PointNT> void
352  std::vector <int> &pt_union_indices,
353  Eigen::Vector3f &vo)
354 {
355  std::vector <double> pt_union_dist (pt_union_indices.size ());
356  std::vector <double> pt_union_weight (pt_union_indices.size ());
357  Eigen::Vector3f out_vector (0, 0, 0);
358  double sum = 0.0;
359  double mag = 0.0;
360 
361  for (size_t i = 0; i < pt_union_indices.size (); ++i)
362  {
363  Eigen::Vector4f pp (data_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[pt_union_indices[i]].z, 0);
364  pt_union_dist[i] = (pp - p).squaredNorm ();
365  pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
366  mag += pow (M_E, -pow (sqrt (pt_union_dist[i]), 2.0) / gaussian_scale_);
367  sum += pt_union_weight[i];
368  }
369 
370  pcl::VectorAverage3f vector_average;
371 
372  Eigen::Vector3f v (
373  data_->points[pt_union_indices[0]].normal[0],
374  data_->points[pt_union_indices[0]].normal[1],
375  data_->points[pt_union_indices[0]].normal[2]);
376 
377  for (size_t i = 0; i < pt_union_weight.size (); ++i)
378  {
379  pt_union_weight[i] /= sum;
380  Eigen::Vector3f vec (data_->points[pt_union_indices[i]].normal[0],
381  data_->points[pt_union_indices[i]].normal[1],
382  data_->points[pt_union_indices[i]].normal[2]);
383  if (vec.dot (v) < 0)
384  vec = -vec;
385  vector_average.add (vec, static_cast<float> (pt_union_weight[i]));
386  }
387  out_vector = vector_average.getMean ();
388  // vector_average.getEigenVector1(out_vector);
389 
390  out_vector.normalize ();
391  double d1 = getD1AtPoint (p, out_vector, pt_union_indices);
392  out_vector *= static_cast<float> (sum);
393  vo = ((d1 > 0) ? -1 : 1) * out_vector;
394 }
395 
396 //////////////////////////////////////////////////////////////////////////////////////////////
397 template <typename PointNT> void
399  std::vector <int> &k_indices,
400  std::vector <float> &k_squared_distances,
401  Eigen::Vector3f &vo)
402 {
403  Eigen::Vector3f out_vector (0, 0, 0);
404  std::vector <float> k_weight;
405  k_weight.resize (k_);
406  float sum = 0.0;
407  for (int i = 0; i < k_; i++)
408  {
409  //k_weight[i] = pow (M_E, -pow (k_squared_distances[i], 2.0) / gaussian_scale_);
410  k_weight[i] = std::pow (static_cast<float>(M_E), static_cast<float>(-pow (static_cast<float>(k_squared_distances[i]), 2.0f) / gaussian_scale_));
411  sum += k_weight[i];
412  }
413  pcl::VectorAverage3f vector_average;
414  for (int i = 0; i < k_; i++)
415  {
416  k_weight[i] /= sum;
417  Eigen::Vector3f vec (data_->points[k_indices[i]].normal[0],
418  data_->points[k_indices[i]].normal[1],
419  data_->points[k_indices[i]].normal[2]);
420  vector_average.add (vec, k_weight[i]);
421  }
422  vector_average.getEigenVector1 (out_vector);
423  out_vector.normalize ();
424  double d1 = getD1AtPoint (p, out_vector, k_indices);
425  out_vector = out_vector * sum;
426  vo = ((d1 > 0) ? -1 : 1) * out_vector;
427 
428 }
429 
430 //////////////////////////////////////////////////////////////////////////////////////////////
431 template <typename PointNT> double
433  const std::vector <int> &pt_union_indices)
434 {
435  std::vector <double> pt_union_dist (pt_union_indices.size ());
436  std::vector <double> pt_union_weight (pt_union_indices.size ());
437  double sum = 0.0;
438  for (size_t i = 0; i < pt_union_indices.size (); ++i)
439  {
440  Eigen::Vector4f pp (data_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[pt_union_indices[i]].z, 0);
441  pt_union_dist[i] = (pp - p).norm ();
442  sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
443  }
444  return (sum);
445 }
446 
447 //////////////////////////////////////////////////////////////////////////////////////////////
448 template <typename PointNT> double
449 pcl::GridProjection<PointNT>::getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
450  const std::vector <int> &pt_union_indices)
451 {
452  double sz = 0.01 * leaf_size_;
453  Eigen::Vector3f v = vec * static_cast<float> (sz);
454 
455  double forward = getMagAtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
456  double backward = getMagAtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
457  return ((forward - backward) / (0.02 * leaf_size_));
458 }
459 
460 //////////////////////////////////////////////////////////////////////////////////////////////
461 template <typename PointNT> double
462 pcl::GridProjection<PointNT>::getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
463  const std::vector <int> &pt_union_indices)
464 {
465  double sz = 0.01 * leaf_size_;
466  Eigen::Vector3f v = vec * static_cast<float> (sz);
467 
468  double forward = getD1AtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
469  double backward = getD1AtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
470  return ((forward - backward) / (0.02 * leaf_size_));
471 }
472 
473 //////////////////////////////////////////////////////////////////////////////////////////////
474 template <typename PointNT> bool
475 pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
476  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
477  std::vector <int> &pt_union_indices)
478 {
479  assert (end_pts.size () == 2);
480  assert (vect_at_end_pts.size () == 2);
481 
482  double length[2];
483  for (size_t i = 0; i < 2; ++i)
484  {
485  length[i] = vect_at_end_pts[i].norm ();
486  vect_at_end_pts[i].normalize ();
487  }
488  double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
489  if (dot_prod < 0)
490  {
491  double ratio = length[0] / (length[0] + length[1]);
492  Eigen::Vector4f start_pt =
493  end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
494  Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
495  findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
496 
497  Eigen::Vector3f vec;
498  getVectorAtPoint (intersection_pt, pt_union_indices, vec);
499  vec.normalize ();
500 
501  double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
502  if (d2 < 0)
503  return (true);
504  }
505  return (false);
506 }
507 
508 //////////////////////////////////////////////////////////////////////////////////////////////
509 template <typename PointNT> void
511  const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
512  const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
513  const Eigen::Vector4f &start_pt,
514  std::vector <int> &pt_union_indices,
515  Eigen::Vector4f &intersection)
516 {
517  assert (end_pts.size () == 2);
518  assert (vect_at_end_pts.size () == 2);
519 
520  Eigen::Vector3f vec;
521  getVectorAtPoint (start_pt, pt_union_indices, vec);
522  double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
523  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
524  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
525  if ((fabs (d1) < 10e-3) || (level == max_binary_search_level_))
526  {
527  intersection = start_pt;
528  return;
529  }
530  else
531  {
532  vec.normalize ();
533  if (vec.dot (vect_at_end_pts[0]) < 0)
534  {
535  Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
536  new_end_pts[0] = end_pts[0];
537  new_end_pts[1] = start_pt;
538  new_vect_at_end_pts[0] = vect_at_end_pts[0];
539  new_vect_at_end_pts[1] = vec;
540  findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
541  return;
542  }
543  if (vec.dot (vect_at_end_pts[1]) < 0)
544  {
545  Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
546  new_end_pts[0] = start_pt;
547  new_end_pts[1] = end_pts[1];
548  new_vect_at_end_pts[0] = vec;
549  new_vect_at_end_pts[1] = vect_at_end_pts[1];
550  findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
551  return;
552  }
553  intersection = start_pt;
554  return;
555  }
556 }
557 
558 
559 //////////////////////////////////////////////////////////////////////////////////////////////
560 template <typename PointNT> void
561 pcl::GridProjection<PointNT>::fillPad (const Eigen::Vector3i &index)
562 {
563  for (int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
564  {
565  for (int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
566  {
567  for (int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
568  {
569  Eigen::Vector3i cell_index_3d (i, j, k);
570  unsigned int cell_index_1d = getIndexIn1D (cell_index_3d);
571  if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ())
572  {
573  cell_hash_map_[cell_index_1d].data_indices.resize (0);
574  getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface);
575  }
576  }
577  }
578  }
579 }
580 
581 
582 //////////////////////////////////////////////////////////////////////////////////////////////
583 template <typename PointNT> void
585  const Eigen::Vector3i &,
586  std::vector <int> &pt_union_indices,
587  const Leaf &cell_data)
588 {
589  // Get point on grid
590  Eigen::Vector4f grid_pt (
591  cell_data.pt_on_surface.x () - static_cast<float> (leaf_size_) / 2.0f,
592  cell_data.pt_on_surface.y () + static_cast<float> (leaf_size_) / 2.0f,
593  cell_data.pt_on_surface.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
594 
595  // Save the vector and the point on the surface
596  getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt);
597  getProjection (cell_data.pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface);
598 }
599 
600 //////////////////////////////////////////////////////////////////////////////////////////////
601 template <typename PointNT> void
602 pcl::GridProjection<PointNT>::storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &,
603  const Leaf &cell_data)
604 {
605  Eigen::Vector4f cell_center = cell_data.pt_on_surface;
606  Eigen::Vector4f grid_pt (
607  cell_center.x () - static_cast<float> (leaf_size_) / 2.0f,
608  cell_center.y () + static_cast<float> (leaf_size_) / 2.0f,
609  cell_center.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
610 
611  std::vector <int> k_indices;
612  k_indices.resize (k_);
613  std::vector <float> k_squared_distances;
614  k_squared_distances.resize (k_);
615 
616  PointNT pt; pt.x = grid_pt.x (); pt.y = grid_pt.y (); pt.z = grid_pt.z ();
617  tree_->nearestKSearch (pt, k_, k_indices, k_squared_distances);
618 
619  getVectorAtPointKNN (grid_pt, k_indices, k_squared_distances, cell_hash_map_[index_1d].vect_at_grid_pt);
620  getProjectionWithPlaneFit (cell_center, k_indices, cell_hash_map_[index_1d].pt_on_surface);
621 }
622 
623 //////////////////////////////////////////////////////////////////////////////////////////////
624 template <typename PointNT> bool
625 pcl::GridProjection<PointNT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
626 {
627  data_.reset (new pcl::PointCloud<PointNT> (*input_));
628  getBoundingBox ();
629 
630  // Store the point cloud data into the voxel grid, and at the same time
631  // create a hash map to store the information for each cell
632  cell_hash_map_.max_load_factor (2.0);
633  cell_hash_map_.rehash (data_->points.size () / static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
634 
635  // Go over all points and insert them into the right leaf
636  for (int cp = 0; cp < static_cast<int> (data_->points.size ()); ++cp)
637  {
638  // Check if the point is invalid
639  if (!pcl_isfinite (data_->points[cp].x) ||
640  !pcl_isfinite (data_->points[cp].y) ||
641  !pcl_isfinite (data_->points[cp].z))
642  continue;
643 
644  Eigen::Vector3i index_3d;
645  getCellIndex (data_->points[cp].getVector4fMap (), index_3d);
646  int index_1d = getIndexIn1D (index_3d);
647  if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
648  {
649  Leaf cell_data;
650  cell_data.data_indices.push_back (cp);
651  getCellCenterFromIndex (index_3d, cell_data.pt_on_surface);
652  cell_hash_map_[index_1d] = cell_data;
653  occupied_cell_list_[index_1d] = 1;
654  }
655  else
656  {
657  Leaf cell_data = cell_hash_map_.at (index_1d);
658  cell_data.data_indices.push_back (cp);
659  cell_hash_map_[index_1d] = cell_data;
660  }
661  }
662 
663  Eigen::Vector3i index;
664  int numOfFilledPad = 0;
665 
666  for (int i = 0; i < data_size_; ++i)
667  {
668  for (int j = 0; j < data_size_; ++j)
669  {
670  for (int k = 0; k < data_size_; ++k)
671  {
672  index[0] = i;
673  index[1] = j;
674  index[2] = k;
675  if (occupied_cell_list_[getIndexIn1D (index)])
676  {
677  fillPad (index);
678  numOfFilledPad++;
679  }
680  }
681  }
682  }
683 
684  // Update the hashtable and store the vector and point
685  BOOST_FOREACH (typename HashMap::value_type entry, cell_hash_map_)
686  {
687  getIndexIn3D (entry.first, index);
688  std::vector <int> pt_union_indices;
689  getDataPtsUnion (index, pt_union_indices);
690 
691  // Needs at least 10 points (?)
692  // NOTE: set as parameter later
693  if (pt_union_indices.size () > 10)
694  {
695  storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
696  //storeVectAndSurfacePointKNN(entry.first, index, entry.second);
697  occupied_cell_list_[entry.first] = 1;
698  }
699  }
700 
701  // Go through the hash table another time to extract surface
702  BOOST_FOREACH (typename HashMap::value_type entry, cell_hash_map_)
703  {
704  getIndexIn3D (entry.first, index);
705  std::vector <int> pt_union_indices;
706  getDataPtsUnion (index, pt_union_indices);
707 
708  // Needs at least 10 points (?)
709  // NOTE: set as parameter later
710  if (pt_union_indices.size () > 10)
711  createSurfaceForCell (index, pt_union_indices);
712  }
713 
714  polygons.resize (surface_.size () / 4);
715  // Copy the data from surface_ to polygons
716  for (int i = 0; i < static_cast<int> (polygons.size ()); ++i)
717  {
718  pcl::Vertices v;
719  v.vertices.resize (4);
720  for (int j = 0; j < 4; ++j)
721  v.vertices[j] = i*4+j;
722  polygons[i] = v;
723  }
724  return (true);
725 }
726 
727 //////////////////////////////////////////////////////////////////////////////////////////////
728 template <typename PointNT> void
730 {
731  if (!reconstructPolygons (output.polygons))
732  return;
733 
734  // The mesh surface is held in surface_. Copy it to the output format
735  output.header = input_->header;
736 
738  cloud.width = static_cast<uint32_t> (surface_.size ());
739  cloud.height = 1;
740  cloud.is_dense = true;
741 
742  cloud.points.resize (surface_.size ());
743  // Copy the data from surface_ to cloud
744  for (size_t i = 0; i < cloud.points.size (); ++i)
745  {
746  cloud.points[i].x = surface_[i].x ();
747  cloud.points[i].y = surface_[i].y ();
748  cloud.points[i].z = surface_[i].z ();
749  }
750  pcl::toPCLPointCloud2 (cloud, output.cloud);
751 }
752 
753 //////////////////////////////////////////////////////////////////////////////////////////////
754 template <typename PointNT> void
756  std::vector<pcl::Vertices> &polygons)
757 {
758  if (!reconstructPolygons (polygons))
759  return;
760 
761  // The mesh surface is held in surface_. Copy it to the output format
762  points.header = input_->header;
763  points.width = static_cast<uint32_t> (surface_.size ());
764  points.height = 1;
765  points.is_dense = true;
766 
767  points.resize (surface_.size ());
768  // Copy the data from surface_ to cloud
769  for (size_t i = 0; i < points.size (); ++i)
770  {
771  points[i].x = surface_[i].x ();
772  points[i].y = surface_[i].y ();
773  points[i].z = surface_[i].z ();
774  }
775 }
776 
777 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
778 
779 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
780