Point Cloud Library (PCL)  1.14.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  if (computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid) == 0) {
323  PCL_ERROR("[pcl::GridProjection::getProjectionWithPlaneFit] cloud or indices are empty!\n");
324  projection = p;
325  return;
326  }
327 
328  // Get the plane normal
329  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
330  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
331  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
332 
333  // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
334  model_coefficients[0] = eigen_vector [0];
335  model_coefficients[1] = eigen_vector [1];
336  model_coefficients[2] = eigen_vector [2];
337  model_coefficients[3] = 0;
338  // Hessian form (D = nc . p_plane (centroid here) + p)
339  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
340 
341  // Projected point
342  Eigen::Vector3f point (p.x (), p.y (), p.z ()); //= Eigen::Vector3f::MapAligned (&output[cp].x, 3);
343  float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
344  point -= distance * model_coefficients.head < 3 > ();
345 
346  projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
347 }
348 
349 //////////////////////////////////////////////////////////////////////////////////////////////
350 template <typename PointNT> void
352  pcl::Indices &pt_union_indices,
353  Eigen::Vector3f &vo)
354 {
355  std::vector <double> pt_union_dist (pt_union_indices.size ());
356  std::vector <double> pt_union_weight (pt_union_indices.size ());
357  Eigen::Vector3f out_vector (0, 0, 0);
358  double sum = 0.0;
359 
360  for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
361  {
362  Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
363  pt_union_dist[i] = (pp - p).squaredNorm ();
364  pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
365  sum += pt_union_weight[i];
366  }
367 
368  pcl::VectorAverage3f vector_average;
369 
370  Eigen::Vector3f v (
371  (*data_)[pt_union_indices[0]].normal[0],
372  (*data_)[pt_union_indices[0]].normal[1],
373  (*data_)[pt_union_indices[0]].normal[2]);
374 
375  for (std::size_t i = 0; i < pt_union_weight.size (); ++i)
376  {
377  pt_union_weight[i] /= sum;
378  Eigen::Vector3f vec ((*data_)[pt_union_indices[i]].normal[0],
379  (*data_)[pt_union_indices[i]].normal[1],
380  (*data_)[pt_union_indices[i]].normal[2]);
381  if (vec.dot (v) < 0)
382  vec = -vec;
383  vector_average.add (vec, static_cast<float> (pt_union_weight[i]));
384  }
385  out_vector = vector_average.getMean ();
386  // vector_average.getEigenVector1(out_vector);
387 
388  out_vector.normalize ();
389  double d1 = getD1AtPoint (p, out_vector, pt_union_indices);
390  out_vector *= static_cast<float> (sum);
391  vo = ((d1 > 0) ? -1 : 1) * out_vector;
392 }
393 
394 //////////////////////////////////////////////////////////////////////////////////////////////
395 template <typename PointNT> void
397  pcl::Indices &k_indices,
398  std::vector <float> &k_squared_distances,
399  Eigen::Vector3f &vo)
400 {
401  Eigen::Vector3f out_vector (0, 0, 0);
402  std::vector <float> k_weight;
403  k_weight.resize (k_);
404  float sum = 0.0;
405  for (int i = 0; i < k_; i++)
406  {
407  //k_weight[i] = pow (M_E, -pow (k_squared_distances[i], 2.0) / gaussian_scale_);
408  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_));
409  sum += k_weight[i];
410  }
411  pcl::VectorAverage3f vector_average;
412  for (int i = 0; i < k_; i++)
413  {
414  k_weight[i] /= sum;
415  Eigen::Vector3f vec ((*data_)[k_indices[i]].normal[0],
416  (*data_)[k_indices[i]].normal[1],
417  (*data_)[k_indices[i]].normal[2]);
418  vector_average.add (vec, k_weight[i]);
419  }
420  vector_average.getEigenVector1 (out_vector);
421  out_vector.normalize ();
422  double d1 = getD1AtPoint (p, out_vector, k_indices);
423  out_vector *= sum;
424  vo = ((d1 > 0) ? -1 : 1) * out_vector;
425 
426 }
427 
428 //////////////////////////////////////////////////////////////////////////////////////////////
429 template <typename PointNT> double
431  const pcl::Indices &pt_union_indices)
432 {
433  std::vector <double> pt_union_dist (pt_union_indices.size ());
434  double sum = 0.0;
435  for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
436  {
437  Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
438  pt_union_dist[i] = (pp - p).norm ();
439  sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
440  }
441  return (sum);
442 }
443 
444 //////////////////////////////////////////////////////////////////////////////////////////////
445 template <typename PointNT> double
446 pcl::GridProjection<PointNT>::getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
447  const pcl::Indices &pt_union_indices)
448 {
449  double sz = 0.01 * leaf_size_;
450  Eigen::Vector3f v = vec * static_cast<float> (sz);
451 
452  double forward = getMagAtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
453  double backward = getMagAtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
454  return ((forward - backward) / (0.02 * leaf_size_));
455 }
456 
457 //////////////////////////////////////////////////////////////////////////////////////////////
458 template <typename PointNT> double
459 pcl::GridProjection<PointNT>::getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
460  const pcl::Indices &pt_union_indices)
461 {
462  double sz = 0.01 * leaf_size_;
463  Eigen::Vector3f v = vec * static_cast<float> (sz);
464 
465  double forward = getD1AtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
466  double backward = getD1AtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
467  return ((forward - backward) / (0.02 * leaf_size_));
468 }
469 
470 //////////////////////////////////////////////////////////////////////////////////////////////
471 template <typename PointNT> bool
472 pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
473  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
474  pcl::Indices &pt_union_indices)
475 {
476  assert (end_pts.size () == 2);
477  assert (vect_at_end_pts.size () == 2);
478 
479  double length[2];
480  for (std::size_t i = 0; i < 2; ++i)
481  {
482  length[i] = vect_at_end_pts[i].norm ();
483  vect_at_end_pts[i].normalize ();
484  }
485  double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
486  if (dot_prod < 0)
487  {
488  double ratio = length[0] / (length[0] + length[1]);
489  Eigen::Vector4f start_pt =
490  end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
491  Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
492  findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
493 
494  Eigen::Vector3f vec;
495  getVectorAtPoint (intersection_pt, pt_union_indices, vec);
496  vec.normalize ();
497 
498  double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
499  if (d2 < 0)
500  return (true);
501  }
502  return (false);
503 }
504 
505 //////////////////////////////////////////////////////////////////////////////////////////////
506 template <typename PointNT> void
508  const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
509  const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
510  const Eigen::Vector4f &start_pt,
511  pcl::Indices &pt_union_indices,
512  Eigen::Vector4f &intersection)
513 {
514  assert (end_pts.size () == 2);
515  assert (vect_at_end_pts.size () == 2);
516 
517  Eigen::Vector3f vec;
518  getVectorAtPoint (start_pt, pt_union_indices, vec);
519  double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
520  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
521  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
522  if ((std::abs (d1) < 10e-3) || (level == max_binary_search_level_))
523  {
524  intersection = start_pt;
525  return;
526  }
527  vec.normalize ();
528  if (vec.dot (vect_at_end_pts[0]) < 0)
529  {
530  Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
531  new_end_pts[0] = end_pts[0];
532  new_end_pts[1] = start_pt;
533  new_vect_at_end_pts[0] = vect_at_end_pts[0];
534  new_vect_at_end_pts[1] = vec;
535  findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
536  return;
537  }
538  if (vec.dot (vect_at_end_pts[1]) < 0)
539  {
540  Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
541  new_end_pts[0] = start_pt;
542  new_end_pts[1] = end_pts[1];
543  new_vect_at_end_pts[0] = vec;
544  new_vect_at_end_pts[1] = vect_at_end_pts[1];
545  findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
546  return;
547  }
548  intersection = start_pt;
549  return;
550 }
551 
552 
553 //////////////////////////////////////////////////////////////////////////////////////////////
554 template <typename PointNT> void
555 pcl::GridProjection<PointNT>::fillPad (const Eigen::Vector3i &index)
556 {
557  for (int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
558  {
559  for (int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
560  {
561  for (int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
562  {
563  Eigen::Vector3i cell_index_3d (i, j, k);
564  unsigned int cell_index_1d = getIndexIn1D (cell_index_3d);
565  if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ())
566  {
567  cell_hash_map_[cell_index_1d].data_indices.resize (0);
568  getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface);
569  }
570  }
571  }
572  }
573 }
574 
575 
576 //////////////////////////////////////////////////////////////////////////////////////////////
577 template <typename PointNT> void
579  const Eigen::Vector3i &,
580  pcl::Indices &pt_union_indices,
581  const Leaf &cell_data)
582 {
583  // Get point on grid
584  Eigen::Vector4f grid_pt (
585  cell_data.pt_on_surface.x () - static_cast<float> (leaf_size_) / 2.0f,
586  cell_data.pt_on_surface.y () + static_cast<float> (leaf_size_) / 2.0f,
587  cell_data.pt_on_surface.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
588 
589  // Save the vector and the point on the surface
590  getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt);
591  getProjection (cell_data.pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface);
592 }
593 
594 //////////////////////////////////////////////////////////////////////////////////////////////
595 template <typename PointNT> void
596 pcl::GridProjection<PointNT>::storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &,
597  const Leaf &cell_data)
598 {
599  Eigen::Vector4f cell_center = cell_data.pt_on_surface;
600  Eigen::Vector4f grid_pt (
601  cell_center.x () - static_cast<float> (leaf_size_) / 2.0f,
602  cell_center.y () + static_cast<float> (leaf_size_) / 2.0f,
603  cell_center.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
604 
605  pcl::Indices k_indices;
606  k_indices.resize (k_);
607  std::vector <float> k_squared_distances;
608  k_squared_distances.resize (k_);
609 
610  PointNT pt; pt.x = grid_pt.x (); pt.y = grid_pt.y (); pt.z = grid_pt.z ();
611  tree_->nearestKSearch (pt, k_, k_indices, k_squared_distances);
612 
613  getVectorAtPointKNN (grid_pt, k_indices, k_squared_distances, cell_hash_map_[index_1d].vect_at_grid_pt);
614  getProjectionWithPlaneFit (cell_center, k_indices, cell_hash_map_[index_1d].pt_on_surface);
615 }
616 
617 //////////////////////////////////////////////////////////////////////////////////////////////
618 template <typename PointNT> bool
619 pcl::GridProjection<PointNT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
620 {
621  data_.reset (new pcl::PointCloud<PointNT> (*input_));
622  getBoundingBox ();
623 
624  // Store the point cloud data into the voxel grid, and at the same time
625  // create a hash map to store the information for each cell
626  cell_hash_map_.max_load_factor (2.0);
627  cell_hash_map_.rehash (data_->size () / static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
628 
629  // Go over all points and insert them into the right leaf
630  for (pcl::index_t cp = 0; cp < static_cast<pcl::index_t> (data_->size ()); ++cp)
631  {
632  // Check if the point is invalid
633  if (!std::isfinite ((*data_)[cp].x) ||
634  !std::isfinite ((*data_)[cp].y) ||
635  !std::isfinite ((*data_)[cp].z))
636  continue;
637 
638  Eigen::Vector3i index_3d;
639  getCellIndex ((*data_)[cp].getVector4fMap (), index_3d);
640  int index_1d = getIndexIn1D (index_3d);
641  if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
642  {
643  Leaf cell_data;
644  cell_data.data_indices.push_back (cp);
645  getCellCenterFromIndex (index_3d, cell_data.pt_on_surface);
646  cell_hash_map_[index_1d] = cell_data;
647  occupied_cell_list_[index_1d] = true;
648  }
649  else
650  {
651  Leaf cell_data = cell_hash_map_.at (index_1d);
652  cell_data.data_indices.push_back (cp);
653  cell_hash_map_[index_1d] = cell_data;
654  }
655  }
656 
657  Eigen::Vector3i index;
658 
659  for (int i = 0; i < data_size_; ++i)
660  {
661  for (int j = 0; j < data_size_; ++j)
662  {
663  for (int k = 0; k < data_size_; ++k)
664  {
665  index[0] = i;
666  index[1] = j;
667  index[2] = k;
668  if (occupied_cell_list_[getIndexIn1D (index)])
669  {
670  fillPad (index);
671  }
672  }
673  }
674  }
675 
676  // Update the hashtable and store the vector and point
677  for (const auto &entry : cell_hash_map_)
678  {
679  getIndexIn3D (entry.first, index);
680  pcl::Indices pt_union_indices;
681  getDataPtsUnion (index, pt_union_indices);
682 
683  // Needs at least 10 points (?)
684  // NOTE: set as parameter later
685  if (pt_union_indices.size () > 10)
686  {
687  storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
688  //storeVectAndSurfacePointKNN(entry.first, index, entry.second);
689  occupied_cell_list_[entry.first] = 1;
690  }
691  }
692 
693  // Go through the hash table another time to extract surface
694  for (const auto &entry : cell_hash_map_)
695  {
696  getIndexIn3D (entry.first, index);
697  pcl::Indices pt_union_indices;
698  getDataPtsUnion (index, pt_union_indices);
699 
700  // Needs at least 10 points (?)
701  // NOTE: set as parameter later
702  if (pt_union_indices.size () > 10)
703  createSurfaceForCell (index, pt_union_indices);
704  }
705 
706  polygons.resize (surface_.size () / 4);
707  // Copy the data from surface_ to polygons
708  for (int i = 0; i < static_cast<int> (polygons.size ()); ++i)
709  {
710  pcl::Vertices v;
711  v.vertices.resize (4);
712  for (int j = 0; j < 4; ++j)
713  v.vertices[j] = i*4+j;
714  polygons[i] = v;
715  }
716  return (true);
717 }
718 
719 //////////////////////////////////////////////////////////////////////////////////////////////
720 template <typename PointNT> void
722 {
723  if (!reconstructPolygons (output.polygons))
724  return;
725 
726  // The mesh surface is held in surface_. Copy it to the output format
727  output.header = input_->header;
728 
730  cloud.width = surface_.size ();
731  cloud.height = 1;
732  cloud.is_dense = true;
733 
734  cloud.resize (surface_.size ());
735  // Copy the data from surface_ to cloud
736  for (std::size_t i = 0; i < cloud.size (); ++i)
737  {
738  cloud[i].x = cloud_scale_factor_*surface_[i].x ();
739  cloud[i].y = cloud_scale_factor_*surface_[i].y ();
740  cloud[i].z = cloud_scale_factor_*surface_[i].z ();
741  }
742  pcl::toPCLPointCloud2 (cloud, output.cloud);
743 }
744 
745 //////////////////////////////////////////////////////////////////////////////////////////////
746 template <typename PointNT> void
748  std::vector<pcl::Vertices> &polygons)
749 {
750  if (!reconstructPolygons (polygons))
751  return;
752 
753  // The mesh surface is held in surface_. Copy it to the output format
754  points.header = input_->header;
755  points.width = surface_.size ();
756  points.height = 1;
757  points.is_dense = true;
758 
759  points.resize (surface_.size ());
760  // Copy the data from surface_ to cloud
761  for (std::size_t i = 0; i < points.size (); ++i)
762  {
763  points[i].x = cloud_scale_factor_*surface_[i].x ();
764  points[i].y = cloud_scale_factor_*surface_[i].y ();
765  points[i].z = cloud_scale_factor_*surface_[i].z ();
766  }
767 }
768 
769 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
770 
771 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
772 
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, bool padding)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:372
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