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 147 std::vector <int> &pt_union_indices)
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 173 std::vector <int> &pt_union_indices)
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 std::vector <int> &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 311 std::vector<int> (&pt_union_indices),
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 346 std::vector <int> &pt_union_indices,
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 393 std::vector <int> &k_indices,
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 427 const std::vector <int> &pt_union_indices)
429 std::vector <double> pt_union_dist (pt_union_indices.size ());
430 std::vector <double> pt_union_weight (pt_union_indices.size ());
432 for (std::size_t i = 0; i < pt_union_indices.size (); ++i)
434 Eigen::Vector4f pp ((*data_)[pt_union_indices[i]].x, (*data_)[pt_union_indices[i]].y, (*data_)[pt_union_indices[i]].z, 0);
435 pt_union_dist[i] = (pp - p).norm ();
436 sum += pow (
M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
442 template <
typename Po
intNT>
double 444 const std::vector <int> &pt_union_indices)
446 double sz = 0.01 * leaf_size_;
447 Eigen::Vector3f v = vec *
static_cast<float> (sz);
449 double forward = getMagAtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
450 double backward = getMagAtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
451 return ((forward - backward) / (0.02 * leaf_size_));
455 template <
typename Po
intNT>
double 457 const std::vector <int> &pt_union_indices)
459 double sz = 0.01 * leaf_size_;
460 Eigen::Vector3f v = vec *
static_cast<float> (sz);
462 double forward = getD1AtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
463 double backward = getD1AtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
464 return ((forward - backward) / (0.02 * leaf_size_));
468 template <
typename Po
intNT>
bool 470 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
471 std::vector <int> &pt_union_indices)
473 assert (end_pts.size () == 2);
474 assert (vect_at_end_pts.size () == 2);
477 for (std::size_t i = 0; i < 2; ++i)
479 length[i] = vect_at_end_pts[i].norm ();
480 vect_at_end_pts[i].normalize ();
482 double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
485 double ratio = length[0] / (length[0] + length[1]);
486 Eigen::Vector4f start_pt =
487 end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
488 Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
489 findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
492 getVectorAtPoint (intersection_pt, pt_union_indices, vec);
495 double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
503 template <
typename Po
intNT>
void 505 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
506 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
507 const Eigen::Vector4f &start_pt,
508 std::vector <int> &pt_union_indices,
509 Eigen::Vector4f &intersection)
511 assert (end_pts.size () == 2);
512 assert (vect_at_end_pts.size () == 2);
515 getVectorAtPoint (start_pt, pt_union_indices, vec);
516 double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
517 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
518 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
519 if ((std::abs (d1) < 10e-3) || (level == max_binary_search_level_))
521 intersection = start_pt;
525 if (vec.dot (vect_at_end_pts[0]) < 0)
527 Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
528 new_end_pts[0] = end_pts[0];
529 new_end_pts[1] = start_pt;
530 new_vect_at_end_pts[0] = vect_at_end_pts[0];
531 new_vect_at_end_pts[1] = vec;
532 findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
535 if (vec.dot (vect_at_end_pts[1]) < 0)
537 Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
538 new_end_pts[0] = start_pt;
539 new_end_pts[1] = end_pts[1];
540 new_vect_at_end_pts[0] = vec;
541 new_vect_at_end_pts[1] = vect_at_end_pts[1];
542 findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
545 intersection = start_pt;
551 template <
typename Po
intNT>
void 554 for (
int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
556 for (
int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
558 for (
int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
560 Eigen::Vector3i cell_index_3d (i, j, k);
561 unsigned int cell_index_1d = getIndexIn1D (cell_index_3d);
562 if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ())
564 cell_hash_map_[cell_index_1d].data_indices.resize (0);
565 getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface);
574 template <
typename Po
intNT>
void 576 const Eigen::Vector3i &,
577 std::vector <int> &pt_union_indices,
578 const Leaf &cell_data)
581 Eigen::Vector4f grid_pt (
582 cell_data.
pt_on_surface.x () -
static_cast<float> (leaf_size_) / 2.0f,
583 cell_data.
pt_on_surface.y () +
static_cast<float> (leaf_size_) / 2.0f,
584 cell_data.
pt_on_surface.z () +
static_cast<float> (leaf_size_) / 2.0f, 0.0f);
587 getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt);
588 getProjection (cell_data.
pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface);
592 template <
typename Po
intNT>
void 594 const Leaf &cell_data)
597 Eigen::Vector4f grid_pt (
598 cell_center.x () -
static_cast<float> (leaf_size_) / 2.0f,
599 cell_center.y () +
static_cast<float> (leaf_size_) / 2.0f,
600 cell_center.z () +
static_cast<float> (leaf_size_) / 2.0f, 0.0f);
602 std::vector <int> k_indices;
603 k_indices.resize (k_);
604 std::vector <float> k_squared_distances;
605 k_squared_distances.resize (k_);
607 PointNT pt; pt.x = grid_pt.x (); pt.y = grid_pt.y (); pt.z = grid_pt.z ();
608 tree_->nearestKSearch (pt, k_, k_indices, k_squared_distances);
610 getVectorAtPointKNN (grid_pt, k_indices, k_squared_distances, cell_hash_map_[index_1d].vect_at_grid_pt);
611 getProjectionWithPlaneFit (cell_center, k_indices, cell_hash_map_[index_1d].pt_on_surface);
615 template <
typename Po
intNT>
bool 623 cell_hash_map_.max_load_factor (2.0);
624 cell_hash_map_.rehash (data_->size () /
static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
627 for (
int cp = 0; cp < static_cast<int> (data_->size ()); ++cp)
630 if (!std::isfinite ((*data_)[cp].x) ||
631 !std::isfinite ((*data_)[cp].y) ||
632 !std::isfinite ((*data_)[cp].z))
635 Eigen::Vector3i index_3d;
636 getCellIndex ((*data_)[cp].getVector4fMap (), index_3d);
637 int index_1d = getIndexIn1D (index_3d);
638 if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
643 cell_hash_map_[index_1d] = cell_data;
644 occupied_cell_list_[index_1d] = 1;
648 Leaf cell_data = cell_hash_map_.at (index_1d);
650 cell_hash_map_[index_1d] = cell_data;
654 Eigen::Vector3i index;
655 int numOfFilledPad = 0;
657 for (
int i = 0; i < data_size_; ++i)
659 for (
int j = 0; j < data_size_; ++j)
661 for (
int k = 0; k < data_size_; ++k)
666 if (occupied_cell_list_[getIndexIn1D (index)])
676 for (
const auto &entry : cell_hash_map_)
678 getIndexIn3D (entry.first, index);
679 std::vector <int> pt_union_indices;
680 getDataPtsUnion (index, pt_union_indices);
684 if (pt_union_indices.size () > 10)
686 storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
688 occupied_cell_list_[entry.first] = 1;
693 for (
const auto &entry : cell_hash_map_)
695 getIndexIn3D (entry.first, index);
696 std::vector <int> pt_union_indices;
697 getDataPtsUnion (index, pt_union_indices);
701 if (pt_union_indices.size () > 10)
702 createSurfaceForCell (index, pt_union_indices);
705 polygons.resize (surface_.size () / 4);
707 for (
int i = 0; i < static_cast<int> (polygons.size ()); ++i)
711 for (
int j = 0; j < 4; ++j)
719 template <
typename Po
intNT>
void 722 if (!reconstructPolygons (output.
polygons))
726 output.
header = input_->header;
729 cloud.
width = surface_.size ();
733 cloud.
points.resize (surface_.size ());
735 for (std::size_t i = 0; i < cloud.
size (); ++i)
737 cloud[i].x = surface_[i].x ();
738 cloud[i].y = surface_[i].y ();
739 cloud[i].z = surface_[i].z ();
745 template <
typename Po
intNT>
void 747 std::vector<pcl::Vertices> &polygons)
749 if (!reconstructPolygons (polygons))
753 points.
header = input_->header;
754 points.
width = surface_.size ();
758 points.
resize (surface_.size ());
760 for (std::size_t i = 0; i < points.
size (); ++i)
762 points[i].x = surface_[i].x ();
763 points[i].y = surface_[i].y ();
764 points[i].z = surface_[i].z ();
768 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>; 770 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_ 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.
~GridProjection()
Destructor.
const VectorType & getMean() const
Get the mean of the added vectors.
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 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).
Define standard C methods and C++ classes that are common to all methods.
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...
float distance(const PointT &p1, const PointT &p2)
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 resize(std::size_t count)
Resizes the container to contain count elements.
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
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<T> 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 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.
void getEigenVector1(VectorType &eigen_vector1) const
Get the eigenvector corresponding to the smallest eigenvalue.
GridProjection()
Constructor.
Define methods for centroid estimation and covariance matrix calculus.