Point Cloud Library (PCL)  1.11.0
grid_projection.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_SURFACE_IMPL_GRID_PROJECTION_H_
39 #define PCL_SURFACE_IMPL_GRID_PROJECTION_H_
40 
41 #include <pcl/surface/grid_projection.h>
42 #include <pcl/common/common.h>
43 #include <pcl/common/centroid.h>
44 #include <pcl/common/vector_average.h>
45 #include <pcl/Vertices.h>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointNT>
50  cell_hash_map_ (), leaf_size_ (0.001), gaussian_scale_ (),
51  data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ ()
52 {}
53 
54 //////////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointNT>
57  cell_hash_map_ (), leaf_size_ (resolution), gaussian_scale_ (),
58  data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ ()
59 {}
60 
61 //////////////////////////////////////////////////////////////////////////////////////////////
62 template <typename PointNT>
64 {
65  vector_at_data_point_.clear ();
66  surface_.clear ();
67  cell_hash_map_.clear ();
68  occupied_cell_list_.clear ();
69  data_.reset ();
70 }
71 
72 //////////////////////////////////////////////////////////////////////////////////////////////
73 template <typename PointNT> void
75 {
76  for (std::size_t i = 0; i < data_->points.size(); ++i)
77  {
78  data_->points[i].x /= static_cast<float> (scale_factor);
79  data_->points[i].y /= static_cast<float> (scale_factor);
80  data_->points[i].z /= static_cast<float> (scale_factor);
81  }
82  max_p_ /= static_cast<float> (scale_factor);
83  min_p_ /= static_cast<float> (scale_factor);
84 }
85 
86 //////////////////////////////////////////////////////////////////////////////////////////////
87 template <typename PointNT> void
89 {
90  pcl::getMinMax3D (*data_, min_p_, max_p_);
91 
92  Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
93  double scale_factor = (std::max)((std::max)(bounding_box_size.x (),
94  bounding_box_size.y ()),
95  bounding_box_size.z ());
96  if (scale_factor > 1)
97  scaleInputDataPoint (scale_factor);
98 
99  // Round the max_p_, min_p_ so that they are aligned with the cells vertices
100  int upper_right_index[3];
101  int lower_left_index[3];
102  for (std::size_t i = 0; i < 3; ++i)
103  {
104  upper_right_index[i] = static_cast<int> (max_p_(i) / leaf_size_ + 5);
105  lower_left_index[i] = static_cast<int> (min_p_(i) / leaf_size_ - 5);
106  max_p_(i) = static_cast<float> (upper_right_index[i] * leaf_size_);
107  min_p_(i) = static_cast<float> (lower_left_index[i] * leaf_size_);
108  }
109  bounding_box_size = max_p_ - min_p_;
110  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
111  bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
112  double max_size =
113  (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
114  bounding_box_size.z ());
115 
116  data_size_ = static_cast<int> (max_size / leaf_size_);
117  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n",
118  min_p_.x (), min_p_.y (), min_p_.z ());
119  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n",
120  max_p_.x (), max_p_.y (), max_p_.z ());
121  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_);
122  PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_);
123  occupied_cell_list_.resize (data_size_ * data_size_ * data_size_);
124  gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
125 }
126 
127 //////////////////////////////////////////////////////////////////////////////////////////////
128 template <typename PointNT> void
130  const Eigen::Vector4f &cell_center,
131  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const
132 {
133  assert (pts.size () == 8);
134 
135  float sz = static_cast<float> (leaf_size_) / 2.0f;
136 
137  pts[0] = cell_center + Eigen::Vector4f (-sz, sz, -sz, 0);
138  pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0);
139  pts[2] = cell_center + Eigen::Vector4f (sz, -sz, -sz, 0);
140  pts[3] = cell_center + Eigen::Vector4f (sz, sz, -sz, 0);
141  pts[4] = cell_center + Eigen::Vector4f (-sz, sz, sz, 0);
142  pts[5] = cell_center + Eigen::Vector4f (-sz, -sz, sz, 0);
143  pts[6] = cell_center + Eigen::Vector4f (sz, -sz, sz, 0);
144  pts[7] = cell_center + Eigen::Vector4f (sz, sz, sz, 0);
145 }
146 
147 //////////////////////////////////////////////////////////////////////////////////////////////
148 template <typename PointNT> void
149 pcl::GridProjection<PointNT>::getDataPtsUnion (const Eigen::Vector3i &index,
150  std::vector <int> &pt_union_indices)
151 {
152  for (int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
153  {
154  for (int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
155  {
156  for (int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
157  {
158  Eigen::Vector3i cell_index_3d (i, j, k);
159  int cell_index_1d = getIndexIn1D (cell_index_3d);
160  if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
161  {
162  // Get the indices of the input points within the cell(i,j,k), which
163  // is stored in the hash map
164  pt_union_indices.insert (pt_union_indices.end (),
165  cell_hash_map_.at (cell_index_1d).data_indices.begin (),
166  cell_hash_map_.at (cell_index_1d).data_indices.end ());
167  }
168  }
169  }
170  }
171 }
172 
173 //////////////////////////////////////////////////////////////////////////////////////////////
174 template <typename PointNT> void
176  std::vector <int> &pt_union_indices)
177 {
178  // 8 vertices of the cell
179  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
180 
181  // 4 end points that shared by 3 edges connecting the upper left front points
182  Eigen::Vector4f pts[4];
183  Eigen::Vector3f vector_at_pts[4];
184 
185  // Given the index of cell, caluate the coordinates of the eight vertices of the cell
186  // index the index of the cell in (x,y,z) 3d format
187  Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
188  getCellCenterFromIndex (index, cell_center);
189  getVertexFromCellCenter (cell_center, vertices);
190 
191  // Get the indices of the cells which stores the 4 end points.
192  Eigen::Vector3i indices[4];
193  indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
194  indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
195  indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
196  indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
197 
198  // Get the coordinate of the 4 end points, and the corresponding vectors
199  for (std::size_t i = 0; i < 4; ++i)
200  {
201  pts[i] = vertices[I_SHIFT_PT[i]];
202  unsigned int index_1d = getIndexIn1D (indices[i]);
203  if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () ||
204  occupied_cell_list_[index_1d] == 0)
205  return;
206  vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
207  }
208 
209  // Go through the 3 edges, test whether they are intersected by the surface
210  for (std::size_t i = 0; i < 3; ++i)
211  {
212  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
213  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
214  for (std::size_t j = 0; j < 2; ++j)
215  {
216  end_pts[j] = pts[I_SHIFT_EDGE[i][j]];
217  vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]];
218  }
219 
220  if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
221  {
222  // Indices of cells that contains points which will be connected to
223  // create a polygon
224  Eigen::Vector3i polygon[4];
225  Eigen::Vector4f polygon_pts[4];
226  int polygon_indices_1d[4];
227  bool is_all_in_hash_map = true;
228  switch (i)
229  {
230  case 0:
231  polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
232  polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
233  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
234  polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
235  break;
236  case 1:
237  polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
238  polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
239  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
240  polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
241  break;
242  case 2:
243  polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
244  polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
245  polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
246  polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
247  break;
248  default:
249  break;
250  }
251  for (std::size_t k = 0; k < 4; k++)
252  {
253  polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
254  if (!occupied_cell_list_[polygon_indices_1d[k]])
255  {
256  is_all_in_hash_map = false;
257  break;
258  }
259  }
260  if (is_all_in_hash_map)
261  {
262  for (std::size_t k = 0; k < 4; k++)
263  {
264  polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
265  surface_.push_back (polygon_pts[k]);
266  }
267  }
268  }
269  }
270 }
271 
272 //////////////////////////////////////////////////////////////////////////////////////////////
273 template <typename PointNT> void
275  std::vector <int> &pt_union_indices, Eigen::Vector4f &projection)
276 {
277  const double projection_distance = leaf_size_ * 3;
278  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
279  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
280  end_pt[0] = p;
281  getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
282  end_pt_vect[0].normalize();
283 
284  double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
285 
286  // Find another point which is projection_distance away from the p, do a
287  // binary search between these two points, to find the projected point on the
288  // surface
289  if (dSecond > 0)
290  end_pt[1] = end_pt[0] + Eigen::Vector4f (
291  end_pt_vect[0][0] * static_cast<float> (projection_distance),
292  end_pt_vect[0][1] * static_cast<float> (projection_distance),
293  end_pt_vect[0][2] * static_cast<float> (projection_distance),
294  0.0f);
295  else
296  end_pt[1] = end_pt[0] - Eigen::Vector4f (
297  end_pt_vect[0][0] * static_cast<float> (projection_distance),
298  end_pt_vect[0][1] * static_cast<float> (projection_distance),
299  end_pt_vect[0][2] * static_cast<float> (projection_distance),
300  0.0f);
301  getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
302  if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
303  {
304  Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
305  findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
306  }
307  else
308  projection = p;
309 }
310 
311 //////////////////////////////////////////////////////////////////////////////////////////////
312 template <typename PointNT> void
314  std::vector<int> (&pt_union_indices),
315  Eigen::Vector4f &projection)
316 {
317  // Compute the plane coefficients
318  Eigen::Vector4f model_coefficients;
319  /// @remark iterative weighted least squares or sac might give better results
320  Eigen::Matrix3f covariance_matrix;
321  Eigen::Vector4f xyz_centroid;
322 
323  computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid);
324 
325  // Get the plane normal
326  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
327  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
328  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
329 
330  // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
331  model_coefficients[0] = eigen_vector [0];
332  model_coefficients[1] = eigen_vector [1];
333  model_coefficients[2] = eigen_vector [2];
334  model_coefficients[3] = 0;
335  // Hessian form (D = nc . p_plane (centroid here) + p)
336  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
337 
338  // Projected point
339  Eigen::Vector3f point (p.x (), p.y (), p.z ()); //= Eigen::Vector3f::MapAligned (&output.points[cp].x, 3);
340  float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
341  point -= distance * model_coefficients.head < 3 > ();
342 
343  projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
344 }
345 
346 //////////////////////////////////////////////////////////////////////////////////////////////
347 template <typename PointNT> void
349  std::vector <int> &pt_union_indices,
350  Eigen::Vector3f &vo)
351 {
352  std::vector <double> pt_union_dist (pt_union_indices.size ());
353  std::vector <double> pt_union_weight (pt_union_indices.size ());
354  Eigen::Vector3f out_vector (0, 0, 0);
355  double sum = 0.0;
356  double mag = 0.0;
357 
358  for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
359  {
360  Eigen::Vector4f pp (data_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[pt_union_indices[i]].z, 0);
361  pt_union_dist[i] = (pp - p).squaredNorm ();
362  pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
363  mag += pow (M_E, -pow (sqrt (pt_union_dist[i]), 2.0) / gaussian_scale_);
364  sum += pt_union_weight[i];
365  }
366 
367  pcl::VectorAverage3f vector_average;
368 
369  Eigen::Vector3f v (
370  data_->points[pt_union_indices[0]].normal[0],
371  data_->points[pt_union_indices[0]].normal[1],
372  data_->points[pt_union_indices[0]].normal[2]);
373 
374  for (std::size_t i = 0; i < pt_union_weight.size (); ++i)
375  {
376  pt_union_weight[i] /= sum;
377  Eigen::Vector3f vec (data_->points[pt_union_indices[i]].normal[0],
378  data_->points[pt_union_indices[i]].normal[1],
379  data_->points[pt_union_indices[i]].normal[2]);
380  if (vec.dot (v) < 0)
381  vec = -vec;
382  vector_average.add (vec, static_cast<float> (pt_union_weight[i]));
383  }
384  out_vector = vector_average.getMean ();
385  // vector_average.getEigenVector1(out_vector);
386 
387  out_vector.normalize ();
388  double d1 = getD1AtPoint (p, out_vector, pt_union_indices);
389  out_vector *= static_cast<float> (sum);
390  vo = ((d1 > 0) ? -1 : 1) * out_vector;
391 }
392 
393 //////////////////////////////////////////////////////////////////////////////////////////////
394 template <typename PointNT> void
396  std::vector <int> &k_indices,
397  std::vector <float> &k_squared_distances,
398  Eigen::Vector3f &vo)
399 {
400  Eigen::Vector3f out_vector (0, 0, 0);
401  std::vector <float> k_weight;
402  k_weight.resize (k_);
403  float sum = 0.0;
404  for (int i = 0; i < k_; i++)
405  {
406  //k_weight[i] = pow (M_E, -pow (k_squared_distances[i], 2.0) / gaussian_scale_);
407  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_));
408  sum += k_weight[i];
409  }
410  pcl::VectorAverage3f vector_average;
411  for (int i = 0; i < k_; i++)
412  {
413  k_weight[i] /= sum;
414  Eigen::Vector3f vec (data_->points[k_indices[i]].normal[0],
415  data_->points[k_indices[i]].normal[1],
416  data_->points[k_indices[i]].normal[2]);
417  vector_average.add (vec, k_weight[i]);
418  }
419  vector_average.getEigenVector1 (out_vector);
420  out_vector.normalize ();
421  double d1 = getD1AtPoint (p, out_vector, k_indices);
422  out_vector *= sum;
423  vo = ((d1 > 0) ? -1 : 1) * out_vector;
424 
425 }
426 
427 //////////////////////////////////////////////////////////////////////////////////////////////
428 template <typename PointNT> double
430  const std::vector <int> &pt_union_indices)
431 {
432  std::vector <double> pt_union_dist (pt_union_indices.size ());
433  std::vector <double> pt_union_weight (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_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[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 std::vector <int> &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 std::vector <int> &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  std::vector <int> &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  std::vector <int> &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  std::vector <int> &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  std::vector <int> 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_->points.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 (int cp = 0; cp < static_cast<int> (data_->points.size ()); ++cp)
631  {
632  // Check if the point is invalid
633  if (!std::isfinite (data_->points[cp].x) ||
634  !std::isfinite (data_->points[cp].y) ||
635  !std::isfinite (data_->points[cp].z))
636  continue;
637 
638  Eigen::Vector3i index_3d;
639  getCellIndex (data_->points[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] = 1;
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  int numOfFilledPad = 0;
659 
660  for (int i = 0; i < data_size_; ++i)
661  {
662  for (int j = 0; j < data_size_; ++j)
663  {
664  for (int k = 0; k < data_size_; ++k)
665  {
666  index[0] = i;
667  index[1] = j;
668  index[2] = k;
669  if (occupied_cell_list_[getIndexIn1D (index)])
670  {
671  fillPad (index);
672  numOfFilledPad++;
673  }
674  }
675  }
676  }
677 
678  // Update the hashtable and store the vector and point
679  for (const auto &entry : cell_hash_map_)
680  {
681  getIndexIn3D (entry.first, index);
682  std::vector <int> pt_union_indices;
683  getDataPtsUnion (index, pt_union_indices);
684 
685  // Needs at least 10 points (?)
686  // NOTE: set as parameter later
687  if (pt_union_indices.size () > 10)
688  {
689  storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
690  //storeVectAndSurfacePointKNN(entry.first, index, entry.second);
691  occupied_cell_list_[entry.first] = 1;
692  }
693  }
694 
695  // Go through the hash table another time to extract surface
696  for (const auto &entry : cell_hash_map_)
697  {
698  getIndexIn3D (entry.first, index);
699  std::vector <int> pt_union_indices;
700  getDataPtsUnion (index, pt_union_indices);
701 
702  // Needs at least 10 points (?)
703  // NOTE: set as parameter later
704  if (pt_union_indices.size () > 10)
705  createSurfaceForCell (index, pt_union_indices);
706  }
707 
708  polygons.resize (surface_.size () / 4);
709  // Copy the data from surface_ to polygons
710  for (int i = 0; i < static_cast<int> (polygons.size ()); ++i)
711  {
712  pcl::Vertices v;
713  v.vertices.resize (4);
714  for (int j = 0; j < 4; ++j)
715  v.vertices[j] = i*4+j;
716  polygons[i] = v;
717  }
718  return (true);
719 }
720 
721 //////////////////////////////////////////////////////////////////////////////////////////////
722 template <typename PointNT> void
724 {
725  if (!reconstructPolygons (output.polygons))
726  return;
727 
728  // The mesh surface is held in surface_. Copy it to the output format
729  output.header = input_->header;
730 
732  cloud.width = static_cast<std::uint32_t> (surface_.size ());
733  cloud.height = 1;
734  cloud.is_dense = true;
735 
736  cloud.points.resize (surface_.size ());
737  // Copy the data from surface_ to cloud
738  for (std::size_t i = 0; i < cloud.points.size (); ++i)
739  {
740  cloud.points[i].x = surface_[i].x ();
741  cloud.points[i].y = surface_[i].y ();
742  cloud.points[i].z = surface_[i].z ();
743  }
744  pcl::toPCLPointCloud2 (cloud, output.cloud);
745 }
746 
747 //////////////////////////////////////////////////////////////////////////////////////////////
748 template <typename PointNT> void
750  std::vector<pcl::Vertices> &polygons)
751 {
752  if (!reconstructPolygons (polygons))
753  return;
754 
755  // The mesh surface is held in surface_. Copy it to the output format
756  points.header = input_->header;
757  points.width = static_cast<std::uint32_t> (surface_.size ());
758  points.height = 1;
759  points.is_dense = true;
760 
761  points.resize (surface_.size ());
762  // Copy the data from surface_ to cloud
763  for (std::size_t i = 0; i < points.size (); ++i)
764  {
765  points[i].x = surface_[i].x ();
766  points[i].y = surface_[i].y ();
767  points[i].z = surface_[i].z ();
768  }
769 }
770 
771 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
772 
773 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
774 
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:418
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
std::size_t size() const
Definition: point_cloud.h:448
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:455
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:242
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:489
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
std::uint32_t uint32_t
Definition: types.h:58
const int I_SHIFT_PT[4]
const int I_SHIFT_EDGE[3][2]
#define M_E
Definition: pcl_macros.h:190
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