38 #ifndef PCL_SURFACE_IMPL_GRID_PROJECTION_H_
39 #define PCL_SURFACE_IMPL_GRID_PROJECTION_H_
41 #include <pcl/surface/grid_projection.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 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);
81 max_p_ /=
static_cast<float> (scale_factor);
82 min_p_ /=
static_cast<float> (scale_factor);
86 template <
typename Po
intNT>
void
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 ());
96 scaleInputDataPoint (scale_factor);
99 int upper_right_index[3];
100 int lower_left_index[3];
101 for (std::size_t i = 0; i < 3; ++i)
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_);
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 ());
112 (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
113 bounding_box_size.z ());
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);
127 template <
typename Po
intNT>
void
129 const Eigen::Vector4f &cell_center,
130 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts)
const
132 assert (pts.size () == 8);
134 float sz =
static_cast<float> (leaf_size_) / 2.0f;
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);
147 template <
typename Po
intNT>
void
151 for (
int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
153 for (
int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
155 for (
int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
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 ())
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 ());
173 template <
typename Po
intNT>
void
178 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
181 Eigen::Vector4f pts[4];
182 Eigen::Vector3f vector_at_pts[4];
186 Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
187 getCellCenterFromIndex (index, cell_center);
188 getVertexFromCellCenter (cell_center, vertices);
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]);
198 for (std::size_t i = 0; i < 4; ++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)
205 vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
209 for (std::size_t i = 0; i < 3; ++i)
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)
219 if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
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;
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]);
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);
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);
250 for (std::size_t k = 0; k < 4; k++)
252 polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
253 if (!occupied_cell_list_[polygon_indices_1d[k]])
255 is_all_in_hash_map =
false;
259 if (is_all_in_hash_map)
261 for (std::size_t k = 0; k < 4; k++)
263 polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
264 surface_.push_back (polygon_pts[k]);
272 template <
typename Po
intNT>
void
274 pcl::Indices &pt_union_indices, Eigen::Vector4f &projection)
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);
280 getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
281 end_pt_vect[0].normalize();
283 double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
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),
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),
300 getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
301 if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
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);
311 template <
typename Po
intNT>
void
314 Eigen::Vector4f &projection)
317 Eigen::Vector4f model_coefficients;
319 Eigen::Matrix3f covariance_matrix;
320 Eigen::Vector4f xyz_centroid;
325 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
326 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
327 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
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;
335 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
338 Eigen::Vector3f point (p.x (), p.y (), p.z ());
339 float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
340 point -=
distance * model_coefficients.head < 3 > ();
342 projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
346 template <
typename Po
intNT>
void
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);
356 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
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];
367 (*data_)[pt_union_indices[0]].normal[0],
368 (*data_)[pt_union_indices[0]].normal[1],
369 (*data_)[pt_union_indices[0]].normal[2]);
371 for (std::size_t i = 0; i < pt_union_weight.size (); ++i)
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]);
379 vector_average.
add (vec,
static_cast<float> (pt_union_weight[i]));
381 out_vector = vector_average.
getMean ();
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;
391 template <
typename Po
intNT>
void
394 std::vector <float> &k_squared_distances,
397 Eigen::Vector3f out_vector (0, 0, 0);
398 std::vector <float> k_weight;
399 k_weight.resize (k_);
401 for (
int i = 0; i < k_; i++)
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_));
408 for (
int i = 0; i < k_; i++)
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]);
417 out_vector.normalize ();
418 double d1 = getD1AtPoint (p, out_vector, k_indices);
420 vo = ((d1 > 0) ? -1 : 1) * out_vector;
425 template <
typename Po
intNT>
double
429 std::vector <double> pt_union_dist (pt_union_indices.size ());
431 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
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_);
441 template <
typename Po
intNT>
double
445 double sz = 0.01 * leaf_size_;
446 Eigen::Vector3f v = vec *
static_cast<float> (sz);
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_));
454 template <
typename Po
intNT>
double
458 double sz = 0.01 * leaf_size_;
459 Eigen::Vector3f v = vec *
static_cast<float> (sz);
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_));
467 template <
typename Po
intNT>
bool
469 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
472 assert (end_pts.size () == 2);
473 assert (vect_at_end_pts.size () == 2);
476 for (std::size_t i = 0; i < 2; ++i)
478 length[i] = vect_at_end_pts[i].norm ();
479 vect_at_end_pts[i].normalize ();
481 double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
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);
491 getVectorAtPoint (intersection_pt, pt_union_indices, vec);
494 double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
502 template <
typename Po
intNT>
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,
508 Eigen::Vector4f &intersection)
510 assert (end_pts.size () == 2);
511 assert (vect_at_end_pts.size () == 2);
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_))
520 intersection = start_pt;
524 if (vec.dot (vect_at_end_pts[0]) < 0)
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);
534 if (vec.dot (vect_at_end_pts[1]) < 0)
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);
544 intersection = start_pt;
550 template <
typename Po
intNT>
void
553 for (
int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
555 for (
int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
557 for (
int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
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 ())
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);
573 template <
typename Po
intNT>
void
575 const Eigen::Vector3i &,
577 const Leaf &cell_data)
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);
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);
591 template <
typename Po
intNT>
void
593 const Leaf &cell_data)
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);
602 k_indices.resize (k_);
603 std::vector <float> k_squared_distances;
604 k_squared_distances.resize (k_);
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);
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);
614 template <
typename Po
intNT>
bool
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 ()));
626 for (
pcl::index_t cp = 0; cp < static_cast<pcl::index_t> (data_->size ()); ++cp)
629 if (!std::isfinite ((*data_)[cp].x) ||
630 !std::isfinite ((*data_)[cp].y) ||
631 !std::isfinite ((*data_)[cp].z))
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 ())
642 cell_hash_map_[index_1d] = cell_data;
643 occupied_cell_list_[index_1d] =
true;
647 Leaf cell_data = cell_hash_map_.at (index_1d);
649 cell_hash_map_[index_1d] = cell_data;
653 Eigen::Vector3i index;
655 for (
int i = 0; i < data_size_; ++i)
657 for (
int j = 0; j < data_size_; ++j)
659 for (
int k = 0; k < data_size_; ++k)
664 if (occupied_cell_list_[getIndexIn1D (index)])
673 for (
const auto &entry : cell_hash_map_)
675 getIndexIn3D (entry.first, index);
677 getDataPtsUnion (index, pt_union_indices);
681 if (pt_union_indices.size () > 10)
683 storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
685 occupied_cell_list_[entry.first] = 1;
690 for (
const auto &entry : cell_hash_map_)
692 getIndexIn3D (entry.first, index);
694 getDataPtsUnion (index, pt_union_indices);
698 if (pt_union_indices.size () > 10)
699 createSurfaceForCell (index, pt_union_indices);
702 polygons.resize (surface_.size () / 4);
704 for (
int i = 0; i < static_cast<int> (polygons.size ()); ++i)
708 for (
int j = 0; j < 4; ++j)
716 template <
typename Po
intNT>
void
719 if (!reconstructPolygons (output.
polygons))
723 output.
header = input_->header;
726 cloud.
width = surface_.size ();
730 cloud.
resize (surface_.size ());
732 for (std::size_t i = 0; i < cloud.
size (); ++i)
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 ();
742 template <
typename Po
intNT>
void
744 std::vector<pcl::Vertices> &polygons)
746 if (!reconstructPolygons (polygons))
750 points.
header = input_->header;
751 points.
width = surface_.size ();
755 points.
resize (surface_.size ());
757 for (std::size_t i = 0; i < points.
size (); ++i)
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 ();
765 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
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.
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).
Calculates the weighted average and the covariance matrix.
void add(const VectorType &sample, real weight=1.0)
Add a new sample.
const VectorType & getMean() const
Get the mean of the added vectors.
void getEigenVector1(VectorType &eigen_vector1) const
Get the eigenvector corresponding to the smallest eigenvalue.
Define standard C methods and C++ classes that are common to all methods.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
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 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...
float distance(const PointT &p1, const PointT &p2)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
IndicesAllocator<> Indices
Type used for indices in PCL.
const int I_SHIFT_EDGE[3][2]
Eigen::Vector4f pt_on_surface
pcl::Indices data_indices
std::vector< ::pcl::Vertices > polygons
::pcl::PCLPointCloud2 cloud
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.