Point Cloud Library (PCL)  1.12.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_ (), 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  pcl::Indices &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  pcl::Indices &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  pcl::Indices &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  pcl::Indices (&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  pcl::Indices &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  pcl::Indices &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 pcl::Indices &pt_union_indices)
428 {
429  std::vector <double> pt_union_dist (pt_union_indices.size ());
430  double sum = 0.0;
431  for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
432  {
433  Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
434  pt_union_dist[i] = (pp - p).norm ();
435  sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
436  }
437  return (sum);
438 }
439 
440 //////////////////////////////////////////////////////////////////////////////////////////////
441 template <typename PointNT> double
442 pcl::GridProjection<PointNT>::getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
443  const pcl::Indices &pt_union_indices)
444 {
445  double sz = 0.01 * leaf_size_;
446  Eigen::Vector3f v = vec * static_cast<float> (sz);
447 
448  double forward = getMagAtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
449  double backward = getMagAtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
450  return ((forward - backward) / (0.02 * leaf_size_));
451 }
452 
453 //////////////////////////////////////////////////////////////////////////////////////////////
454 template <typename PointNT> double
455 pcl::GridProjection<PointNT>::getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
456  const pcl::Indices &pt_union_indices)
457 {
458  double sz = 0.01 * leaf_size_;
459  Eigen::Vector3f v = vec * static_cast<float> (sz);
460 
461  double forward = getD1AtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
462  double backward = getD1AtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
463  return ((forward - backward) / (0.02 * leaf_size_));
464 }
465 
466 //////////////////////////////////////////////////////////////////////////////////////////////
467 template <typename PointNT> bool
468 pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
469  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
470  pcl::Indices &pt_union_indices)
471 {
472  assert (end_pts.size () == 2);
473  assert (vect_at_end_pts.size () == 2);
474 
475  double length[2];
476  for (std::size_t i = 0; i < 2; ++i)
477  {
478  length[i] = vect_at_end_pts[i].norm ();
479  vect_at_end_pts[i].normalize ();
480  }
481  double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
482  if (dot_prod < 0)
483  {
484  double ratio = length[0] / (length[0] + length[1]);
485  Eigen::Vector4f start_pt =
486  end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
487  Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
488  findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
489 
490  Eigen::Vector3f vec;
491  getVectorAtPoint (intersection_pt, pt_union_indices, vec);
492  vec.normalize ();
493 
494  double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
495  if (d2 < 0)
496  return (true);
497  }
498  return (false);
499 }
500 
501 //////////////////////////////////////////////////////////////////////////////////////////////
502 template <typename PointNT> void
504  const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
505  const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
506  const Eigen::Vector4f &start_pt,
507  pcl::Indices &pt_union_indices,
508  Eigen::Vector4f &intersection)
509 {
510  assert (end_pts.size () == 2);
511  assert (vect_at_end_pts.size () == 2);
512 
513  Eigen::Vector3f vec;
514  getVectorAtPoint (start_pt, pt_union_indices, vec);
515  double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
516  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
517  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
518  if ((std::abs (d1) < 10e-3) || (level == max_binary_search_level_))
519  {
520  intersection = start_pt;
521  return;
522  }
523  vec.normalize ();
524  if (vec.dot (vect_at_end_pts[0]) < 0)
525  {
526  Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
527  new_end_pts[0] = end_pts[0];
528  new_end_pts[1] = start_pt;
529  new_vect_at_end_pts[0] = vect_at_end_pts[0];
530  new_vect_at_end_pts[1] = vec;
531  findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
532  return;
533  }
534  if (vec.dot (vect_at_end_pts[1]) < 0)
535  {
536  Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
537  new_end_pts[0] = start_pt;
538  new_end_pts[1] = end_pts[1];
539  new_vect_at_end_pts[0] = vec;
540  new_vect_at_end_pts[1] = vect_at_end_pts[1];
541  findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
542  return;
543  }
544  intersection = start_pt;
545  return;
546 }
547 
548 
549 //////////////////////////////////////////////////////////////////////////////////////////////
550 template <typename PointNT> void
551 pcl::GridProjection<PointNT>::fillPad (const Eigen::Vector3i &index)
552 {
553  for (int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
554  {
555  for (int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
556  {
557  for (int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
558  {
559  Eigen::Vector3i cell_index_3d (i, j, k);
560  unsigned int cell_index_1d = getIndexIn1D (cell_index_3d);
561  if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ())
562  {
563  cell_hash_map_[cell_index_1d].data_indices.resize (0);
564  getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface);
565  }
566  }
567  }
568  }
569 }
570 
571 
572 //////////////////////////////////////////////////////////////////////////////////////////////
573 template <typename PointNT> void
575  const Eigen::Vector3i &,
576  pcl::Indices &pt_union_indices,
577  const Leaf &cell_data)
578 {
579  // Get point on grid
580  Eigen::Vector4f grid_pt (
581  cell_data.pt_on_surface.x () - static_cast<float> (leaf_size_) / 2.0f,
582  cell_data.pt_on_surface.y () + static_cast<float> (leaf_size_) / 2.0f,
583  cell_data.pt_on_surface.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
584 
585  // Save the vector and the point on the surface
586  getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt);
587  getProjection (cell_data.pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface);
588 }
589 
590 //////////////////////////////////////////////////////////////////////////////////////////////
591 template <typename PointNT> void
592 pcl::GridProjection<PointNT>::storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &,
593  const Leaf &cell_data)
594 {
595  Eigen::Vector4f cell_center = cell_data.pt_on_surface;
596  Eigen::Vector4f grid_pt (
597  cell_center.x () - static_cast<float> (leaf_size_) / 2.0f,
598  cell_center.y () + static_cast<float> (leaf_size_) / 2.0f,
599  cell_center.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
600 
601  pcl::Indices k_indices;
602  k_indices.resize (k_);
603  std::vector <float> k_squared_distances;
604  k_squared_distances.resize (k_);
605 
606  PointNT pt; pt.x = grid_pt.x (); pt.y = grid_pt.y (); pt.z = grid_pt.z ();
607  tree_->nearestKSearch (pt, k_, k_indices, k_squared_distances);
608 
609  getVectorAtPointKNN (grid_pt, k_indices, k_squared_distances, cell_hash_map_[index_1d].vect_at_grid_pt);
610  getProjectionWithPlaneFit (cell_center, k_indices, cell_hash_map_[index_1d].pt_on_surface);
611 }
612 
613 //////////////////////////////////////////////////////////////////////////////////////////////
614 template <typename PointNT> bool
615 pcl::GridProjection<PointNT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
616 {
617  data_.reset (new pcl::PointCloud<PointNT> (*input_));
618  getBoundingBox ();
619 
620  // Store the point cloud data into the voxel grid, and at the same time
621  // create a hash map to store the information for each cell
622  cell_hash_map_.max_load_factor (2.0);
623  cell_hash_map_.rehash (data_->size () / static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
624 
625  // Go over all points and insert them into the right leaf
626  for (pcl::index_t cp = 0; cp < static_cast<pcl::index_t> (data_->size ()); ++cp)
627  {
628  // Check if the point is invalid
629  if (!std::isfinite ((*data_)[cp].x) ||
630  !std::isfinite ((*data_)[cp].y) ||
631  !std::isfinite ((*data_)[cp].z))
632  continue;
633 
634  Eigen::Vector3i index_3d;
635  getCellIndex ((*data_)[cp].getVector4fMap (), index_3d);
636  int index_1d = getIndexIn1D (index_3d);
637  if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
638  {
639  Leaf cell_data;
640  cell_data.data_indices.push_back (cp);
641  getCellCenterFromIndex (index_3d, cell_data.pt_on_surface);
642  cell_hash_map_[index_1d] = cell_data;
643  occupied_cell_list_[index_1d] = 1;
644  }
645  else
646  {
647  Leaf cell_data = cell_hash_map_.at (index_1d);
648  cell_data.data_indices.push_back (cp);
649  cell_hash_map_[index_1d] = cell_data;
650  }
651  }
652 
653  Eigen::Vector3i index;
654  int numOfFilledPad = 0;
655 
656  for (int i = 0; i < data_size_; ++i)
657  {
658  for (int j = 0; j < data_size_; ++j)
659  {
660  for (int k = 0; k < data_size_; ++k)
661  {
662  index[0] = i;
663  index[1] = j;
664  index[2] = k;
665  if (occupied_cell_list_[getIndexIn1D (index)])
666  {
667  fillPad (index);
668  numOfFilledPad++;
669  }
670  }
671  }
672  }
673 
674  // Update the hashtable and store the vector and point
675  for (const auto &entry : cell_hash_map_)
676  {
677  getIndexIn3D (entry.first, index);
678  pcl::Indices pt_union_indices;
679  getDataPtsUnion (index, pt_union_indices);
680 
681  // Needs at least 10 points (?)
682  // NOTE: set as parameter later
683  if (pt_union_indices.size () > 10)
684  {
685  storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
686  //storeVectAndSurfacePointKNN(entry.first, index, entry.second);
687  occupied_cell_list_[entry.first] = 1;
688  }
689  }
690 
691  // Go through the hash table another time to extract surface
692  for (const auto &entry : cell_hash_map_)
693  {
694  getIndexIn3D (entry.first, index);
695  pcl::Indices pt_union_indices;
696  getDataPtsUnion (index, pt_union_indices);
697 
698  // Needs at least 10 points (?)
699  // NOTE: set as parameter later
700  if (pt_union_indices.size () > 10)
701  createSurfaceForCell (index, pt_union_indices);
702  }
703 
704  polygons.resize (surface_.size () / 4);
705  // Copy the data from surface_ to polygons
706  for (int i = 0; i < static_cast<int> (polygons.size ()); ++i)
707  {
708  pcl::Vertices v;
709  v.vertices.resize (4);
710  for (int j = 0; j < 4; ++j)
711  v.vertices[j] = i*4+j;
712  polygons[i] = v;
713  }
714  return (true);
715 }
716 
717 //////////////////////////////////////////////////////////////////////////////////////////////
718 template <typename PointNT> void
720 {
721  if (!reconstructPolygons (output.polygons))
722  return;
723 
724  // The mesh surface is held in surface_. Copy it to the output format
725  output.header = input_->header;
726 
728  cloud.width = surface_.size ();
729  cloud.height = 1;
730  cloud.is_dense = true;
731 
732  cloud.resize (surface_.size ());
733  // Copy the data from surface_ to cloud
734  for (std::size_t i = 0; i < cloud.size (); ++i)
735  {
736  cloud[i].x = surface_[i].x ();
737  cloud[i].y = surface_[i].y ();
738  cloud[i].z = surface_[i].z ();
739  }
740  pcl::toPCLPointCloud2 (cloud, output.cloud);
741 }
742 
743 //////////////////////////////////////////////////////////////////////////////////////////////
744 template <typename PointNT> void
746  std::vector<pcl::Vertices> &polygons)
747 {
748  if (!reconstructPolygons (polygons))
749  return;
750 
751  // The mesh surface is held in surface_. Copy it to the output format
752  points.header = input_->header;
753  points.width = surface_.size ();
754  points.height = 1;
755  points.is_dense = true;
756 
757  points.resize (surface_.size ());
758  // Copy the data from surface_ to cloud
759  for (std::size_t i = 0; i < points.size (); ++i)
760  {
761  points[i].x = surface_[i].x ();
762  points[i].y = surface_[i].y ();
763  points[i].z = surface_[i].z ();
764  }
765 }
766 
767 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
768 
769 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
770 
Define methods for centroid estimation and covariance matrix calculus.
void getProjection(const Eigen::Vector4f &p, pcl::Indices &pt_union_indices, Eigen::Vector4f &projection)
Given the coordinates of one point, project it onto the surface, return the projected point.
bool reconstructPolygons(std::vector< pcl::Vertices > &polygons)
The actual surface reconstruction method.
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.
void getBoundingBox()
Get the bounding box for the input data points, also calculating the cell size, and the gaussian scal...
void getVectorAtPointKNN(const Eigen::Vector4f &p, pcl::Indices &k_indices, std::vector< float > &k_squared_distances, Eigen::Vector3f &vo)
Given the location of a point, get it's vector.
double getD2AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const pcl::Indices &pt_union_indices)
Get the 2nd derivative.
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...
~GridProjection()
Destructor.
GridProjection()
Constructor.
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.
void createSurfaceForCell(const Eigen::Vector3i &index, pcl::Indices &pt_union_indices)
Given the index of a cell, exam it's up, left, front edges, and add the vectices to m_surface list....
double getD1AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const pcl::Indices &pt_union_indices)
Get the 1st derivative.
void storeVectAndSurfacePoint(int index_1d, const Eigen::Vector3i &index_3d, pcl::Indices &pt_union_indices, const Leaf &cell_data)
Go through all the entries in the hash table and update the cellData.
void getDataPtsUnion(const Eigen::Vector3i &index, pcl::Indices &pt_union_indices)
Obtain the index of a cell and the pad size.
void getProjectionWithPlaneFit(const Eigen::Vector4f &p, pcl::Indices &pt_union_indices, Eigen::Vector4f &projection)
Given the coordinates of one point, project it onto the surface, return the projected point.
void performReconstruction(pcl::PolygonMesh &output) override
Create the surface.
double getMagAtPoint(const Eigen::Vector4f &p, const pcl::Indices &pt_union_indices)
Get the magnitude of the vector by summing up the distance.
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...
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, pcl::Indices &pt_union_indices)
Test whether the edge is intersected by the surface by doing the dot product of the vector at two end...
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, pcl::Indices &pt_union_indices, Eigen::Vector4f &intersection)
Find point where the edge intersects the surface.
void getVectorAtPoint(const Eigen::Vector4f &p, pcl::Indices &pt_union_indices, Eigen::Vector3f &vo)
Given the location of a point, get it's vector.
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:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
Calculates the weighted average and the covariance matrix.
void add(const VectorType &sample, real weight=1.0)
Add a new sample.
const VectorType & getMean() const
Get the mean of the added vectors.
void getEigenVector1(VectorType &eigen_vector1) const
Get the eigenvector corresponding to the smallest eigenvalue.
Define standard C methods and C++ classes that are common to all methods.
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
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
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
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
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:239
const int I_SHIFT_PT[4]
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
const int I_SHIFT_EDGE[3][2]
#define M_E
Definition: pcl_macros.h:196
Eigen::Vector4f pt_on_surface
::pcl::PCLHeader header
Definition: PolygonMesh.h:19
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:23
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:21
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:15
Indices vertices
Definition: Vertices.h:19