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>;
769 #endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_