42 #include <pcl/surface/boost.h>
43 #include <pcl/surface/reconstruction.h>
45 #include <unordered_map>
51 {0, 4}, {1, 5}, {2, 6}, {3, 7},
52 {0, 1}, {1, 2}, {2, 3}, {3, 0},
53 {4, 5}, {5, 6}, {6, 7}, {7, 4}
74 template <
typename Po
intNT>
78 using Ptr = shared_ptr<GridProjection<PointNT> >;
79 using ConstPtr = shared_ptr<const GridProjection<PointNT> >;
99 typedef std::unordered_map<int, Leaf, std::hash<int>, std::equal_to<>, Eigen::aligned_allocator<std::pair<const int, Leaf>>>
HashMap;
118 leaf_size_ = resolution;
139 padding_size_ = padding_size;
144 return (padding_size_);
169 max_binary_search_level_ = max_binary_search_level;
174 return (max_binary_search_level_);
181 return (cell_hash_map_);
184 inline const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >&
187 return (vector_at_data_point_);
190 inline const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&
233 std::vector<pcl::Vertices> &polygons)
override;
251 for (
int i = 0; i < 3; ++i)
252 index[i] =
static_cast<int> ((p[i] - min_p_(i)) / leaf_size_);
263 for (
int i = 0; i < 3; ++i)
265 min_p_[i] +
static_cast<float> (index[i]) *
266 static_cast<float> (leaf_size_) +
267 static_cast<float> (leaf_size_) / 2.0f;
276 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts)
const;
286 return (index[0] * data_size_ * data_size_ +
287 index[1] * data_size_ + index[2]);
299 index_3d[0] = index_1d / (data_size_ * data_size_);
300 index_1d -= index_3d[0] * data_size_ * data_size_;
301 index_3d[1] = index_1d / data_size_;
302 index_1d -= index_3d[1] * data_size_;
303 index_3d[2] = index_1d;
311 fillPad (
const Eigen::Vector3i &index);
318 getDataPtsUnion (
const Eigen::Vector3i &index, std::vector <int> &pt_union_indices);
338 getProjection (
const Eigen::Vector4f &p, std::vector<int> &pt_union_indices, Eigen::Vector4f &projection);
349 std::vector<int> &pt_union_indices,
350 Eigen::Vector4f &projection);
360 std::vector <int> &pt_union_indices, Eigen::Vector3f &vo);
371 std::vector<int> &k_indices,
372 std::vector<float> &k_squared_distances,
373 Eigen::Vector3f &vo);
380 getMagAtPoint (
const Eigen::Vector4f &p,
const std::vector <int> &pt_union_indices);
388 getD1AtPoint (
const Eigen::Vector4f &p,
const Eigen::Vector3f &vec,
389 const std::vector <int> &pt_union_indices);
397 getD2AtPoint (
const Eigen::Vector4f &p,
const Eigen::Vector3f &vec,
398 const std::vector <int> &pt_union_indices);
409 isIntersected (
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
410 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
411 std::vector <int> &pt_union_indices);
423 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
424 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
425 const Eigen::Vector4f &start_pt,
426 std::vector<int> &pt_union_indices,
427 Eigen::Vector4f &intersection);
445 std::vector<int> &pt_union_indices,
const Leaf &cell_data);
468 Eigen::Vector4f min_p_, max_p_;
474 double gaussian_scale_;
480 int max_binary_search_level_;
492 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vector_at_data_point_;
495 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > surface_;
498 boost::dynamic_bitset<> occupied_cell_list_;
501 std::string getClassName ()
const override {
return (
"GridProjection"); }
Grid projection surface reconstruction method.
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 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 getIndexIn3D(int index_1d, Eigen::Vector3i &index_3d) const
Given an index in 1d, translate it into the index (x, y, z) in 3d.
double getMagAtPoint(const Eigen::Vector4f &p, const std::vector< int > &pt_union_indices)
Get the magnitude of the vector by summing up the distance.
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.
const HashMap & getCellHashMap() const
void getBoundingBox()
Get the bounding box for the input data points, also calculating the cell size, and the gaussian scal...
void getCellIndex(const Eigen::Vector4f &p, Eigen::Vector3i &index) const
Get the 3d index (x,y,z) of the cell based on the location of the cell.
double getD1AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 1st derivative.
const std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > & getVectorAtDataPoint() const
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...
double getD2AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 2nd derivative.
shared_ptr< const GridProjection< PointNT > > ConstPtr
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.
~GridProjection()
Destructor.
void setPaddingSize(int padding_size)
When averaging the vectors, we find the union of all the input data points within the padding area,...
GridProjection()
Constructor.
void getCellCenterFromIndex(const Eigen::Vector3i &index, Eigen::Vector4f ¢er) const
Given the 3d index (x, y, z) of the cell, get the coordinates of the cell center.
void setMaxBinarySearchLevel(int max_binary_search_level)
Binary search is used in projection.
void getDataPtsUnion(const Eigen::Vector3i &index, std::vector< int > &pt_union_indices)
Obtain the index of a cell and the pad size.
std::unordered_map< int, Leaf, std::hash< int >, std::equal_to<>, Eigen::aligned_allocator< std::pair< const int, Leaf > > > HashMap
int getMaxBinarySearchLevel() const
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 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.
int getNearestNeighborNum() const
shared_ptr< GridProjection< PointNT > > Ptr
const std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > & getSurface() const
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.
void setNearestNeighborNum(int k)
Set this only when using the k nearest neighbors search instead of finding the point union.
typename pcl::PointCloud< PointNT >::Ptr PointCloudPtr
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....
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...
int getIndexIn1D(const Eigen::Vector3i &index) const
Given an index (x, y, z) in 3d, translate it into the index in 1d.
void performReconstruction(pcl::PolygonMesh &output) override
Create the surface.
int getPaddingSize() const
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...
double getResolution() const
typename KdTree::Ptr KdTreePtr
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.
void setResolution(double resolution)
Set the size of the grid cell.
KdTree represents the base spatial locator class for kd-tree implementations.
shared_ptr< KdTree< PointT > > Ptr
shared_ptr< PointCloud< PointT > > Ptr
SurfaceReconstruction represents a base surface reconstruction class.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Defines functions, macros and traits for allocating and using memory.
const int I_SHIFT_EP[12][2]
The 12 edges of a cell.
const int I_SHIFT_EDGE[3][2]
Defines all the PCL and non-PCL macros used.
Eigen::Vector3f vect_at_grid_pt
Eigen::Vector4f pt_on_surface
std::vector< int > data_indices