Point Cloud Library (PCL)  1.9.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  for (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 (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 (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 (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 (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 (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 (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 (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 (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 (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 (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  BOOST_FOREACH (typename HashMap::value_type 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  BOOST_FOREACH (typename HashMap::value_type 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<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 (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<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 (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 
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.
~GridProjection()
Destructor.
void getEigenVector1(Eigen::Matrix< real, dimension, 1 > &eigen_vector1) const
Get the eigenvector corresponding to the smallest eigenvalue.
void getCellIndex(const Eigen::Vector4f &p, Eigen::Vector3i &index) const
Get the 3d index (x,y,z) of the cell based on the location of the cell.
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...
double getD1AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 1st derivative.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:426
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...
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:483
void getCellCenterFromIndex(const Eigen::Vector3i &index, Eigen::Vector4f &center) const
Given the 3d index (x, y, z) of the cell, get the coordinates of the cell center. ...
std::vector< uint32_t > vertices
Definition: Vertices.h:19
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:471
void scaleInputDataPoint(double scale_factor)
When the input data points don&#39;t fill into the 1*1*1 box, scale them so that they can be filled in th...
std::size_t size() const
Definition: point_cloud.h:464
const int I_SHIFT_PT[4]
void add(const Eigen::Matrix< real, dimension, 1 > &sample, real weight=1.0)
Add a new sample.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:431
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:14
Define standard C methods and C++ classes that are common to all methods.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:429
::pcl::PCLHeader header
Definition: PolygonMesh.h:20
int getIndexIn1D(const Eigen::Vector3i &index) const
Given an index (x, y, z) in 3d, translate it into the index in 1d.
Eigen::Vector4f pt_on_surface
const Eigen::Matrix< real, dimension, 1 > & getMean() const
Get the mean of the added vectors.
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.
std::vector< int > data_indices
void getBoundingBox()
Get the bounding box for the input data points, also calculating the cell size, and the gaussian scal...
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
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
void createSurfaceForCell(const Eigen::Vector3i &index, std::vector< int > &pt_union_indices)
Given the index of a cell, exam it&#39;s up, left, front edges, and add the vectices to m_surface list...
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 getIndexIn3D(int index_1d, Eigen::Vector3i &index_3d) const
Given an index in 1d, translate it into the index (x, y, z) in 3d.
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:24
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:423
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:253
void getDataPtsUnion(const Eigen::Vector3i &index, std::vector< int > &pt_union_indices)
Obtain the index of a cell and the pad size.
double getMagAtPoint(const Eigen::Vector4f &p, const std::vector< int > &pt_union_indices)
Get the magnitude of the vector by summing up the distance.
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.
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:22
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
void performReconstruction(pcl::PolygonMesh &output) override
Create the surface.
double getD2AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 2nd derivative.
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:434
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&#39;s vector.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
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.
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...
const int I_SHIFT_EDGE[3][2]
bool reconstructPolygons(std::vector< pcl::Vertices > &polygons)
The actual surface reconstruction method.
Calculates the weighted average and the covariance matrix.
KdTreePtr tree_
A pointer to the spatial search object.
void getVectorAtPoint(const Eigen::Vector4f &p, std::vector< int > &pt_union_indices, Eigen::Vector3f &vo)
Given the location of a point, get it&#39;s vector.
GridProjection()
Constructor.
Define methods for centroid estimation and covariance matrix calculus.