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