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