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 for (
auto& point: *data_) {
77 point.getVector3fMap() /=
static_cast<float> (scale_factor);
79 max_p_ /=
static_cast<float> (scale_factor);
80 min_p_ /=
static_cast<float> (scale_factor);
84 template <
typename Po
intNT>
void
89 Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
90 double scale_factor = (std::max)((std::max)(bounding_box_size.x (),
91 bounding_box_size.y ()),
92 bounding_box_size.z ());
94 scaleInputDataPoint (scale_factor);
97 int upper_right_index[3];
98 int lower_left_index[3];
99 for (std::size_t i = 0; i < 3; ++i)
101 upper_right_index[i] =
static_cast<int> (max_p_(i) / leaf_size_ + 5);
102 lower_left_index[i] =
static_cast<int> (min_p_(i) / leaf_size_ - 5);
103 max_p_(i) =
static_cast<float> (upper_right_index[i] * leaf_size_);
104 min_p_(i) =
static_cast<float> (lower_left_index[i] * leaf_size_);
106 bounding_box_size = max_p_ - min_p_;
107 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
108 bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
110 (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
111 bounding_box_size.z ());
113 data_size_ =
static_cast<int> (max_size / leaf_size_);
114 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n",
115 min_p_.x (), min_p_.y (), min_p_.z ());
116 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n",
117 max_p_.x (), max_p_.y (), max_p_.z ());
118 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_);
119 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_);
120 occupied_cell_list_.resize (data_size_ * data_size_ * data_size_);
121 gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
125 template <
typename Po
intNT>
void
127 const Eigen::Vector4f &cell_center,
128 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts)
const
130 assert (pts.size () == 8);
132 float sz =
static_cast<float> (leaf_size_) / 2.0f;
134 pts[0] = cell_center + Eigen::Vector4f (-sz, sz, -sz, 0);
135 pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0);
136 pts[2] = cell_center + Eigen::Vector4f (sz, -sz, -sz, 0);
137 pts[3] = cell_center + Eigen::Vector4f (sz, sz, -sz, 0);
138 pts[4] = cell_center + Eigen::Vector4f (-sz, sz, sz, 0);
139 pts[5] = cell_center + Eigen::Vector4f (-sz, -sz, sz, 0);
140 pts[6] = cell_center + Eigen::Vector4f (sz, -sz, sz, 0);
141 pts[7] = cell_center + Eigen::Vector4f (sz, sz, sz, 0);
145 template <
typename Po
intNT>
void
149 for (
int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
151 for (
int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
153 for (
int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
155 Eigen::Vector3i cell_index_3d (i, j, k);
156 int cell_index_1d = getIndexIn1D (cell_index_3d);
157 if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
161 pt_union_indices.insert (pt_union_indices.end (),
162 cell_hash_map_.at (cell_index_1d).data_indices.begin (),
163 cell_hash_map_.at (cell_index_1d).data_indices.end ());
171 template <
typename Po
intNT>
void
176 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
179 Eigen::Vector4f pts[4];
180 Eigen::Vector3f vector_at_pts[4];
184 Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
185 getCellCenterFromIndex (index, cell_center);
186 getVertexFromCellCenter (cell_center, vertices);
189 Eigen::Vector3i indices[4];
190 indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
191 indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
192 indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
193 indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
196 for (std::size_t i = 0; i < 4; ++i)
199 unsigned int index_1d = getIndexIn1D (indices[i]);
200 if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () ||
201 occupied_cell_list_[index_1d] == 0)
203 vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
207 for (std::size_t i = 0; i < 3; ++i)
209 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
210 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
211 for (std::size_t j = 0; j < 2; ++j)
217 if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
221 Eigen::Vector3i polygon[4];
222 Eigen::Vector4f polygon_pts[4];
223 int polygon_indices_1d[4];
224 bool is_all_in_hash_map =
true;
228 polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
229 polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
230 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
231 polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
234 polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
235 polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
236 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
237 polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
240 polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
241 polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
242 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
243 polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
248 for (std::size_t k = 0; k < 4; k++)
250 polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
251 if (!occupied_cell_list_[polygon_indices_1d[k]])
253 is_all_in_hash_map =
false;
257 if (is_all_in_hash_map)
259 for (std::size_t k = 0; k < 4; k++)
261 polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
262 surface_.push_back (polygon_pts[k]);
270 template <
typename Po
intNT>
void
272 pcl::Indices &pt_union_indices, Eigen::Vector4f &projection)
274 const double projection_distance = leaf_size_ * 3;
275 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
276 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
278 getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
279 end_pt_vect[0].normalize();
281 double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
287 end_pt[1] = end_pt[0] + Eigen::Vector4f (
288 end_pt_vect[0][0] *
static_cast<float> (projection_distance),
289 end_pt_vect[0][1] *
static_cast<float> (projection_distance),
290 end_pt_vect[0][2] *
static_cast<float> (projection_distance),
293 end_pt[1] = end_pt[0] - Eigen::Vector4f (
294 end_pt_vect[0][0] *
static_cast<float> (projection_distance),
295 end_pt_vect[0][1] *
static_cast<float> (projection_distance),
296 end_pt_vect[0][2] *
static_cast<float> (projection_distance),
298 getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
299 if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
301 Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
302 findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
309 template <
typename Po
intNT>
void
312 Eigen::Vector4f &projection)
315 Eigen::Vector4f model_coefficients;
317 Eigen::Matrix3f covariance_matrix;
318 Eigen::Vector4f xyz_centroid;
323 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
324 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
325 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
328 model_coefficients[0] = eigen_vector [0];
329 model_coefficients[1] = eigen_vector [1];
330 model_coefficients[2] = eigen_vector [2];
331 model_coefficients[3] = 0;
333 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
336 Eigen::Vector3f point (p.x (), p.y (), p.z ());
337 float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
338 point -=
distance * model_coefficients.head < 3 > ();
340 projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
344 template <
typename Po
intNT>
void
349 std::vector <double> pt_union_dist (pt_union_indices.size ());
350 std::vector <double> pt_union_weight (pt_union_indices.size ());
351 Eigen::Vector3f out_vector (0, 0, 0);
355 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
357 Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
358 pt_union_dist[i] = (pp - p).squaredNorm ();
359 pt_union_weight[i] = pow (
M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
360 mag += pow (
M_E, -pow (sqrt (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] = 1;
647 Leaf cell_data = cell_hash_map_.at (index_1d);
649 cell_hash_map_[index_1d] = cell_data;
653 Eigen::Vector3i index;
654 int numOfFilledPad = 0;
656 for (
int i = 0; i < data_size_; ++i)
658 for (
int j = 0; j < data_size_; ++j)
660 for (
int k = 0; k < data_size_; ++k)
665 if (occupied_cell_list_[getIndexIn1D (index)])
675 for (
const auto &entry : cell_hash_map_)
677 getIndexIn3D (entry.first, index);
679 getDataPtsUnion (index, pt_union_indices);
683 if (pt_union_indices.size () > 10)
685 storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
687 occupied_cell_list_[entry.first] = 1;
692 for (
const auto &entry : cell_hash_map_)
694 getIndexIn3D (entry.first, index);
696 getDataPtsUnion (index, pt_union_indices);
700 if (pt_union_indices.size () > 10)
701 createSurfaceForCell (index, pt_union_indices);
704 polygons.resize (surface_.size () / 4);
706 for (
int i = 0; i < static_cast<int> (polygons.size ()); ++i)
710 for (
int j = 0; j < 4; ++j)
718 template <
typename Po
intNT>
void
721 if (!reconstructPolygons (output.
polygons))
725 output.
header = input_->header;
728 cloud.
width = surface_.size ();
732 cloud.
resize (surface_.size ());
734 for (std::size_t i = 0; i < cloud.
size (); ++i)
736 cloud[i].x = surface_[i].x ();
737 cloud[i].y = surface_[i].y ();
738 cloud[i].z = surface_[i].z ();
744 template <
typename Po
intNT>
void
746 std::vector<pcl::Vertices> &polygons)
748 if (!reconstructPolygons (polygons))
752 points.
header = input_->header;
753 points.
width = surface_.size ();
757 points.
resize (surface_.size ());
759 for (std::size_t i = 0; i < points.
size (); ++i)
761 points[i].x = surface_[i].x ();
762 points[i].y = surface_[i].y ();
763 points[i].z = surface_[i].z ();
767 #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()
Destructor.
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 vectices 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.
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.