Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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//////////////////////////////////////////////////////////////////////////////////////////////
48template <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//////////////////////////////////////////////////////////////////////////////////////////////
55template <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//////////////////////////////////////////////////////////////////////////////////////////////
62template <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//////////////////////////////////////////////////////////////////////////////////////////////
73template <typename PointNT> void
75{
76 for (auto& point: *data_) {
77 point.getVector3fMap() /= static_cast<float> (scale_factor);
78 }
79 max_p_ /= static_cast<float> (scale_factor);
80 min_p_ /= static_cast<float> (scale_factor);
81}
82
83//////////////////////////////////////////////////////////////////////////////////////////////
84template <typename PointNT> void
86{
87 pcl::getMinMax3D (*data_, min_p_, max_p_);
88
89 Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
90 double scale_factor = (std::max)((std::max)(bounding_box_size.x (),
91 bounding_box_size.y ()),
92 bounding_box_size.z ());
93 if (scale_factor > 1)
94 scaleInputDataPoint (scale_factor);
95
96 // Round the max_p_, min_p_ so that they are aligned with the cells vertices
97 int upper_right_index[3];
98 int lower_left_index[3];
99 for (std::size_t i = 0; i < 3; ++i)
100 {
101 upper_right_index[i] = static_cast<int> (max_p_(i) / leaf_size_ + 5);
102 lower_left_index[i] = static_cast<int> (min_p_(i) / leaf_size_ - 5);
103 max_p_(i) = static_cast<float> (upper_right_index[i] * leaf_size_);
104 min_p_(i) = static_cast<float> (lower_left_index[i] * leaf_size_);
105 }
106 bounding_box_size = max_p_ - min_p_;
107 PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
108 bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
109 double max_size =
110 (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
111 bounding_box_size.z ());
112
113 data_size_ = static_cast<int> (max_size / leaf_size_);
114 PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n",
115 min_p_.x (), min_p_.y (), min_p_.z ());
116 PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n",
117 max_p_.x (), max_p_.y (), max_p_.z ());
118 PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_);
119 PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_);
120 occupied_cell_list_.resize (data_size_ * data_size_ * data_size_);
121 gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
122}
123
124//////////////////////////////////////////////////////////////////////////////////////////////
125template <typename PointNT> void
127 const Eigen::Vector4f &cell_center,
128 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const
129{
130 assert (pts.size () == 8);
131
132 float sz = static_cast<float> (leaf_size_) / 2.0f;
133
134 pts[0] = cell_center + Eigen::Vector4f (-sz, sz, -sz, 0);
135 pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0);
136 pts[2] = cell_center + Eigen::Vector4f (sz, -sz, -sz, 0);
137 pts[3] = cell_center + Eigen::Vector4f (sz, sz, -sz, 0);
138 pts[4] = cell_center + Eigen::Vector4f (-sz, sz, sz, 0);
139 pts[5] = cell_center + Eigen::Vector4f (-sz, -sz, sz, 0);
140 pts[6] = cell_center + Eigen::Vector4f (sz, -sz, sz, 0);
141 pts[7] = cell_center + Eigen::Vector4f (sz, sz, sz, 0);
142}
143
144//////////////////////////////////////////////////////////////////////////////////////////////
145template <typename PointNT> void
147 pcl::Indices &pt_union_indices)
148{
149 for (int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
150 {
151 for (int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
152 {
153 for (int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
154 {
155 Eigen::Vector3i cell_index_3d (i, j, k);
156 int cell_index_1d = getIndexIn1D (cell_index_3d);
157 if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
158 {
159 // Get the indices of the input points within the cell(i,j,k), which
160 // is stored in the hash map
161 pt_union_indices.insert (pt_union_indices.end (),
162 cell_hash_map_.at (cell_index_1d).data_indices.begin (),
163 cell_hash_map_.at (cell_index_1d).data_indices.end ());
164 }
165 }
166 }
167 }
168}
169
170//////////////////////////////////////////////////////////////////////////////////////////////
171template <typename PointNT> void
173 pcl::Indices &pt_union_indices)
174{
175 // 8 vertices of the cell
176 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
177
178 // 4 end points that shared by 3 edges connecting the upper left front points
179 Eigen::Vector4f pts[4];
180 Eigen::Vector3f vector_at_pts[4];
181
182 // Given the index of cell, caluate the coordinates of the eight vertices of the cell
183 // index the index of the cell in (x,y,z) 3d format
184 Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
185 getCellCenterFromIndex (index, cell_center);
186 getVertexFromCellCenter (cell_center, vertices);
187
188 // Get the indices of the cells which stores the 4 end points.
189 Eigen::Vector3i indices[4];
190 indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
191 indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
192 indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
193 indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
194
195 // Get the coordinate of the 4 end points, and the corresponding vectors
196 for (std::size_t i = 0; i < 4; ++i)
197 {
198 pts[i] = vertices[I_SHIFT_PT[i]];
199 unsigned int index_1d = getIndexIn1D (indices[i]);
200 if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () ||
201 occupied_cell_list_[index_1d] == 0)
202 return;
203 vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
204 }
205
206 // Go through the 3 edges, test whether they are intersected by the surface
207 for (std::size_t i = 0; i < 3; ++i)
208 {
209 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
210 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
211 for (std::size_t j = 0; j < 2; ++j)
212 {
213 end_pts[j] = pts[I_SHIFT_EDGE[i][j]];
214 vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]];
215 }
216
217 if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
218 {
219 // Indices of cells that contains points which will be connected to
220 // create a polygon
221 Eigen::Vector3i polygon[4];
222 Eigen::Vector4f polygon_pts[4];
223 int polygon_indices_1d[4];
224 bool is_all_in_hash_map = true;
225 switch (i)
226 {
227 case 0:
228 polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
229 polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
230 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
231 polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
232 break;
233 case 1:
234 polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
235 polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
236 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
237 polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
238 break;
239 case 2:
240 polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
241 polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
242 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
243 polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
244 break;
245 default:
246 break;
247 }
248 for (std::size_t k = 0; k < 4; k++)
249 {
250 polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
251 if (!occupied_cell_list_[polygon_indices_1d[k]])
252 {
253 is_all_in_hash_map = false;
254 break;
255 }
256 }
257 if (is_all_in_hash_map)
258 {
259 for (std::size_t k = 0; k < 4; k++)
260 {
261 polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
262 surface_.push_back (polygon_pts[k]);
263 }
264 }
265 }
266 }
267}
268
269//////////////////////////////////////////////////////////////////////////////////////////////
270template <typename PointNT> void
272 pcl::Indices &pt_union_indices, Eigen::Vector4f &projection)
273{
274 const double projection_distance = leaf_size_ * 3;
275 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
276 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
277 end_pt[0] = p;
278 getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
279 end_pt_vect[0].normalize();
280
281 double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
282
283 // Find another point which is projection_distance away from the p, do a
284 // binary search between these two points, to find the projected point on the
285 // surface
286 if (dSecond > 0)
287 end_pt[1] = end_pt[0] + Eigen::Vector4f (
288 end_pt_vect[0][0] * static_cast<float> (projection_distance),
289 end_pt_vect[0][1] * static_cast<float> (projection_distance),
290 end_pt_vect[0][2] * static_cast<float> (projection_distance),
291 0.0f);
292 else
293 end_pt[1] = end_pt[0] - Eigen::Vector4f (
294 end_pt_vect[0][0] * static_cast<float> (projection_distance),
295 end_pt_vect[0][1] * static_cast<float> (projection_distance),
296 end_pt_vect[0][2] * static_cast<float> (projection_distance),
297 0.0f);
298 getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
299 if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
300 {
301 Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
302 findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
303 }
304 else
305 projection = p;
306}
307
308//////////////////////////////////////////////////////////////////////////////////////////////
309template <typename PointNT> void
311 pcl::Indices (&pt_union_indices),
312 Eigen::Vector4f &projection)
313{
314 // Compute the plane coefficients
315 Eigen::Vector4f model_coefficients;
316 /// @remark iterative weighted least squares or sac might give better results
317 Eigen::Matrix3f covariance_matrix;
318 Eigen::Vector4f xyz_centroid;
319
320 computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid);
321
322 // Get the plane normal
323 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
324 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
325 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
326
327 // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
328 model_coefficients[0] = eigen_vector [0];
329 model_coefficients[1] = eigen_vector [1];
330 model_coefficients[2] = eigen_vector [2];
331 model_coefficients[3] = 0;
332 // Hessian form (D = nc . p_plane (centroid here) + p)
333 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
334
335 // Projected point
336 Eigen::Vector3f point (p.x (), p.y (), p.z ()); //= Eigen::Vector3f::MapAligned (&output[cp].x, 3);
337 float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
338 point -= distance * model_coefficients.head < 3 > ();
339
340 projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
341}
342
343//////////////////////////////////////////////////////////////////////////////////////////////
344template <typename PointNT> void
346 pcl::Indices &pt_union_indices,
347 Eigen::Vector3f &vo)
348{
349 std::vector <double> pt_union_dist (pt_union_indices.size ());
350 std::vector <double> pt_union_weight (pt_union_indices.size ());
351 Eigen::Vector3f out_vector (0, 0, 0);
352 double sum = 0.0;
353 double mag = 0.0;
354
355 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
356 {
357 Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
358 pt_union_dist[i] = (pp - p).squaredNorm ();
359 pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
360 mag += pow (M_E, -pow (sqrt (pt_union_dist[i]), 2.0) / gaussian_scale_);
361 sum += pt_union_weight[i];
362 }
363
364 pcl::VectorAverage3f vector_average;
365
366 Eigen::Vector3f v (
367 (*data_)[pt_union_indices[0]].normal[0],
368 (*data_)[pt_union_indices[0]].normal[1],
369 (*data_)[pt_union_indices[0]].normal[2]);
370
371 for (std::size_t i = 0; i < pt_union_weight.size (); ++i)
372 {
373 pt_union_weight[i] /= sum;
374 Eigen::Vector3f vec ((*data_)[pt_union_indices[i]].normal[0],
375 (*data_)[pt_union_indices[i]].normal[1],
376 (*data_)[pt_union_indices[i]].normal[2]);
377 if (vec.dot (v) < 0)
378 vec = -vec;
379 vector_average.add (vec, static_cast<float> (pt_union_weight[i]));
380 }
381 out_vector = vector_average.getMean ();
382 // vector_average.getEigenVector1(out_vector);
383
384 out_vector.normalize ();
385 double d1 = getD1AtPoint (p, out_vector, pt_union_indices);
386 out_vector *= static_cast<float> (sum);
387 vo = ((d1 > 0) ? -1 : 1) * out_vector;
388}
389
390//////////////////////////////////////////////////////////////////////////////////////////////
391template <typename PointNT> void
393 pcl::Indices &k_indices,
394 std::vector <float> &k_squared_distances,
395 Eigen::Vector3f &vo)
396{
397 Eigen::Vector3f out_vector (0, 0, 0);
398 std::vector <float> k_weight;
399 k_weight.resize (k_);
400 float sum = 0.0;
401 for (int i = 0; i < k_; i++)
402 {
403 //k_weight[i] = pow (M_E, -pow (k_squared_distances[i], 2.0) / gaussian_scale_);
404 k_weight[i] = std::pow (static_cast<float>(M_E), static_cast<float>(-std::pow (static_cast<float>(k_squared_distances[i]), 2.0f) / gaussian_scale_));
405 sum += k_weight[i];
406 }
407 pcl::VectorAverage3f vector_average;
408 for (int i = 0; i < k_; i++)
409 {
410 k_weight[i] /= sum;
411 Eigen::Vector3f vec ((*data_)[k_indices[i]].normal[0],
412 (*data_)[k_indices[i]].normal[1],
413 (*data_)[k_indices[i]].normal[2]);
414 vector_average.add (vec, k_weight[i]);
415 }
416 vector_average.getEigenVector1 (out_vector);
417 out_vector.normalize ();
418 double d1 = getD1AtPoint (p, out_vector, k_indices);
419 out_vector *= sum;
420 vo = ((d1 > 0) ? -1 : 1) * out_vector;
421
422}
423
424//////////////////////////////////////////////////////////////////////////////////////////////
425template <typename PointNT> double
427 const pcl::Indices &pt_union_indices)
428{
429 std::vector <double> pt_union_dist (pt_union_indices.size ());
430 double sum = 0.0;
431 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
432 {
433 Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
434 pt_union_dist[i] = (pp - p).norm ();
435 sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
436 }
437 return (sum);
438}
439
440//////////////////////////////////////////////////////////////////////////////////////////////
441template <typename PointNT> double
442pcl::GridProjection<PointNT>::getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
443 const pcl::Indices &pt_union_indices)
444{
445 double sz = 0.01 * leaf_size_;
446 Eigen::Vector3f v = vec * static_cast<float> (sz);
447
448 double forward = getMagAtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
449 double backward = getMagAtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
450 return ((forward - backward) / (0.02 * leaf_size_));
451}
452
453//////////////////////////////////////////////////////////////////////////////////////////////
454template <typename PointNT> double
455pcl::GridProjection<PointNT>::getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec,
456 const pcl::Indices &pt_union_indices)
457{
458 double sz = 0.01 * leaf_size_;
459 Eigen::Vector3f v = vec * static_cast<float> (sz);
460
461 double forward = getD1AtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
462 double backward = getD1AtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
463 return ((forward - backward) / (0.02 * leaf_size_));
464}
465
466//////////////////////////////////////////////////////////////////////////////////////////////
467template <typename PointNT> bool
468pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
469 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
470 pcl::Indices &pt_union_indices)
471{
472 assert (end_pts.size () == 2);
473 assert (vect_at_end_pts.size () == 2);
474
475 double length[2];
476 for (std::size_t i = 0; i < 2; ++i)
477 {
478 length[i] = vect_at_end_pts[i].norm ();
479 vect_at_end_pts[i].normalize ();
480 }
481 double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
482 if (dot_prod < 0)
483 {
484 double ratio = length[0] / (length[0] + length[1]);
485 Eigen::Vector4f start_pt =
486 end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
487 Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
488 findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
489
490 Eigen::Vector3f vec;
491 getVectorAtPoint (intersection_pt, pt_union_indices, vec);
492 vec.normalize ();
493
494 double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
495 if (d2 < 0)
496 return (true);
497 }
498 return (false);
499}
500
501//////////////////////////////////////////////////////////////////////////////////////////////
502template <typename PointNT> void
504 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
505 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
506 const Eigen::Vector4f &start_pt,
507 pcl::Indices &pt_union_indices,
508 Eigen::Vector4f &intersection)
509{
510 assert (end_pts.size () == 2);
511 assert (vect_at_end_pts.size () == 2);
512
513 Eigen::Vector3f vec;
514 getVectorAtPoint (start_pt, pt_union_indices, vec);
515 double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
516 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
517 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
518 if ((std::abs (d1) < 10e-3) || (level == max_binary_search_level_))
519 {
520 intersection = start_pt;
521 return;
522 }
523 vec.normalize ();
524 if (vec.dot (vect_at_end_pts[0]) < 0)
525 {
526 Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
527 new_end_pts[0] = end_pts[0];
528 new_end_pts[1] = start_pt;
529 new_vect_at_end_pts[0] = vect_at_end_pts[0];
530 new_vect_at_end_pts[1] = vec;
531 findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
532 return;
533 }
534 if (vec.dot (vect_at_end_pts[1]) < 0)
535 {
536 Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
537 new_end_pts[0] = start_pt;
538 new_end_pts[1] = end_pts[1];
539 new_vect_at_end_pts[0] = vec;
540 new_vect_at_end_pts[1] = vect_at_end_pts[1];
541 findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
542 return;
543 }
544 intersection = start_pt;
545 return;
546}
547
548
549//////////////////////////////////////////////////////////////////////////////////////////////
550template <typename PointNT> void
551pcl::GridProjection<PointNT>::fillPad (const Eigen::Vector3i &index)
552{
553 for (int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
554 {
555 for (int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
556 {
557 for (int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
558 {
559 Eigen::Vector3i cell_index_3d (i, j, k);
560 unsigned int cell_index_1d = getIndexIn1D (cell_index_3d);
561 if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ())
562 {
563 cell_hash_map_[cell_index_1d].data_indices.resize (0);
564 getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface);
565 }
566 }
567 }
568 }
569}
570
571
572//////////////////////////////////////////////////////////////////////////////////////////////
573template <typename PointNT> void
575 const Eigen::Vector3i &,
576 pcl::Indices &pt_union_indices,
577 const Leaf &cell_data)
578{
579 // Get point on grid
580 Eigen::Vector4f grid_pt (
581 cell_data.pt_on_surface.x () - static_cast<float> (leaf_size_) / 2.0f,
582 cell_data.pt_on_surface.y () + static_cast<float> (leaf_size_) / 2.0f,
583 cell_data.pt_on_surface.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
584
585 // Save the vector and the point on the surface
586 getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt);
587 getProjection (cell_data.pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface);
588}
589
590//////////////////////////////////////////////////////////////////////////////////////////////
591template <typename PointNT> void
593 const Leaf &cell_data)
594{
595 Eigen::Vector4f cell_center = cell_data.pt_on_surface;
596 Eigen::Vector4f grid_pt (
597 cell_center.x () - static_cast<float> (leaf_size_) / 2.0f,
598 cell_center.y () + static_cast<float> (leaf_size_) / 2.0f,
599 cell_center.z () + static_cast<float> (leaf_size_) / 2.0f, 0.0f);
600
601 pcl::Indices k_indices;
602 k_indices.resize (k_);
603 std::vector <float> k_squared_distances;
604 k_squared_distances.resize (k_);
605
606 PointNT pt; pt.x = grid_pt.x (); pt.y = grid_pt.y (); pt.z = grid_pt.z ();
607 tree_->nearestKSearch (pt, k_, k_indices, k_squared_distances);
608
609 getVectorAtPointKNN (grid_pt, k_indices, k_squared_distances, cell_hash_map_[index_1d].vect_at_grid_pt);
610 getProjectionWithPlaneFit (cell_center, k_indices, cell_hash_map_[index_1d].pt_on_surface);
611}
612
613//////////////////////////////////////////////////////////////////////////////////////////////
614template <typename PointNT> bool
615pcl::GridProjection<PointNT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
616{
617 data_.reset (new pcl::PointCloud<PointNT> (*input_));
618 getBoundingBox ();
619
620 // Store the point cloud data into the voxel grid, and at the same time
621 // create a hash map to store the information for each cell
622 cell_hash_map_.max_load_factor (2.0);
623 cell_hash_map_.rehash (data_->size () / static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
624
625 // Go over all points and insert them into the right leaf
626 for (pcl::index_t cp = 0; cp < static_cast<pcl::index_t> (data_->size ()); ++cp)
627 {
628 // Check if the point is invalid
629 if (!std::isfinite ((*data_)[cp].x) ||
630 !std::isfinite ((*data_)[cp].y) ||
631 !std::isfinite ((*data_)[cp].z))
632 continue;
633
634 Eigen::Vector3i index_3d;
635 getCellIndex ((*data_)[cp].getVector4fMap (), index_3d);
636 int index_1d = getIndexIn1D (index_3d);
637 if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
638 {
639 Leaf cell_data;
640 cell_data.data_indices.push_back (cp);
641 getCellCenterFromIndex (index_3d, cell_data.pt_on_surface);
642 cell_hash_map_[index_1d] = cell_data;
643 occupied_cell_list_[index_1d] = 1;
644 }
645 else
646 {
647 Leaf cell_data = cell_hash_map_.at (index_1d);
648 cell_data.data_indices.push_back (cp);
649 cell_hash_map_[index_1d] = cell_data;
650 }
651 }
652
653 Eigen::Vector3i index;
654 int numOfFilledPad = 0;
655
656 for (int i = 0; i < data_size_; ++i)
657 {
658 for (int j = 0; j < data_size_; ++j)
659 {
660 for (int k = 0; k < data_size_; ++k)
661 {
662 index[0] = i;
663 index[1] = j;
664 index[2] = k;
665 if (occupied_cell_list_[getIndexIn1D (index)])
666 {
667 fillPad (index);
668 numOfFilledPad++;
669 }
670 }
671 }
672 }
673
674 // Update the hashtable and store the vector and point
675 for (const auto &entry : cell_hash_map_)
676 {
677 getIndexIn3D (entry.first, index);
678 pcl::Indices pt_union_indices;
679 getDataPtsUnion (index, pt_union_indices);
680
681 // Needs at least 10 points (?)
682 // NOTE: set as parameter later
683 if (pt_union_indices.size () > 10)
684 {
685 storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
686 //storeVectAndSurfacePointKNN(entry.first, index, entry.second);
687 occupied_cell_list_[entry.first] = 1;
688 }
689 }
690
691 // Go through the hash table another time to extract surface
692 for (const auto &entry : cell_hash_map_)
693 {
694 getIndexIn3D (entry.first, index);
695 pcl::Indices pt_union_indices;
696 getDataPtsUnion (index, pt_union_indices);
697
698 // Needs at least 10 points (?)
699 // NOTE: set as parameter later
700 if (pt_union_indices.size () > 10)
701 createSurfaceForCell (index, pt_union_indices);
702 }
703
704 polygons.resize (surface_.size () / 4);
705 // Copy the data from surface_ to polygons
706 for (int i = 0; i < static_cast<int> (polygons.size ()); ++i)
707 {
709 v.vertices.resize (4);
710 for (int j = 0; j < 4; ++j)
711 v.vertices[j] = i*4+j;
712 polygons[i] = v;
713 }
714 return (true);
715}
716
717//////////////////////////////////////////////////////////////////////////////////////////////
718template <typename PointNT> void
720{
721 if (!reconstructPolygons (output.polygons))
722 return;
723
724 // The mesh surface is held in surface_. Copy it to the output format
725 output.header = input_->header;
726
728 cloud.width = surface_.size ();
729 cloud.height = 1;
730 cloud.is_dense = true;
731
732 cloud.resize (surface_.size ());
733 // Copy the data from surface_ to cloud
734 for (std::size_t i = 0; i < cloud.size (); ++i)
735 {
736 cloud[i].x = surface_[i].x ();
737 cloud[i].y = surface_[i].y ();
738 cloud[i].z = surface_[i].z ();
739 }
740 pcl::toPCLPointCloud2 (cloud, output.cloud);
741}
742
743//////////////////////////////////////////////////////////////////////////////////////////////
744template <typename PointNT> void
746 std::vector<pcl::Vertices> &polygons)
747{
748 if (!reconstructPolygons (polygons))
749 return;
750
751 // The mesh surface is held in surface_. Copy it to the output format
752 points.header = input_->header;
753 points.width = surface_.size ();
754 points.height = 1;
755 points.is_dense = true;
756
757 points.resize (surface_.size ());
758 // Copy the data from surface_ to cloud
759 for (std::size_t i = 0; i < points.size (); ++i)
760 {
761 points[i].x = surface_[i].x ();
762 points[i].y = surface_[i].y ();
763 points[i].z = surface_[i].z ();
764 }
765}
766
767#define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
768
769#endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
770
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()
Destructor.
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 vectices 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.
void getVectorAtPoint(const Eigen::Vector4f &p, pcl::Indices &pt_union_indices, Eigen::Vector3f &vo)
Given the location of a point, get it's vector.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
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:485
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition eigen.hpp:296
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
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:19
std::vector< ::pcl::Vertices > polygons
Definition PolygonMesh.h:23
::pcl::PCLPointCloud2 cloud
Definition PolygonMesh.h:21
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:19