Point Cloud Library (PCL) 1.13.1
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 computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid);
323
324 // Get the plane normal
325 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
326 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
327 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
328
329 // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
330 model_coefficients[0] = eigen_vector [0];
331 model_coefficients[1] = eigen_vector [1];
332 model_coefficients[2] = eigen_vector [2];
333 model_coefficients[3] = 0;
334 // Hessian form (D = nc . p_plane (centroid here) + p)
335 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
336
337 // Projected point
338 Eigen::Vector3f point (p.x (), p.y (), p.z ()); //= Eigen::Vector3f::MapAligned (&output[cp].x, 3);
339 float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
340 point -= distance * model_coefficients.head < 3 > ();
341
342 projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
343}
344
345//////////////////////////////////////////////////////////////////////////////////////////////
346template <typename PointNT> void
348 pcl::Indices &pt_union_indices,
349 Eigen::Vector3f &vo)
350{
351 std::vector <double> pt_union_dist (pt_union_indices.size ());
352 std::vector <double> pt_union_weight (pt_union_indices.size ());
353 Eigen::Vector3f out_vector (0, 0, 0);
354 double sum = 0.0;
355
356 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
357 {
358 Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
359 pt_union_dist[i] = (pp - p).squaredNorm ();
360 pt_union_weight[i] = pow (M_E, -pow (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] = true;
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
655 for (int i = 0; i < data_size_; ++i)
656 {
657 for (int j = 0; j < data_size_; ++j)
658 {
659 for (int k = 0; k < data_size_; ++k)
660 {
661 index[0] = i;
662 index[1] = j;
663 index[2] = k;
664 if (occupied_cell_list_[getIndexIn1D (index)])
665 {
666 fillPad (index);
667 }
668 }
669 }
670 }
671
672 // Update the hashtable and store the vector and point
673 for (const auto &entry : cell_hash_map_)
674 {
675 getIndexIn3D (entry.first, index);
676 pcl::Indices pt_union_indices;
677 getDataPtsUnion (index, pt_union_indices);
678
679 // Needs at least 10 points (?)
680 // NOTE: set as parameter later
681 if (pt_union_indices.size () > 10)
682 {
683 storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
684 //storeVectAndSurfacePointKNN(entry.first, index, entry.second);
685 occupied_cell_list_[entry.first] = 1;
686 }
687 }
688
689 // Go through the hash table another time to extract surface
690 for (const auto &entry : cell_hash_map_)
691 {
692 getIndexIn3D (entry.first, index);
693 pcl::Indices pt_union_indices;
694 getDataPtsUnion (index, pt_union_indices);
695
696 // Needs at least 10 points (?)
697 // NOTE: set as parameter later
698 if (pt_union_indices.size () > 10)
699 createSurfaceForCell (index, pt_union_indices);
700 }
701
702 polygons.resize (surface_.size () / 4);
703 // Copy the data from surface_ to polygons
704 for (int i = 0; i < static_cast<int> (polygons.size ()); ++i)
705 {
707 v.vertices.resize (4);
708 for (int j = 0; j < 4; ++j)
709 v.vertices[j] = i*4+j;
710 polygons[i] = v;
711 }
712 return (true);
713}
714
715//////////////////////////////////////////////////////////////////////////////////////////////
716template <typename PointNT> void
718{
719 if (!reconstructPolygons (output.polygons))
720 return;
721
722 // The mesh surface is held in surface_. Copy it to the output format
723 output.header = input_->header;
724
726 cloud.width = surface_.size ();
727 cloud.height = 1;
728 cloud.is_dense = true;
729
730 cloud.resize (surface_.size ());
731 // Copy the data from surface_ to cloud
732 for (std::size_t i = 0; i < cloud.size (); ++i)
733 {
734 cloud[i].x = cloud_scale_factor_*surface_[i].x ();
735 cloud[i].y = cloud_scale_factor_*surface_[i].y ();
736 cloud[i].z = cloud_scale_factor_*surface_[i].z ();
737 }
738 pcl::toPCLPointCloud2 (cloud, output.cloud);
739}
740
741//////////////////////////////////////////////////////////////////////////////////////////////
742template <typename PointNT> void
744 std::vector<pcl::Vertices> &polygons)
745{
746 if (!reconstructPolygons (polygons))
747 return;
748
749 // The mesh surface is held in surface_. Copy it to the output format
750 points.header = input_->header;
751 points.width = surface_.size ();
752 points.height = 1;
753 points.is_dense = true;
754
755 points.resize (surface_.size ());
756 // Copy the data from surface_ to cloud
757 for (std::size_t i = 0; i < points.size (); ++i)
758 {
759 points[i].x = cloud_scale_factor_*surface_[i].x ();
760 points[i].y = cloud_scale_factor_*surface_[i].y ();
761 points[i].z = cloud_scale_factor_*surface_[i].z ();
762 }
763}
764
765#define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
766
767#endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
768
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:508
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