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_ (), min_p_ (), max_p_ (), leaf_size_ (0.001), gaussian_scale_ (),
51 data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ (),
52 vector_at_data_point_ (), surface_ (), occupied_cell_list_ ()
56 template <
typename Po
intNT>
58 cell_hash_map_ (), min_p_ (), max_p_ (), leaf_size_ (resolution), gaussian_scale_ (),
59 data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ (),
60 vector_at_data_point_ (), surface_ (), occupied_cell_list_ ()
64 template <
typename Po
intNT>
67 vector_at_data_point_.clear ();
69 cell_hash_map_.clear ();
70 occupied_cell_list_.clear ();
75 template <
typename Po
intNT>
void
78 for (
size_t i = 0; i < data_->points.size(); ++i)
80 data_->points[i].x /=
static_cast<float> (scale_factor);
81 data_->points[i].y /=
static_cast<float> (scale_factor);
82 data_->points[i].z /=
static_cast<float> (scale_factor);
84 max_p_ /=
static_cast<float> (scale_factor);
85 min_p_ /=
static_cast<float> (scale_factor);
89 template <
typename Po
intNT>
void
94 Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
95 double scale_factor = (std::max)((std::max)(bounding_box_size.x (),
96 bounding_box_size.y ()),
97 bounding_box_size.z ());
99 scaleInputDataPoint (scale_factor);
102 int upper_right_index[3];
103 int lower_left_index[3];
104 for (
size_t i = 0; i < 3; ++i)
106 upper_right_index[i] =
static_cast<int> (max_p_(i) / leaf_size_ + 5);
107 lower_left_index[i] =
static_cast<int> (min_p_(i) / leaf_size_ - 5);
108 max_p_(i) =
static_cast<float> (upper_right_index[i] * leaf_size_);
109 min_p_(i) =
static_cast<float> (lower_left_index[i] * leaf_size_);
111 bounding_box_size = max_p_ - min_p_;
112 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
113 bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
115 (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
116 bounding_box_size.z ());
118 data_size_ =
static_cast<int> (max_size / leaf_size_);
119 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n",
120 min_p_.x (), min_p_.y (), min_p_.z ());
121 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n",
122 max_p_.x (), max_p_.y (), max_p_.z ());
123 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_);
124 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_);
125 occupied_cell_list_.resize (data_size_ * data_size_ * data_size_);
126 gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
130 template <
typename Po
intNT>
void
132 const Eigen::Vector4f &cell_center,
133 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts)
const
135 assert (pts.size () == 8);
137 float sz =
static_cast<float> (leaf_size_) / 2.0f;
139 pts[0] = cell_center + Eigen::Vector4f (-sz, sz, -sz, 0);
140 pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0);
141 pts[2] = cell_center + Eigen::Vector4f (sz, -sz, -sz, 0);
142 pts[3] = cell_center + Eigen::Vector4f (sz, sz, -sz, 0);
143 pts[4] = cell_center + Eigen::Vector4f (-sz, sz, sz, 0);
144 pts[5] = cell_center + Eigen::Vector4f (-sz, -sz, sz, 0);
145 pts[6] = cell_center + Eigen::Vector4f (sz, -sz, sz, 0);
146 pts[7] = cell_center + Eigen::Vector4f (sz, sz, sz, 0);
150 template <
typename Po
intNT>
void
152 std::vector <int> &pt_union_indices)
154 for (
int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
156 for (
int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
158 for (
int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
160 Eigen::Vector3i cell_index_3d (i, j, k);
161 int cell_index_1d = getIndexIn1D (cell_index_3d);
162 if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
166 pt_union_indices.insert (pt_union_indices.end (),
167 cell_hash_map_.at (cell_index_1d).data_indices.begin (),
168 cell_hash_map_.at (cell_index_1d).data_indices.end ());
176 template <
typename Po
intNT>
void
178 std::vector <int> &pt_union_indices)
181 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
184 Eigen::Vector4f pts[4];
185 Eigen::Vector3f vector_at_pts[4];
189 Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
190 getCellCenterFromIndex (index, cell_center);
191 getVertexFromCellCenter (cell_center, vertices);
194 Eigen::Vector3i indices[4];
195 indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
196 indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
197 indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
198 indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
201 for (
size_t i = 0; i < 4; ++i)
204 unsigned int index_1d = getIndexIn1D (indices[i]);
205 if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () ||
206 occupied_cell_list_[index_1d] == 0)
209 vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
213 for (
size_t i = 0; i < 3; ++i)
215 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
216 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
217 for (
size_t j = 0; j < 2; ++j)
220 vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]];
223 if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
227 Eigen::Vector3i polygon[4];
228 Eigen::Vector4f polygon_pts[4];
229 int polygon_indices_1d[4];
230 bool is_all_in_hash_map =
true;
234 polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
235 polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
236 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
237 polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
240 polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
241 polygon[1] = Eigen::Vector3i (index[0], index[1] + 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);
246 polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
247 polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
248 polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
249 polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
254 for (
size_t k = 0; k < 4; k++)
256 polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
257 if (!occupied_cell_list_[polygon_indices_1d[k]])
259 is_all_in_hash_map =
false;
263 if (is_all_in_hash_map)
265 for (
size_t k = 0; k < 4; k++)
267 polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
268 surface_.push_back (polygon_pts[k]);
276 template <
typename Po
intNT>
void
278 std::vector <int> &pt_union_indices, Eigen::Vector4f &projection)
280 const double projection_distance = leaf_size_ * 3;
281 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
282 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
284 getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
285 end_pt_vect[0].normalize();
287 double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
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),
299 end_pt[1] = end_pt[0] - Eigen::Vector4f (
300 end_pt_vect[0][0] * static_cast<float> (projection_distance),
301 end_pt_vect[0][1] * static_cast<float> (projection_distance),
302 end_pt_vect[0][2] * static_cast<float> (projection_distance),
304 getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
305 if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
307 Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
308 findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
315 template <
typename Po
intNT>
void
317 std::vector<int> (&pt_union_indices),
318 Eigen::Vector4f &projection)
321 Eigen::Vector4f model_coefficients;
323 Eigen::Matrix3f covariance_matrix;
324 Eigen::Vector4f xyz_centroid;
331 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
334 model_coefficients[0] = eigen_vector [0];
335 model_coefficients[1] = eigen_vector [1];
336 model_coefficients[2] = eigen_vector [2];
337 model_coefficients[3] = 0;
339 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
342 Eigen::Vector3f point (p.x (), p.y (), p.z ());
343 float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
344 point -= distance * model_coefficients.head < 3 > ();
346 projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
350 template <
typename Po
intNT>
void
352 std::vector <int> &pt_union_indices,
355 std::vector <double> pt_union_dist (pt_union_indices.size ());
356 std::vector <double> pt_union_weight (pt_union_indices.size ());
357 Eigen::Vector3f out_vector (0, 0, 0);
361 for (
size_t i = 0; i < pt_union_indices.size (); ++i)
363 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);
364 pt_union_dist[i] = (pp - p).squaredNorm ();
365 pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
366 mag += pow (M_E, -pow (sqrt (pt_union_dist[i]), 2.0) / gaussian_scale_);
367 sum += pt_union_weight[i];
373 data_->points[pt_union_indices[0]].normal[0],
374 data_->points[pt_union_indices[0]].normal[1],
375 data_->points[pt_union_indices[0]].normal[2]);
377 for (
size_t i = 0; i < pt_union_weight.size (); ++i)
379 pt_union_weight[i] /= sum;
380 Eigen::Vector3f vec (data_->points[pt_union_indices[i]].normal[0],
381 data_->points[pt_union_indices[i]].normal[1],
382 data_->points[pt_union_indices[i]].normal[2]);
385 vector_average.
add (vec, static_cast<float> (pt_union_weight[i]));
387 out_vector = vector_average.
getMean ();
390 out_vector.normalize ();
391 double d1 = getD1AtPoint (p, out_vector, pt_union_indices);
392 out_vector *=
static_cast<float> (sum);
393 vo = ((d1 > 0) ? -1 : 1) * out_vector;
397 template <
typename Po
intNT>
void
399 std::vector <int> &k_indices,
400 std::vector <float> &k_squared_distances,
403 Eigen::Vector3f out_vector (0, 0, 0);
404 std::vector <float> k_weight;
405 k_weight.resize (k_);
407 for (
int i = 0; i < k_; i++)
410 k_weight[i] = std::pow (static_cast<float>(M_E), static_cast<float>(-pow (static_cast<float>(k_squared_distances[i]), 2.0f) / gaussian_scale_));
414 for (
int i = 0; i < k_; i++)
417 Eigen::Vector3f vec (data_->points[k_indices[i]].normal[0],
418 data_->points[k_indices[i]].normal[1],
419 data_->points[k_indices[i]].normal[2]);
420 vector_average.
add (vec, k_weight[i]);
423 out_vector.normalize ();
424 double d1 = getD1AtPoint (p, out_vector, k_indices);
425 out_vector = out_vector * sum;
426 vo = ((d1 > 0) ? -1 : 1) * out_vector;
431 template <
typename Po
intNT>
double
433 const std::vector <int> &pt_union_indices)
435 std::vector <double> pt_union_dist (pt_union_indices.size ());
436 std::vector <double> pt_union_weight (pt_union_indices.size ());
438 for (
size_t i = 0; i < pt_union_indices.size (); ++i)
440 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);
441 pt_union_dist[i] = (pp - p).norm ();
442 sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
448 template <
typename Po
intNT>
double
450 const std::vector <int> &pt_union_indices)
452 double sz = 0.01 * leaf_size_;
453 Eigen::Vector3f v = vec *
static_cast<float> (sz);
455 double forward = getMagAtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
456 double backward = getMagAtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
457 return ((forward - backward) / (0.02 * leaf_size_));
461 template <
typename Po
intNT>
double
463 const std::vector <int> &pt_union_indices)
465 double sz = 0.01 * leaf_size_;
466 Eigen::Vector3f v = vec *
static_cast<float> (sz);
468 double forward = getD1AtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
469 double backward = getD1AtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
470 return ((forward - backward) / (0.02 * leaf_size_));
474 template <
typename Po
intNT>
bool
476 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
477 std::vector <int> &pt_union_indices)
479 assert (end_pts.size () == 2);
480 assert (vect_at_end_pts.size () == 2);
483 for (
size_t i = 0; i < 2; ++i)
485 length[i] = vect_at_end_pts[i].norm ();
486 vect_at_end_pts[i].normalize ();
488 double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
491 double ratio = length[0] / (length[0] + length[1]);
492 Eigen::Vector4f start_pt =
493 end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
494 Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
495 findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
498 getVectorAtPoint (intersection_pt, pt_union_indices, vec);
501 double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
509 template <
typename Po
intNT>
void
511 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
512 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
513 const Eigen::Vector4f &start_pt,
514 std::vector <int> &pt_union_indices,
515 Eigen::Vector4f &intersection)
517 assert (end_pts.size () == 2);
518 assert (vect_at_end_pts.size () == 2);
521 getVectorAtPoint (start_pt, pt_union_indices, vec);
522 double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
523 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
524 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
525 if ((fabs (d1) < 10e-3) || (level == max_binary_search_level_))
527 intersection = start_pt;
533 if (vec.dot (vect_at_end_pts[0]) < 0)
535 Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
536 new_end_pts[0] = end_pts[0];
537 new_end_pts[1] = start_pt;
538 new_vect_at_end_pts[0] = vect_at_end_pts[0];
539 new_vect_at_end_pts[1] = vec;
540 findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
543 if (vec.dot (vect_at_end_pts[1]) < 0)
545 Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
546 new_end_pts[0] = start_pt;
547 new_end_pts[1] = end_pts[1];
548 new_vect_at_end_pts[0] = vec;
549 new_vect_at_end_pts[1] = vect_at_end_pts[1];
550 findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
553 intersection = start_pt;
560 template <
typename Po
intNT>
void
563 for (
int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
565 for (
int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
567 for (
int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
569 Eigen::Vector3i cell_index_3d (i, j, k);
570 unsigned int cell_index_1d = getIndexIn1D (cell_index_3d);
571 if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ())
573 cell_hash_map_[cell_index_1d].data_indices.resize (0);
574 getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface);
583 template <
typename Po
intNT>
void
585 const Eigen::Vector3i &,
586 std::vector <int> &pt_union_indices,
587 const Leaf &cell_data)
590 Eigen::Vector4f grid_pt (
591 cell_data.
pt_on_surface.x () -
static_cast<float> (leaf_size_) / 2.0f,
592 cell_data.
pt_on_surface.y () +
static_cast<float> (leaf_size_) / 2.0f,
593 cell_data.
pt_on_surface.z () +
static_cast<float> (leaf_size_) / 2.0f, 0.0f);
596 getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt);
597 getProjection (cell_data.
pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface);
601 template <
typename Po
intNT>
void
603 const Leaf &cell_data)
606 Eigen::Vector4f grid_pt (
607 cell_center.x () -
static_cast<float> (leaf_size_) / 2.0f,
608 cell_center.y () +
static_cast<float> (leaf_size_) / 2.0f,
609 cell_center.z () +
static_cast<float> (leaf_size_) / 2.0f, 0.0f);
611 std::vector <int> k_indices;
612 k_indices.resize (k_);
613 std::vector <float> k_squared_distances;
614 k_squared_distances.resize (k_);
616 PointNT pt; pt.x = grid_pt.x (); pt.y = grid_pt.y (); pt.z = grid_pt.z ();
617 tree_->nearestKSearch (pt, k_, k_indices, k_squared_distances);
619 getVectorAtPointKNN (grid_pt, k_indices, k_squared_distances, cell_hash_map_[index_1d].vect_at_grid_pt);
620 getProjectionWithPlaneFit (cell_center, k_indices, cell_hash_map_[index_1d].pt_on_surface);
624 template <
typename Po
intNT>
bool
632 cell_hash_map_.max_load_factor (2.0);
633 cell_hash_map_.rehash (data_->points.size () /
static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
636 for (
int cp = 0; cp < static_cast<int> (data_->points.size ()); ++cp)
639 if (!pcl_isfinite (data_->points[cp].x) ||
640 !pcl_isfinite (data_->points[cp].y) ||
641 !pcl_isfinite (data_->points[cp].z))
644 Eigen::Vector3i index_3d;
645 getCellIndex (data_->points[cp].getVector4fMap (), index_3d);
646 int index_1d = getIndexIn1D (index_3d);
647 if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
652 cell_hash_map_[index_1d] = cell_data;
653 occupied_cell_list_[index_1d] = 1;
657 Leaf cell_data = cell_hash_map_.at (index_1d);
659 cell_hash_map_[index_1d] = cell_data;
663 Eigen::Vector3i index;
664 int numOfFilledPad = 0;
666 for (
int i = 0; i < data_size_; ++i)
668 for (
int j = 0; j < data_size_; ++j)
670 for (
int k = 0; k < data_size_; ++k)
675 if (occupied_cell_list_[getIndexIn1D (index)])
685 BOOST_FOREACH (
typename HashMap::value_type entry, cell_hash_map_)
687 getIndexIn3D (entry.first, index);
688 std::vector <int> pt_union_indices;
689 getDataPtsUnion (index, pt_union_indices);
693 if (pt_union_indices.size () > 10)
695 storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
697 occupied_cell_list_[entry.first] = 1;
702 BOOST_FOREACH (
typename HashMap::value_type entry, cell_hash_map_)
704 getIndexIn3D (entry.first, index);
705 std::vector <int> pt_union_indices;
706 getDataPtsUnion (index, pt_union_indices);
710 if (pt_union_indices.size () > 10)
711 createSurfaceForCell (index, pt_union_indices);
714 polygons.resize (surface_.size () / 4);
716 for (
int i = 0; i < static_cast<int> (polygons.size ()); ++i)
720 for (
int j = 0; j < 4; ++j)
728 template <
typename Po
intNT>
void
731 if (!reconstructPolygons (output.
polygons))
735 output.
header = input_->header;
738 cloud.
width =
static_cast<uint32_t
> (surface_.size ());
742 cloud.
points.resize (surface_.size ());
744 for (
size_t i = 0; i < cloud.
points.size (); ++i)
746 cloud.
points[i].x = surface_[i].x ();
747 cloud.
points[i].y = surface_[i].y ();
748 cloud.
points[i].z = surface_[i].z ();
754 template <
typename Po
intNT>
void
756 std::vector<pcl::Vertices> &polygons)
758 if (!reconstructPolygons (polygons))
762 points.
header = input_->header;
763 points.
width =
static_cast<uint32_t
> (surface_.size ());
767 points.
resize (surface_.size ());
769 for (
size_t i = 0; i < points.
size (); ++i)
771 points[i].x = surface_[i].x ();
772 points[i].y = surface_[i].y ();
773 points[i].z = surface_[i].z ();
777 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
779 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
~GridProjection()
Destructor.
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.
uint32_t width
The point cloud width (if organized as an image-structure).
struct pcl::PointXYZIEdge EIGEN_ALIGN16
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...
std::vector< uint32_t > vertices
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...
const Eigen::Matrix< real, dimension, 1 > & getMean() const
Get the mean of the added vectors.
void performReconstruction(pcl::PolygonMesh &output)
Create the surface.
void add(const Eigen::Matrix< real, dimension, 1 > &sample, real weight=1.0)
Add a new sample.
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 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...
uint32_t height
The point cloud height (if organized as an image-structure).
std::vector< ::pcl::Vertices > polygons
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 getEigenVector1(Eigen::Matrix< real, dimension, 1 > &eigen_vector1) const
Get the eigenvector corresponding to the smallest eigenvalue.
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.
double getD2AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 2nd derivative.
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...
const int I_SHIFT_EDGE[3][2]
bool reconstructPolygons(std::vector< pcl::Vertices > &polygons)
The actual surface reconstruction method.
void resize(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.