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.
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 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...
std::uint32_t width
The point cloud width (if organized as an image-structure).
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
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.
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.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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.
void resize(std::size_t n)
Resize the cloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
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.
pcl::PCLHeader header
The point cloud header.