Point Cloud Library (PCL)  1.13.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  cloud_scale_factor_ = scale_factor;
77  PCL_DEBUG ("[pcl::GridProjection::scaleInputDataPoint] scale_factor=%g\n", scale_factor);
78  for (auto& point: *data_) {
79  point.getVector3fMap() /= static_cast<float> (scale_factor);
80  }
81  max_p_ /= static_cast<float> (scale_factor);
82  min_p_ /= static_cast<float> (scale_factor);
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointNT> void
88 {
89  pcl::getMinMax3D (*data_, min_p_, max_p_);
90 
91  Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
92  double scale_factor = (std::max)((std::max)(bounding_box_size.x (),
93  bounding_box_size.y ()),
94  bounding_box_size.z ());
95  if (scale_factor > 1)
96  scaleInputDataPoint (scale_factor);
97 
98  // Round the max_p_, min_p_ so that they are aligned with the cells vertices
99  int upper_right_index[3];
100  int lower_left_index[3];
101  for (std::size_t i = 0; i < 3; ++i)
102  {
103  upper_right_index[i] = static_cast<int> (max_p_(i) / leaf_size_ + 5);
104  lower_left_index[i] = static_cast<int> (min_p_(i) / leaf_size_ - 5);
105  max_p_(i) = static_cast<float> (upper_right_index[i] * leaf_size_);
106  min_p_(i) = static_cast<float> (lower_left_index[i] * leaf_size_);
107  }
108  bounding_box_size = max_p_ - min_p_;
109  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
110  bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
111  double max_size =
112  (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
113  bounding_box_size.z ());
114 
115  data_size_ = static_cast<int> (max_size / leaf_size_);
116  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n",
117  min_p_.x (), min_p_.y (), min_p_.z ());
118  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n",
119  max_p_.x (), max_p_.y (), max_p_.z ());
120  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_);
121  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_);
122  occupied_cell_list_.resize (data_size_ * data_size_ * data_size_);
123  gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename PointNT> void
129  const Eigen::Vector4f &cell_center,
130  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const
131 {
132  assert (pts.size () == 8);
133 
134  float sz = static_cast<float> (leaf_size_) / 2.0f;
135 
136  pts[0] = cell_center + Eigen::Vector4f (-sz, sz, -sz, 0);
137  pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0);
138  pts[2] = cell_center + Eigen::Vector4f (sz, -sz, -sz, 0);
139  pts[3] = cell_center + Eigen::Vector4f (sz, sz, -sz, 0);
140  pts[4] = cell_center + Eigen::Vector4f (-sz, sz, sz, 0);
141  pts[5] = cell_center + Eigen::Vector4f (-sz, -sz, sz, 0);
142  pts[6] = cell_center + Eigen::Vector4f (sz, -sz, sz, 0);
143  pts[7] = cell_center + Eigen::Vector4f (sz, sz, sz, 0);
144 }
145 
146 //////////////////////////////////////////////////////////////////////////////////////////////
147 template <typename PointNT> void
148 pcl::GridProjection<PointNT>::getDataPtsUnion (const Eigen::Vector3i &index,
149  pcl::Indices &pt_union_indices)
150 {
151  for (int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
152  {
153  for (int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
154  {
155  for (int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
156  {
157  Eigen::Vector3i cell_index_3d (i, j, k);
158  int cell_index_1d = getIndexIn1D (cell_index_3d);
159  if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
160  {
161  // Get the indices of the input points within the cell(i,j,k), which
162  // is stored in the hash map
163  pt_union_indices.insert (pt_union_indices.end (),
164  cell_hash_map_.at (cell_index_1d).data_indices.begin (),
165  cell_hash_map_.at (cell_index_1d).data_indices.end ());
166  }
167  }
168  }
169  }
170 }
171 
172 //////////////////////////////////////////////////////////////////////////////////////////////
173 template <typename PointNT> void
175  pcl::Indices &pt_union_indices)
176 {
177  // 8 vertices of the cell
178  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
179 
180  // 4 end points that shared by 3 edges connecting the upper left front points
181  Eigen::Vector4f pts[4];
182  Eigen::Vector3f vector_at_pts[4];
183 
184  // Given the index of cell, caluate the coordinates of the eight vertices of the cell
185  // index the index of the cell in (x,y,z) 3d format
186  Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
187  getCellCenterFromIndex (index, cell_center);
188  getVertexFromCellCenter (cell_center, vertices);
189 
190  // Get the indices of the cells which stores the 4 end points.
191  Eigen::Vector3i indices[4];
192  indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
193  indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
194  indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
195  indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
196 
197  // Get the coordinate of the 4 end points, and the corresponding vectors
198  for (std::size_t i = 0; i < 4; ++i)
199  {
200  pts[i] = vertices[I_SHIFT_PT[i]];
201  unsigned int index_1d = getIndexIn1D (indices[i]);
202  if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () ||
203  occupied_cell_list_[index_1d] == 0)
204  return;
205  vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
206  }
207 
208  // Go through the 3 edges, test whether they are intersected by the surface
209  for (std::size_t i = 0; i < 3; ++i)
210  {
211  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
212  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
213  for (std::size_t j = 0; j < 2; ++j)
214  {
215  end_pts[j] = pts[I_SHIFT_EDGE[i][j]];
216  vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]];
217  }
218 
219  if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
220  {
221  // Indices of cells that contains points which will be connected to
222  // create a polygon
223  Eigen::Vector3i polygon[4];
224  Eigen::Vector4f polygon_pts[4];
225  int polygon_indices_1d[4];
226  bool is_all_in_hash_map = true;
227  switch (i)
228  {
229  case 0:
230  polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
231  polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
232  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
233  polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
234  break;
235  case 1:
236  polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
237  polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
238  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
239  polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
240  break;
241  case 2:
242  polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
243  polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
244  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
245  polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
246  break;
247  default:
248  break;
249  }
250  for (std::size_t k = 0; k < 4; k++)
251  {
252  polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
253  if (!occupied_cell_list_[polygon_indices_1d[k]])
254  {
255  is_all_in_hash_map = false;
256  break;
257  }
258  }
259  if (is_all_in_hash_map)
260  {
261  for (std::size_t k = 0; k < 4; k++)
262  {
263  polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
264  surface_.push_back (polygon_pts[k]);
265  }
266  }
267  }
268  }
269 }
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointNT> void
274  pcl::Indices &pt_union_indices, Eigen::Vector4f &projection)
275 {
276  const double projection_distance = leaf_size_ * 3;
277  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
278  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
279  end_pt[0] = p;
280  getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
281  end_pt_vect[0].normalize();
282 
283  double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
284 
285  // Find another point which is projection_distance away from the p, do a
286  // binary search between these two points, to find the projected point on the
287  // surface
288  if (dSecond > 0)
289  end_pt[1] = end_pt[0] + Eigen::Vector4f (
290  end_pt_vect[0][0] * static_cast<float> (projection_distance),
291  end_pt_vect[0][1] * static_cast<float> (projection_distance),
292  end_pt_vect[0][2] * static_cast<float> (projection_distance),
293  0.0f);
294  else
295  end_pt[1] = end_pt[0] - Eigen::Vector4f (
296  end_pt_vect[0][0] * static_cast<float> (projection_distance),
297  end_pt_vect[0][1] * static_cast<float> (projection_distance),
298  end_pt_vect[0][2] * static_cast<float> (projection_distance),
299  0.0f);
300  getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
301  if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
302  {
303  Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
304  findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
305  }
306  else
307  projection = p;
308 }
309 
310 //////////////////////////////////////////////////////////////////////////////////////////////
311 template <typename PointNT> void
313  pcl::Indices (&pt_union_indices),
314  Eigen::Vector4f &projection)
315 {
316  // Compute the plane coefficients
317  Eigen::Vector4f model_coefficients;
318  /// @remark iterative weighted least squares or sac might give better results
319  Eigen::Matrix3f covariance_matrix;
320  Eigen::Vector4f xyz_centroid;
321 
322  computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid);
323 
324  // Get the plane normal
325  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
326  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
327  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
328 
329  // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
330  model_coefficients[0] = eigen_vector [0];
331  model_coefficients[1] = eigen_vector [1];
332  model_coefficients[2] = eigen_vector [2];
333  model_coefficients[3] = 0;
334  // Hessian form (D = nc . p_plane (centroid here) + p)
335  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
336 
337  // Projected point
338  Eigen::Vector3f point (p.x (), p.y (), p.z ()); //= Eigen::Vector3f::MapAligned (&output[cp].x, 3);
339  float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
340  point -= distance * model_coefficients.head < 3 > ();
341 
342  projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
343 }
344 
345 //////////////////////////////////////////////////////////////////////////////////////////////
346 template <typename PointNT> void
348  pcl::Indices &pt_union_indices,
349  Eigen::Vector3f &vo)
350 {
351  std::vector <double> pt_union_dist (pt_union_indices.size ());
352  std::vector <double> pt_union_weight (pt_union_indices.size ());
353  Eigen::Vector3f out_vector (0, 0, 0);
354  double sum = 0.0;
355 
356  for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
357  {
358  Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
359  pt_union_dist[i] = (pp - p).squaredNorm ();
360  pt_union_weight[i] = pow (M_E, -pow (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] = true;
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 
655  for (int i = 0; i < data_size_; ++i)
656  {
657  for (int j = 0; j < data_size_; ++j)
658  {
659  for (int k = 0; k < data_size_; ++k)
660  {
661  index[0] = i;
662  index[1] = j;
663  index[2] = k;
664  if (occupied_cell_list_[getIndexIn1D (index)])
665  {
666  fillPad (index);
667  }
668  }
669  }
670  }
671 
672  // Update the hashtable and store the vector and point
673  for (const auto &entry : cell_hash_map_)
674  {
675  getIndexIn3D (entry.first, index);
676  pcl::Indices pt_union_indices;
677  getDataPtsUnion (index, pt_union_indices);
678 
679  // Needs at least 10 points (?)
680  // NOTE: set as parameter later
681  if (pt_union_indices.size () > 10)
682  {
683  storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
684  //storeVectAndSurfacePointKNN(entry.first, index, entry.second);
685  occupied_cell_list_[entry.first] = 1;
686  }
687  }
688 
689  // Go through the hash table another time to extract surface
690  for (const auto &entry : cell_hash_map_)
691  {
692  getIndexIn3D (entry.first, index);
693  pcl::Indices pt_union_indices;
694  getDataPtsUnion (index, pt_union_indices);
695 
696  // Needs at least 10 points (?)
697  // NOTE: set as parameter later
698  if (pt_union_indices.size () > 10)
699  createSurfaceForCell (index, pt_union_indices);
700  }
701 
702  polygons.resize (surface_.size () / 4);
703  // Copy the data from surface_ to polygons
704  for (int i = 0; i < static_cast<int> (polygons.size ()); ++i)
705  {
706  pcl::Vertices v;
707  v.vertices.resize (4);
708  for (int j = 0; j < 4; ++j)
709  v.vertices[j] = i*4+j;
710  polygons[i] = v;
711  }
712  return (true);
713 }
714 
715 //////////////////////////////////////////////////////////////////////////////////////////////
716 template <typename PointNT> void
718 {
719  if (!reconstructPolygons (output.polygons))
720  return;
721 
722  // The mesh surface is held in surface_. Copy it to the output format
723  output.header = input_->header;
724 
726  cloud.width = surface_.size ();
727  cloud.height = 1;
728  cloud.is_dense = true;
729 
730  cloud.resize (surface_.size ());
731  // Copy the data from surface_ to cloud
732  for (std::size_t i = 0; i < cloud.size (); ++i)
733  {
734  cloud[i].x = cloud_scale_factor_*surface_[i].x ();
735  cloud[i].y = cloud_scale_factor_*surface_[i].y ();
736  cloud[i].z = cloud_scale_factor_*surface_[i].z ();
737  }
738  pcl::toPCLPointCloud2 (cloud, output.cloud);
739 }
740 
741 //////////////////////////////////////////////////////////////////////////////////////////////
742 template <typename PointNT> void
744  std::vector<pcl::Vertices> &polygons)
745 {
746  if (!reconstructPolygons (polygons))
747  return;
748 
749  // The mesh surface is held in surface_. Copy it to the output format
750  points.header = input_->header;
751  points.width = surface_.size ();
752  points.height = 1;
753  points.is_dense = true;
754 
755  points.resize (surface_.size ());
756  // Copy the data from surface_ to cloud
757  for (std::size_t i = 0; i < points.size (); ++i)
758  {
759  points[i].x = cloud_scale_factor_*surface_[i].x ();
760  points[i].y = cloud_scale_factor_*surface_[i].y ();
761  points[i].z = cloud_scale_factor_*surface_[i].z ();
762  }
763 }
764 
765 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
766 
767 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
768 
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()
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 vertices 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.
~GridProjection() override
Destructor.
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:509
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:295
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:264
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:18
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:22
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:20
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:18