38 #ifndef PCL_SURFACE_GRID_PROJECTION_H_
39 #define PCL_SURFACE_GRID_PROJECTION_H_
41 #include <pcl/surface/boost.h>
42 #include <pcl/surface/reconstruction.h>
48 {0, 4}, {1, 5}, {2, 6}, {3, 7},
49 {0, 1}, {1, 2}, {2, 3}, {3, 0},
50 {4, 5}, {5, 6}, {6, 7}, {7, 4}
71 template <
typename Po
intNT>
75 typedef boost::shared_ptr<GridProjection<PointNT> >
Ptr;
76 typedef boost::shared_ptr<const GridProjection<PointNT> >
ConstPtr;
96 typedef boost::unordered_map<int, Leaf, boost::hash<int>, std::equal_to<int>, Eigen::aligned_allocator<int> >
HashMap;
115 leaf_size_ = resolution;
136 padding_size_ = padding_size;
141 return (padding_size_);
166 max_binary_search_level_ = max_binary_search_level;
171 return (max_binary_search_level_);
175 inline const HashMap&
178 return (cell_hash_map_);
181 inline const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >&
184 return (vector_at_data_point_);
187 inline const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&
230 std::vector<pcl::Vertices> &polygons);
248 for (
int i = 0; i < 3; ++i)
249 index[i] = static_cast<int> ((p[i] - min_p_(i)) / leaf_size_);
260 for (
int i = 0; i < 3; ++i)
262 min_p_[i] + static_cast<float> (index[i]) *
263 static_cast<float> (leaf_size_) +
264 static_cast<float> (leaf_size_) / 2.0f;
273 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts)
const;
283 return (index[0] * data_size_ * data_size_ +
284 index[1] * data_size_ + index[2]);
296 index_3d[0] = index_1d / (data_size_ * data_size_);
297 index_1d -= index_3d[0] * data_size_ * data_size_;
298 index_3d[1] = index_1d / data_size_;
299 index_1d -= index_3d[1] * data_size_;
300 index_3d[2] = index_1d;
308 fillPad (
const Eigen::Vector3i &index);
315 getDataPtsUnion (
const Eigen::Vector3i &index, std::vector <int> &pt_union_indices);
335 getProjection (
const Eigen::Vector4f &p, std::vector<int> &pt_union_indices, Eigen::Vector4f &projection);
346 std::vector<int> &pt_union_indices,
347 Eigen::Vector4f &projection);
357 std::vector <int> &pt_union_indices, Eigen::Vector3f &vo);
368 std::vector<int> &k_indices,
369 std::vector<float> &k_squared_distances,
370 Eigen::Vector3f &vo);
377 getMagAtPoint (
const Eigen::Vector4f &p,
const std::vector <int> &pt_union_indices);
385 getD1AtPoint (
const Eigen::Vector4f &p,
const Eigen::Vector3f &vec,
386 const std::vector <int> &pt_union_indices);
394 getD2AtPoint (
const Eigen::Vector4f &p,
const Eigen::Vector3f &vec,
395 const std::vector <int> &pt_union_indices);
406 isIntersected (
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
407 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
408 std::vector <int> &pt_union_indices);
420 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
421 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
422 const Eigen::Vector4f &start_pt,
423 std::vector<int> &pt_union_indices,
424 Eigen::Vector4f &intersection);
442 std::vector<int> &pt_union_indices,
const Leaf &cell_data);
462 HashMap cell_hash_map_;
465 Eigen::Vector4f min_p_, max_p_;
471 double gaussian_scale_;
477 int max_binary_search_level_;
489 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vector_at_data_point_;
492 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > surface_;
495 boost::dynamic_bitset<> occupied_cell_list_;
498 std::string getClassName ()
const {
return (
"GridProjection"); }
501 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
505 #endif // PCL_SURFACE_GRID_PROJECTION_H_
~GridProjection()
Destructor.
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.
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...
void setMaxBinarySearchLevel(int max_binary_search_level)
Binary search is used in projection.
boost::unordered_map< int, Leaf, boost::hash< int >, std::equal_to< int >, Eigen::aligned_allocator< int > > HashMap
double getD1AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 1st derivative.
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...
SurfaceReconstruction represents a base surface reconstruction class.
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...
pcl::KdTree< PointNT >::Ptr KdTreePtr
boost::shared_ptr< KdTree< PointT > > Ptr
void performReconstruction(pcl::PolygonMesh &output)
Create the surface.
boost::shared_ptr< PointCloud< PointT > > Ptr
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.
int getNearestNeighborNum() const
Eigen::Vector4f pt_on_surface
pcl::KdTree< PointNT > KdTree
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...
const std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > & getSurface() const
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 setPaddingSize(int padding_size)
When averaging the vectors, we find the union of all the input data points within the padding area...
int getMaxBinarySearchLevel() 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...
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. ...
const std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > & getVectorAtDataPoint() const
Grid projection surface reconstruction method.
void setNearestNeighborNum(int k)
Set this only when using the k nearest neighbors search instead of finding the point union...
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.
Eigen::Vector3f vect_at_grid_pt
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.
boost::shared_ptr< const GridProjection< PointNT > > ConstPtr
double getD2AtPoint(const Eigen::Vector4f &p, const Eigen::Vector3f &vec, const std::vector< int > &pt_union_indices)
Get the 2nd derivative.
const HashMap & getCellHashMap() const
const int I_SHIFT_EP[12][2]
The 12 edges of a cell.
boost::shared_ptr< GridProjection< PointNT > > Ptr
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.
int getPaddingSize() const
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 setResolution(double resolution)
Set the size of the grid 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...
int getIndexIn1D(const Eigen::Vector3i &index) const
Given an index (x, y, z) in 3d, translate it into the index in 1d.
const int I_SHIFT_EDGE[3][2]
bool reconstructPolygons(std::vector< pcl::Vertices > &polygons)
The actual surface reconstruction method.
pcl::PointCloud< PointNT >::Ptr PointCloudPtr
KdTree represents the base spatial locator class for kd-tree implementations.
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.
double getResolution() const