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>
48template <
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_ ()
55template <
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_ ()
62template <
typename Po
intNT>
65 vector_at_data_point_.clear ();
67 cell_hash_map_.clear ();
68 occupied_cell_list_.clear ();
73template <
typename Po
intNT>
void
76 for (
auto& point: *data_) {
77 point.getVector3fMap() /=
static_cast<float> (
scale_factor);
84template <
typename Po
intNT>
void
99 for (std::size_t i = 0; i < 3; ++i)
107 PCL_DEBUG (
"[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
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);
125template <
typename Po
intNT>
void
128 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &
pts)
const
132 float sz =
static_cast<float> (leaf_size_) / 2.0f;
145template <
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)
157 if (cell_hash_map_.find (
cell_index_1d) != cell_hash_map_.end ())
171template <
typename Po
intNT>
void
176 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
179 Eigen::Vector4f
pts[4];
184 Eigen::Vector4f
cell_center = Eigen::Vector4f::Zero ();
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 () ||
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)
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++)
259 for (std::size_t k = 0; k < 4; k++)
270template <
typename Po
intNT>
void
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);
309template <
typename Po
intNT>
void
318 Eigen::Vector4f xyz_centroid;
336 Eigen::Vector3f point (p.x (), p.y (), p.z ());
340 projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
344template <
typename Po
intNT>
void
391template <
typename Po
intNT>
void
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++)
425template <
typename Po
intNT>
double
441template <
typename Po
intNT>
double
445 double sz = 0.01 * leaf_size_;
446 Eigen::Vector3f v =
vec *
static_cast<float> (
sz);
454template <
typename Po
intNT>
double
458 double sz = 0.01 * leaf_size_;
459 Eigen::Vector3f v =
vec *
static_cast<float> (
sz);
467template <
typename Po
intNT>
bool
469 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &
vect_at_end_pts,
476 for (std::size_t i = 0; i < 2; ++i)
484 double ratio = length[0] / (length[0] + length[1]);
502template <
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,
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) < 10
e-3) || (level == max_binary_search_level_))
550template <
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)
561 if (cell_hash_map_.find (
cell_index_1d) == cell_hash_map_.end ())
573template <
typename Po
intNT>
void
575 const Eigen::Vector3i &,
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);
591template <
typename Po
intNT>
void
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);
614template <
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 ()));
629 if (!std::isfinite ((*data_)[cp].x) ||
630 !std::isfinite ((*data_)[cp].y) ||
631 !std::isfinite ((*data_)[cp].z))
637 if (cell_hash_map_.find (
index_1d) == cell_hash_map_.end ())
653 Eigen::Vector3i index;
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);
687 occupied_cell_list_[
entry.first] = 1;
692 for (
const auto &
entry : cell_hash_map_)
694 getIndexIn3D (
entry.first, index);
704 polygons.resize (surface_.size () / 4);
710 for (
int j = 0; j < 4; ++j)
718template <
typename Po
intNT>
void
721 if (!reconstructPolygons (
output.polygons))
725 output.header = input_->header;
728 cloud.width = surface_.
size ();
730 cloud.is_dense =
true;
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 ();
744template <
typename Po
intNT>
void
746 std::vector<pcl::Vertices> &polygons)
748 if (!reconstructPolygons (polygons))
752 points.header = input_->header;
753 points.width = surface_.
size ();
755 points.is_dense =
true;
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.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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.
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...
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.
const int I_SHIFT_EDGE[3][2]
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.