Point Cloud Library (PCL)  1.11.1
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.points.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 
Define methods for centroid estimation and covariance matrix calculus.
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.
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.
double getMagAtPoint(const Eigen::Vector4f &p, const std::vector< int > &pt_union_indices)
Get the magnitude of the vector by summing up the distance.
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...
double getD1AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 1st 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...
double getD2AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 2nd derivative.
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.
~GridProjection()
Destructor.
GridProjection()
Constructor.
void getDataPtsUnion(const Eigen::Vector3i &index, std::vector< int > &pt_union_indices)
Obtain the index of a cell and the pad size.
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.
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 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.
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....
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...
void performReconstruction(pcl::PolygonMesh &output) override
Create the surface.
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...
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.
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:419
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:478
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:408
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
std::size_t size() const
Definition: point_cloud.h:459
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
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:243
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
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:241
const int I_SHIFT_PT[4]
const int I_SHIFT_EDGE[3][2]
#define M_E
Definition: pcl_macros.h:187
Eigen::Vector4f pt_on_surface
std::vector< int > data_indices
::pcl::PCLHeader header
Definition: PolygonMesh.h:20
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:24
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:22
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:16
std::vector< std::uint32_t > vertices
Definition: Vertices.h:20