38 #ifndef PCL_SURFACE_IMPL_GRID_PROJECTION_H_
39 #define PCL_SURFACE_IMPL_GRID_PROJECTION_H_
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>
48 template <
typename Po
intNT>
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_ ()
55 template <
typename Po
intNT>
57 cell_hash_map_ (), leaf_size_ (resolution), gaussian_scale_ (),
58 data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ ()
62 template <
typename Po
intNT>
65 vector_at_data_point_.clear ();
67 cell_hash_map_.clear ();
68 occupied_cell_list_.clear ();
73 template <
typename Po
intNT>
void
76 for (std::size_t i = 0; i < data_->points.size(); ++i)
78 data_->points[i].x /=
static_cast<float> (scale_factor);
79 data_->points[i].y /=
static_cast<float> (scale_factor);
80 data_->points[i].z /=
static_cast<float> (scale_factor);
82 max_p_ /=
static_cast<float> (scale_factor);
83 min_p_ /=
static_cast<float> (scale_factor);
87 template <
typename Po
intNT>
void
92 Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
93 double scale_factor = (std::max)((std::max)(bounding_box_size.x (),
94 bounding_box_size.y ()),
95 bounding_box_size.z ());
97 scaleInputDataPoint (scale_factor);
100 int upper_right_index[3];
101 int lower_left_index[3];
102 for (std::size_t i = 0; i < 3; ++i)
104 upper_right_index[i] =
static_cast<int> (max_p_(i) / leaf_size_ + 5);
105 lower_left_index[i] =
static_cast<int> (min_p_(i) / leaf_size_ - 5);
106 max_p_(i) =
static_cast<float> (upper_right_index[i] * leaf_size_);
107 min_p_(i) =
static_cast<float> (lower_left_index[i] * leaf_size_);
109 bounding_box_size = max_p_ - min_p_;
110 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
111 bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
113 (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
114 bounding_box_size.z ());
116 data_size_ =
static_cast<int> (max_size / leaf_size_);
117 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n",
118 min_p_.x (), min_p_.y (), min_p_.z ());
119 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n",
120 max_p_.x (), max_p_.y (), max_p_.z ());
121 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_);
122 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_);
123 occupied_cell_list_.resize (data_size_ * data_size_ * data_size_);
124 gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
128 template <
typename Po
intNT>
void
130 const Eigen::Vector4f &cell_center,
131 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts)
const
133 assert (pts.size () == 8);
135 float sz =
static_cast<float> (leaf_size_) / 2.0f;
137 pts[0] = cell_center + Eigen::Vector4f (-sz, sz, -sz, 0);
138 pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0);
139 pts[2] = cell_center + Eigen::Vector4f (sz, -sz, -sz, 0);
140 pts[3] = cell_center + Eigen::Vector4f (sz, sz, -sz, 0);
141 pts[4] = cell_center + Eigen::Vector4f (-sz, sz, sz, 0);
142 pts[5] = cell_center + Eigen::Vector4f (-sz, -sz, sz, 0);
143 pts[6] = cell_center + Eigen::Vector4f (sz, -sz, sz, 0);
144 pts[7] = cell_center + Eigen::Vector4f (sz, sz, sz, 0);
148 template <
typename Po
intNT>
void
150 std::vector <int> &pt_union_indices)
152 for (
int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
154 for (
int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
156 for (
int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
158 Eigen::Vector3i cell_index_3d (i, j, k);
159 int cell_index_1d = getIndexIn1D (cell_index_3d);
160 if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
164 pt_union_indices.insert (pt_union_indices.end (),
165 cell_hash_map_.at (cell_index_1d).data_indices.begin (),
166 cell_hash_map_.at (cell_index_1d).data_indices.end ());
174 template <
typename Po
intNT>
void
176 std::vector <int> &pt_union_indices)
179 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
182 Eigen::Vector4f pts[4];
183 Eigen::Vector3f vector_at_pts[4];
187 Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
188 getCellCenterFromIndex (index, cell_center);
189 getVertexFromCellCenter (cell_center, vertices);
192 Eigen::Vector3i indices[4];
193 indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
194 indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
195 indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
196 indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
199 for (std::size_t i = 0; i < 4; ++i)
202 unsigned int index_1d = getIndexIn1D (indices[i]);
203 if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () ||
204 occupied_cell_list_[index_1d] == 0)
206 vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
210 for (std::size_t i = 0; i < 3; ++i)
212 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
213 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
214 for (std::size_t j = 0; j < 2; ++j)
217 vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]];
220 if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
224 Eigen::Vector3i polygon[4];
225 Eigen::Vector4f polygon_pts[4];
226 int polygon_indices_1d[4];
227 bool is_all_in_hash_map =
true;
231 polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
232 polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
233 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
234 polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
237 polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
238 polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
239 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
240 polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
243 polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
244 polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
245 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
246 polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
251 for (std::size_t k = 0; k < 4; k++)
253 polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
254 if (!occupied_cell_list_[polygon_indices_1d[k]])
256 is_all_in_hash_map =
false;
260 if (is_all_in_hash_map)
262 for (std::size_t k = 0; k < 4; k++)
264 polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
265 surface_.push_back (polygon_pts[k]);
273 template <
typename Po
intNT>
void
275 std::vector <int> &pt_union_indices, Eigen::Vector4f &projection)
277 const double projection_distance = leaf_size_ * 3;
278 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
279 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
281 getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
282 end_pt_vect[0].normalize();
284 double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
290 end_pt[1] = end_pt[0] + Eigen::Vector4f (
291 end_pt_vect[0][0] * static_cast<float> (projection_distance),
292 end_pt_vect[0][1] * static_cast<float> (projection_distance),
293 end_pt_vect[0][2] * static_cast<float> (projection_distance),
296 end_pt[1] = end_pt[0] - Eigen::Vector4f (
297 end_pt_vect[0][0] * static_cast<float> (projection_distance),
298 end_pt_vect[0][1] * static_cast<float> (projection_distance),
299 end_pt_vect[0][2] * static_cast<float> (projection_distance),
301 getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
302 if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
304 Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
305 findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
312 template <
typename Po
intNT>
void
314 std::vector<int> (&pt_union_indices),
315 Eigen::Vector4f &projection)
318 Eigen::Vector4f model_coefficients;
320 Eigen::Matrix3f covariance_matrix;
321 Eigen::Vector4f xyz_centroid;
326 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
327 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
328 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
331 model_coefficients[0] = eigen_vector [0];
332 model_coefficients[1] = eigen_vector [1];
333 model_coefficients[2] = eigen_vector [2];
334 model_coefficients[3] = 0;
336 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
339 Eigen::Vector3f point (p.x (), p.y (), p.z ());
340 float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
341 point -= distance * model_coefficients.head < 3 > ();
343 projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
347 template <
typename Po
intNT>
void
349 std::vector <int> &pt_union_indices,
352 std::vector <double> pt_union_dist (pt_union_indices.size ());
353 std::vector <double> pt_union_weight (pt_union_indices.size ());
354 Eigen::Vector3f out_vector (0, 0, 0);
358 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
360 Eigen::Vector4f pp (data_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[pt_union_indices[i]].z, 0);
361 pt_union_dist[i] = (pp - p).squaredNorm ();
362 pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
363 mag += pow (M_E, -pow (sqrt (pt_union_dist[i]), 2.0) / gaussian_scale_);
364 sum += pt_union_weight[i];
370 data_->points[pt_union_indices[0]].normal[0],
371 data_->points[pt_union_indices[0]].normal[1],
372 data_->points[pt_union_indices[0]].normal[2]);
374 for (std::size_t i = 0; i < pt_union_weight.size (); ++i)
376 pt_union_weight[i] /= sum;
377 Eigen::Vector3f vec (data_->points[pt_union_indices[i]].normal[0],
378 data_->points[pt_union_indices[i]].normal[1],
379 data_->points[pt_union_indices[i]].normal[2]);
382 vector_average.
add (vec, static_cast<float> (pt_union_weight[i]));
384 out_vector = vector_average.
getMean ();
387 out_vector.normalize ();
388 double d1 = getD1AtPoint (p, out_vector, pt_union_indices);
389 out_vector *=
static_cast<float> (sum);
390 vo = ((d1 > 0) ? -1 : 1) * out_vector;
394 template <
typename Po
intNT>
void
396 std::vector <int> &k_indices,
397 std::vector <float> &k_squared_distances,
400 Eigen::Vector3f out_vector (0, 0, 0);
401 std::vector <float> k_weight;
402 k_weight.resize (k_);
404 for (
int i = 0; i < k_; i++)
407 k_weight[i] = std::pow (static_cast<float>(M_E), static_cast<float>(-std::pow (static_cast<float>(k_squared_distances[i]), 2.0f) / gaussian_scale_));
411 for (
int i = 0; i < k_; i++)
414 Eigen::Vector3f vec (data_->points[k_indices[i]].normal[0],
415 data_->points[k_indices[i]].normal[1],
416 data_->points[k_indices[i]].normal[2]);
417 vector_average.
add (vec, k_weight[i]);
420 out_vector.normalize ();
421 double d1 = getD1AtPoint (p, out_vector, k_indices);
423 vo = ((d1 > 0) ? -1 : 1) * out_vector;
428 template <
typename Po
intNT>
double
430 const std::vector <int> &pt_union_indices)
432 std::vector <double> pt_union_dist (pt_union_indices.size ());
433 std::vector <double> pt_union_weight (pt_union_indices.size ());
435 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
437 Eigen::Vector4f pp (data_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[pt_union_indices[i]].z, 0);
438 pt_union_dist[i] = (pp - p).norm ();
439 sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
445 template <
typename Po
intNT>
double
447 const std::vector <int> &pt_union_indices)
449 double sz = 0.01 * leaf_size_;
450 Eigen::Vector3f v = vec *
static_cast<float> (sz);
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_));
458 template <
typename Po
intNT>
double
460 const std::vector <int> &pt_union_indices)
462 double sz = 0.01 * leaf_size_;
463 Eigen::Vector3f v = vec *
static_cast<float> (sz);
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_));
471 template <
typename Po
intNT>
bool
473 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
474 std::vector <int> &pt_union_indices)
476 assert (end_pts.size () == 2);
477 assert (vect_at_end_pts.size () == 2);
480 for (std::size_t i = 0; i < 2; ++i)
482 length[i] = vect_at_end_pts[i].norm ();
483 vect_at_end_pts[i].normalize ();
485 double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
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);
495 getVectorAtPoint (intersection_pt, pt_union_indices, vec);
498 double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
506 template <
typename Po
intNT>
void
508 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
509 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
510 const Eigen::Vector4f &start_pt,
511 std::vector <int> &pt_union_indices,
512 Eigen::Vector4f &intersection)
514 assert (end_pts.size () == 2);
515 assert (vect_at_end_pts.size () == 2);
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_))
524 intersection = start_pt;
528 if (vec.dot (vect_at_end_pts[0]) < 0)
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);
538 if (vec.dot (vect_at_end_pts[1]) < 0)
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);
548 intersection = start_pt;
554 template <
typename Po
intNT>
void
557 for (
int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
559 for (
int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
561 for (
int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
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 ())
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);
577 template <
typename Po
intNT>
void
579 const Eigen::Vector3i &,
580 std::vector <int> &pt_union_indices,
581 const Leaf &cell_data)
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);
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);
595 template <
typename Po
intNT>
void
597 const Leaf &cell_data)
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);
605 std::vector <int> k_indices;
606 k_indices.resize (k_);
607 std::vector <float> k_squared_distances;
608 k_squared_distances.resize (k_);
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);
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);
618 template <
typename Po
intNT>
bool
626 cell_hash_map_.max_load_factor (2.0);
627 cell_hash_map_.rehash (data_->points.size () /
static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
630 for (
int cp = 0; cp < static_cast<int> (data_->points.size ()); ++cp)
633 if (!std::isfinite (data_->points[cp].x) ||
634 !std::isfinite (data_->points[cp].y) ||
635 !std::isfinite (data_->points[cp].z))
638 Eigen::Vector3i index_3d;
639 getCellIndex (data_->points[cp].getVector4fMap (), index_3d);
640 int index_1d = getIndexIn1D (index_3d);
641 if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
646 cell_hash_map_[index_1d] = cell_data;
647 occupied_cell_list_[index_1d] = 1;
651 Leaf cell_data = cell_hash_map_.at (index_1d);
653 cell_hash_map_[index_1d] = cell_data;
657 Eigen::Vector3i index;
658 int numOfFilledPad = 0;
660 for (
int i = 0; i < data_size_; ++i)
662 for (
int j = 0; j < data_size_; ++j)
664 for (
int k = 0; k < data_size_; ++k)
669 if (occupied_cell_list_[getIndexIn1D (index)])
679 for (
const auto &entry : cell_hash_map_)
681 getIndexIn3D (entry.first, index);
682 std::vector <int> pt_union_indices;
683 getDataPtsUnion (index, pt_union_indices);
687 if (pt_union_indices.size () > 10)
689 storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
691 occupied_cell_list_[entry.first] = 1;
696 for (
const auto &entry : cell_hash_map_)
698 getIndexIn3D (entry.first, index);
699 std::vector <int> pt_union_indices;
700 getDataPtsUnion (index, pt_union_indices);
704 if (pt_union_indices.size () > 10)
705 createSurfaceForCell (index, pt_union_indices);
708 polygons.resize (surface_.size () / 4);
710 for (
int i = 0; i < static_cast<int> (polygons.size ()); ++i)
714 for (
int j = 0; j < 4; ++j)
722 template <
typename Po
intNT>
void
725 if (!reconstructPolygons (output.
polygons))
729 output.
header = input_->header;
732 cloud.
width =
static_cast<std::uint32_t
> (surface_.size ());
736 cloud.
points.resize (surface_.size ());
738 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
740 cloud.
points[i].x = surface_[i].x ();
741 cloud.
points[i].y = surface_[i].y ();
742 cloud.
points[i].z = surface_[i].z ();
748 template <
typename Po
intNT>
void
750 std::vector<pcl::Vertices> &polygons)
752 if (!reconstructPolygons (polygons))
756 points.
header = input_->header;
757 points.
width =
static_cast<std::uint32_t
> (surface_.size ());
761 points.
resize (surface_.size ());
763 for (std::size_t i = 0; i < points.
size (); ++i)
765 points[i].x = surface_[i].x ();
766 points[i].y = surface_[i].y ();
767 points[i].z = surface_[i].z ();
771 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
773 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
~GridProjection()
Destructor.
std::vector< std::uint32_t > vertices
bool isIntersected(const std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &end_pts, std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &vect_at_end_pts, std::vector< int > &pt_union_indices)
Test whether the edge is intersected by the surface by doing the dot product of the vector at two end...
double getD1AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 1st derivative.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void getProjection(const Eigen::Vector4f &p, std::vector< int > &pt_union_indices, Eigen::Vector4f &projection)
Given the coordinates of one point, project it onto the surface, return the projected point...
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
void resize(std::size_t n)
Resize the cloud.
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...
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Eigen::Vector4f pt_on_surface
void storeVectAndSurfacePointKNN(int index_1d, const Eigen::Vector3i &index_3d, const Leaf &cell_data)
Go through all the entries in the hash table and update the cellData.
std::vector< int > data_indices
void getBoundingBox()
Get the bounding box for the input data points, also calculating the cell size, and the gaussian scal...
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
void getEigenVector1(VectorType &eigen_vector1) const
Get the eigenvector corresponding to the smallest eigenvalue.
void createSurfaceForCell(const Eigen::Vector3i &index, std::vector< int > &pt_union_indices)
Given the index of a cell, exam it's up, left, front edges, and add the vectices to m_surface list...
std::uint32_t height
The point cloud height (if organized as an image-structure).
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...
std::vector< ::pcl::Vertices > polygons
const VectorType & getMean() const
Get the mean of the added vectors.
pcl::PCLHeader header
The point cloud header.
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...
void getDataPtsUnion(const Eigen::Vector3i &index, std::vector< int > &pt_union_indices)
Obtain the index of a cell and the pad size.
double getMagAtPoint(const Eigen::Vector4f &p, const std::vector< int > &pt_union_indices)
Get the magnitude of the vector by summing up the distance.
void storeVectAndSurfacePoint(int index_1d, const Eigen::Vector3i &index_3d, std::vector< int > &pt_union_indices, const Leaf &cell_data)
Go through all the entries in the hash table and update the cellData.
::pcl::PCLPointCloud2 cloud
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob.
void performReconstruction(pcl::PolygonMesh &output) override
Create the surface.
double getD2AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 2nd derivative.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
void getVectorAtPointKNN(const Eigen::Vector4f &p, std::vector< int > &k_indices, std::vector< float > &k_squared_distances, Eigen::Vector3f &vo)
Given the location of a point, get it's vector.
void findIntersection(int level, const std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &end_pts, const std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &vect_at_end_pts, const Eigen::Vector4f &start_pt, std::vector< int > &pt_union_indices, Eigen::Vector4f &intersection)
Find point where the edge intersects the surface.
void 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 getProjectionWithPlaneFit(const Eigen::Vector4f &p, std::vector< int > &pt_union_indices, Eigen::Vector4f &projection)
Given the coordinates of one point, project it onto the surface, return the projected point...
void add(const VectorType &sample, real weight=1.0)
Add a new sample.
const int I_SHIFT_EDGE[3][2]
bool reconstructPolygons(std::vector< pcl::Vertices > &polygons)
The actual surface reconstruction method.
Calculates the weighted average and the covariance matrix.
void getVectorAtPoint(const Eigen::Vector4f &p, std::vector< int > &pt_union_indices, Eigen::Vector3f &vo)
Given the location of a point, get it's vector.
GridProjection()
Constructor.