Grid projection surface reconstruction method. More...
#include <grid_projection.h>
Classes | |
struct | Leaf |
Data leaf. More... | |
Public Types | |
typedef boost::unordered_map < int, Leaf, boost::hash< int > , std::equal_to< int > , Eigen::aligned_allocator < int > > | HashMap |
typedef short int | Index3D [3] |
typedef pcl::KdTree< PointNT > | KdTree |
typedef pcl::KdTree< PointNT >::Ptr | KdTreePtr |
typedef pcl::PointCloud < PointNT >::Ptr | PointCloudPtr |
typedef Eigen::Vector3f | vector3d |
Public Member Functions | |
void | getBoundingBox () |
Get the bounding box for the input data points, also calculating the cell size, and the gaussian scale factor. | |
const HashMap & | getCellHashMap () const |
int | getMaxBinarySearchLevel () const |
int | getNearestNeighborNum () const |
int | getPaddingSize () const |
double | getResolution () const |
const std::vector < Eigen::Vector4f, Eigen::aligned_allocator < Eigen::Vector4f > > & | getSurface () const |
const std::vector< vector3d > & | getVectorAtDataPoint () const |
GridProjection (double in_resolution) | |
Constructor. | |
GridProjection () | |
Constructor. | |
void | performReconstruction (pcl::PolygonMesh &output) |
Create the surface. | |
void | setMaxBinarySearchLevel (int max_binary_search_level) |
Binary search is used in projection. given a point x, we find another point which is 3*cell_size_ far away from x. Then we do a binary search between these two points to find where the projected point should be. | |
void | setNearestNeighborNum (int k) |
Set this only when using the k nearest neighbors search instead of finding the point union. | |
void | setPaddingSize (int padding_size) |
When averaging the vectors, we find the union of all the input data points within the padding area,and do a weighted average. Say if the padding size is 1, when we process cell (x,y,z), we will find union of input data points from (x-1) to (x+1), (y-1) to (y+1), (z-1) to (z+1)(in total, 27 cells). In this way, even the cells itself doesnt contain any data points, we will stil process it because there are data points in the padding area. This can help us fix holes which is smaller than the padding size. | |
void | setResolution (double resolution) |
Set the size of the grid cell. | |
~GridProjection () | |
Destructor. | |
Protected Member Functions | |
void | createSurfaceForCell (const Index3D &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.the up, left, front edges only share 4 points, we first get the vectors at these 4 points and exam whether those three edges are intersected by the surface. | |
void | fillPad (const Index3D &index) |
For a given 3d index of a cell, test whether the cells within its padding area exist in the hash table, if no, create an entry for that cell. | |
Eigen::Vector4f | findIntersection (int level, const Eigen::Vector4f(&end_pts)[2], vector3d vect_at_end_pts[2], const Eigen::Vector4f &start_pt, std::vector< int > &pt_union_indices) |
Find point where the edge intersects the surface. | |
void | getCellCenterFromIndex (const Index3D &index, Eigen::Vector4f ¢er) const |
Given the 3d index (x, y, z) of the cell, get the coordinates of the cell center. | |
void | getCellIndex (const Eigen::Vector4f &p, Index3D &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 vector3d &vec, const std::vector< int > &pt_union_indices) |
Get the 1st derivative. | |
double | getD2AtPoint (const Eigen::Vector4f &p, const vector3d &vec, const std::vector< int > &pt_union_indices) |
Get the 2nd derivative. | |
void | getDataPtsUnion (const Index3D &index, std::vector< int > &pt_union_indices) |
Obtain the index of a cell and the pad size. | |
int | getIndexIn1D (Index3D index) const |
Given an index (x, y, z) in 3d, translate it into the index in 1d. | |
void | getIndexIn3D (int index_1d, Index3D &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. | |
Eigen::Vector4f | getProjection (const Eigen::Vector4f &p, std::vector< int > &pt_union_indices) |
Given the coordinates of one point, project it onto the surface, return the projected point. Do a binary search between p and p+projection_distance to find the projected point. | |
Eigen::Vector4f | getProjectionWithPlaneFit (const Eigen::Vector4f &p, std::vector< int > &pt_union_indices) |
Given the coordinates of one point, project it onto the surface, return the projected point. Find the plane which fits all the points in pt_union_indices, projected p to the plane to get the projected point. | |
Eigen::Vector3f | getVectorAtPoint (const Eigen::Vector4f &p, std::vector< int > &pt_union_indices) |
Given the location of a point, get it's vector. | |
Eigen::Vector3f | getVectorAtPointKNN (const Eigen::Vector4f &p, std::vector< int > &k_indices, std::vector< float > &k_squared_distances) |
Given the location of a point, get it's vector. | |
void | getVertexFromCellCenter (const Eigen::Vector4f &cell_center, Eigen::Vector4f(&pts)[8]) const |
Given cell center, caluate the coordinates of the eight vertices of the cell. | |
void | getVertexFromIndex (const Index3D &index, Eigen::Vector4f(&pts)[8]) const |
Given the index of cell, caluate the coordinates of the eight vertices of the cell. | |
bool | isIntersected (const Eigen::Vector4f(&end_pts)[2], vector3d vect_at_end_pts[2], 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 points. Also test whether the edge is intersected by the maximum surface by examing the 2nd derivative of the intersection point. | |
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 the unit box. Otherwise, it will be some drawing problem when doing visulization. | |
void | storeVectAndSurfacePoint (int index_1d, const Index3D &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 | storeVectAndSurfacePointKNN (int index_1d, Index3D index_3d, Leaf cell_data) |
Go through all the entries in the hash table and update the cellData. When creating the hash table, the pt_on_surface field store the center point of the cell.After calling this function, the projection operator will project the center point onto the surface, and the pt_on_surface field will be updated using the projected point.Also the vect_at_grid_pt field will be updated using the vector at the upper left front vertex of the cell. When projecting the point and calculating the vector, using K nearest neighbors instead of using the union of input data point within the cell and pads. | |
Private Member Functions | |
std::string | getClassName () const |
Class get name method. | |
Private Attributes | |
HashMap | cell_hash_map_ |
Map containing the set of leaves. | |
PointCloudPtr | data_ |
The point cloud input (XYZ+Normals). | |
int | data_size_ |
Data size. | |
double | gaussian_scale_ |
Gaussian scale. | |
int | k_ |
Number of neighbors (k) to use. | |
double | leaf_size_ |
The size of a leaf. | |
int | max_binary_search_level_ |
Max binary search level. | |
Eigen::Vector4f | max_p_ |
Eigen::Vector4f | min_p_ |
Min and max data points. | |
boost::dynamic_bitset | occupied_cell_list_ |
Bit map which tells if there is any input data point in the cell. | |
int | padding_size_ |
Padding size. | |
std::vector< Eigen::Vector4f, Eigen::aligned_allocator < Eigen::Vector4f > > | surface_ |
An array of points which lay on the output surface. | |
std::vector< vector3d > | vector_at_data_point_ |
Store the surface normal(vector) at the each input data point. |
Grid projection surface reconstruction method.
Definition at line 67 of file grid_projection.h.
typedef boost::unordered_map<int, Leaf, boost::hash<int>, std::equal_to<int>, Eigen::aligned_allocator<int> > pcl::GridProjection< PointNT >::HashMap |
Definition at line 89 of file grid_projection.h.
typedef short int pcl::GridProjection< PointNT >::Index3D[3] |
Definition at line 78 of file grid_projection.h.
typedef pcl::KdTree<PointNT> pcl::GridProjection< PointNT >::KdTree |
Reimplemented from pcl::SurfaceReconstruction< PointNT >.
Definition at line 75 of file grid_projection.h.
typedef pcl::KdTree<PointNT>::Ptr pcl::GridProjection< PointNT >::KdTreePtr |
Reimplemented from pcl::SurfaceReconstruction< PointNT >.
Definition at line 76 of file grid_projection.h.
typedef pcl::PointCloud<PointNT>::Ptr pcl::GridProjection< PointNT >::PointCloudPtr |
Reimplemented from pcl::PCLBase< PointNT >.
Definition at line 73 of file grid_projection.h.
typedef Eigen::Vector3f pcl::GridProjection< PointNT >::vector3d |
Definition at line 79 of file grid_projection.h.
pcl::GridProjection< PointNT >::GridProjection | ( | ) | [inline] |
Constructor.
Definition at line 49 of file grid_projection.hpp.
pcl::GridProjection< PointNT >::GridProjection | ( | double | in_resolution | ) | [inline] |
Constructor.
in_resolution | set the resolution of the grid |
Definition at line 55 of file grid_projection.hpp.
pcl::GridProjection< PointNT >::~GridProjection | ( | ) | [inline] |
Destructor.
Definition at line 61 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::createSurfaceForCell | ( | const Index3D & | index, | |
std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Given the index of a cell, exam it's up, left, front edges, and add the vectices to m_surface list.the up, left, front edges only share 4 points, we first get the vectors at these 4 points and exam whether those three edges are intersected by the surface.
index | the input index | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 175 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::fillPad | ( | const Index3D & | index | ) | [inline, protected] |
For a given 3d index of a cell, test whether the cells within its padding area exist in the hash table, if no, create an entry for that cell.
index | the index of the cell in (x,y,z) format |
Definition at line 548 of file grid_projection.hpp.
Eigen::Vector4f pcl::GridProjection< PointNT >::findIntersection | ( | int | level, | |
const Eigen::Vector4f(&) | end_pts[2], | |||
vector3d | vect_at_end_pts[2], | |||
const Eigen::Vector4f & | start_pt, | |||
std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Find point where the edge intersects the surface.
level | binary search level | |
end_pts | the two end points on the edge | |
vect_at_end_pts | the vectors at the two end points | |
start_pt | the starting point we use for binary search | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 506 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::getBoundingBox | ( | ) | [inline] |
Get the bounding box for the input data points, also calculating the cell size, and the gaussian scale factor.
Definition at line 81 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::getCellCenterFromIndex | ( | const Index3D & | index, | |
Eigen::Vector4f & | center | |||
) | const [inline, protected] |
Given the 3d index (x, y, z) of the cell, get the coordinates of the cell center.
index | the output 3d index | |
center | the resultant cell center |
Definition at line 231 of file grid_projection.h.
const HashMap& pcl::GridProjection< PointNT >::getCellHashMap | ( | ) | const [inline] |
Definition at line 181 of file grid_projection.h.
void pcl::GridProjection< PointNT >::getCellIndex | ( | const Eigen::Vector4f & | p, | |
Index3D & | index | |||
) | const [inline, protected] |
Get the 3d index (x,y,z) of the cell based on the location of the cell.
p | the coordinate of the input point | |
index | the output 3d index |
Definition at line 219 of file grid_projection.h.
std::string pcl::GridProjection< PointNT >::getClassName | ( | ) | const [inline, private, virtual] |
Class get name method.
Reimplemented from pcl::SurfaceReconstruction< PointNT >.
Definition at line 458 of file grid_projection.h.
double pcl::GridProjection< PointNT >::getD1AtPoint | ( | const Eigen::Vector4f & | p, | |
const vector3d & | vec, | |||
const std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Get the 1st derivative.
p | the coordinate of the input point | |
vec | the vector at point p | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 451 of file grid_projection.hpp.
double pcl::GridProjection< PointNT >::getD2AtPoint | ( | const Eigen::Vector4f & | p, | |
const vector3d & | vec, | |||
const std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Get the 2nd derivative.
p | the coordinate of the input point | |
vec | the vector at point p | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 464 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::getDataPtsUnion | ( | const Index3D & | index, | |
std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Obtain the index of a cell and the pad size.
index | the input index | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 149 of file grid_projection.hpp.
int pcl::GridProjection< PointNT >::getIndexIn1D | ( | Index3D | index | ) | const [inline, protected] |
Given an index (x, y, z) in 3d, translate it into the index in 1d.
index | the index of the cell in (x,y,z) 3d format |
Definition at line 257 of file grid_projection.h.
void pcl::GridProjection< PointNT >::getIndexIn3D | ( | int | index_1d, | |
Index3D & | index_3d | |||
) | const [inline, protected] |
Given an index in 1d, translate it into the index (x, y, z) in 3d.
index_1d | the input 1d index | |
index_3d | the output 3d index |
Definition at line 270 of file grid_projection.h.
double pcl::GridProjection< PointNT >::getMagAtPoint | ( | const Eigen::Vector4f & | p, | |
const std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Get the magnitude of the vector by summing up the distance.
p | the coordinate of the input point | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 434 of file grid_projection.hpp.
int pcl::GridProjection< PointNT >::getMaxBinarySearchLevel | ( | ) | const [inline] |
Definition at line 174 of file grid_projection.h.
int pcl::GridProjection< PointNT >::getNearestNeighborNum | ( | ) | const [inline] |
Definition at line 159 of file grid_projection.h.
int pcl::GridProjection< PointNT >::getPaddingSize | ( | ) | const [inline] |
Definition at line 144 of file grid_projection.h.
Eigen::Vector4f pcl::GridProjection< PointNT >::getProjection | ( | const Eigen::Vector4f & | p, | |
std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Given the coordinates of one point, project it onto the surface, return the projected point. Do a binary search between p and p+projection_distance to find the projected point.
p | the coordinates of the input point | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 302 of file grid_projection.hpp.
Eigen::Vector4f pcl::GridProjection< PointNT >::getProjectionWithPlaneFit | ( | const Eigen::Vector4f & | p, | |
std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Given the coordinates of one point, project it onto the surface, return the projected point. Find the plane which fits all the points in pt_union_indices, projected p to the plane to get the projected point.
p | the coordinates of the input point | |
pt_union_indices | the union of input data points within the cell and padding cells |
: iterative weighted least squares or sac might give better results
Definition at line 336 of file grid_projection.hpp.
double pcl::GridProjection< PointNT >::getResolution | ( | ) | const [inline] |
Definition at line 124 of file grid_projection.h.
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& pcl::GridProjection< PointNT >::getSurface | ( | ) | const [inline] |
Definition at line 193 of file grid_projection.h.
const std::vector<vector3d>& pcl::GridProjection< PointNT >::getVectorAtDataPoint | ( | ) | const [inline] |
Definition at line 187 of file grid_projection.h.
Eigen::Vector3f pcl::GridProjection< PointNT >::getVectorAtPoint | ( | const Eigen::Vector4f & | p, | |
std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Given the location of a point, get it's vector.
p | the coordinates of the input point | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 358 of file grid_projection.hpp.
Eigen::Vector3f pcl::GridProjection< PointNT >::getVectorAtPointKNN | ( | const Eigen::Vector4f & | p, | |
std::vector< int > & | k_indices, | |||
std::vector< float > & | k_squared_distances | |||
) | [inline, protected] |
Given the location of a point, get it's vector.
p | the coordinates of the input point | |
pt_union_indices | the union of input data points within the cell and padding cells |
Definition at line 402 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::getVertexFromCellCenter | ( | const Eigen::Vector4f & | cell_center, | |
Eigen::Vector4f(&) | pts[8] | |||
) | const [inline, protected] |
Given cell center, caluate the coordinates of the eight vertices of the cell.
cell_center | the coordinates of the cell center | |
pts | the coordinates of the 8 vertices |
Definition at line 122 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::getVertexFromIndex | ( | const Index3D & | index, | |
Eigen::Vector4f(&) | pts[8] | |||
) | const [inline, protected] |
Given the index of cell, caluate the coordinates of the eight vertices of the cell.
index | the index of the cell in (x,y,z) 3d format | |
pts | the coordinates of the 8 vertices |
Definition at line 139 of file grid_projection.hpp.
bool pcl::GridProjection< PointNT >::isIntersected | ( | const Eigen::Vector4f(&) | end_pts[2], | |
vector3d | vect_at_end_pts[2], | |||
std::vector< int > & | pt_union_indices | |||
) | [inline, protected] |
Test whether the edge is intersected by the surface by doing the dot product of the vector at two end points. Also test whether the edge is intersected by the maximum surface by examing the 2nd derivative of the intersection point.
\param | pt_union_indices the union of input data points within the cell and padding cells |
Definition at line 477 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::performReconstruction | ( | pcl::PolygonMesh & | output | ) | [inline, virtual] |
Create the surface.
The 1st step is filling the padding, so that all the cells in the padding area are in the hash map. The 2nd step is store the vector, and projected point. The 3rd step is finding all the edges intersects the surface, and creating surface.
output | the resultant polygonal mesh |
Implements pcl::SurfaceReconstruction< PointNT >.
Definition at line 611 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::scaleInputDataPoint | ( | double | scale_factor | ) | [inline, protected] |
When the input data points don't fill into the 1*1*1 box, scale them so that they can be filled in the unit box. Otherwise, it will be some drawing problem when doing visulization.
scale_factor | scale all the input data point by scale_factor |
Definition at line 71 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::setMaxBinarySearchLevel | ( | int | max_binary_search_level | ) | [inline] |
Binary search is used in projection. given a point x, we find another point which is 3*cell_size_ far away from x. Then we do a binary search between these two points to find where the projected point should be.
Definition at line 169 of file grid_projection.h.
void pcl::GridProjection< PointNT >::setNearestNeighborNum | ( | int | k | ) | [inline] |
Set this only when using the k nearest neighbors search instead of finding the point union.
The | number of nearest neighbors we are looking for |
Definition at line 154 of file grid_projection.h.
void pcl::GridProjection< PointNT >::setPaddingSize | ( | int | padding_size | ) | [inline] |
When averaging the vectors, we find the union of all the input data points within the padding area,and do a weighted average. Say if the padding size is 1, when we process cell (x,y,z), we will find union of input data points from (x-1) to (x+1), (y-1) to (y+1), (z-1) to (z+1)(in total, 27 cells). In this way, even the cells itself doesnt contain any data points, we will stil process it because there are data points in the padding area. This can help us fix holes which is smaller than the padding size.
padding_size | The num of padding cells we want to create |
Definition at line 139 of file grid_projection.h.
void pcl::GridProjection< PointNT >::setResolution | ( | double | resolution | ) | [inline] |
Set the size of the grid cell.
resolution | the size of the grid cell |
Definition at line 118 of file grid_projection.h.
void pcl::GridProjection< PointNT >::storeVectAndSurfacePoint | ( | int | index_1d, | |
const Index3D & | index_3d, | |||
std::vector< int > & | pt_union_indices, | |||
const Leaf & | cell_data | |||
) | [inline, protected] |
Go through all the entries in the hash table and update the cellData.
When creating the hash table, the pt_on_surface field store the center point of the cell.After calling this function, the projection operator will project the center point onto the surface, and the pt_on_surface field will be updated using the projected point.Also the vect_at_grid_pt field will be updated using the vector at the upper left front vertex of the cell.
index_1d | the index of the cell after flatting it's 3d index into a 1d array | |
index_3d | the index of the cell in (x,y,z) 3d format | |
pt_union_indices | the union of input data points within the cell and pads | |
cell_data | information stored in the cell |
Definition at line 571 of file grid_projection.hpp.
void pcl::GridProjection< PointNT >::storeVectAndSurfacePointKNN | ( | int | index_1d, | |
Index3D | index_3d, | |||
Leaf | cell_data | |||
) | [inline, protected] |
Go through all the entries in the hash table and update the cellData. When creating the hash table, the pt_on_surface field store the center point of the cell.After calling this function, the projection operator will project the center point onto the surface, and the pt_on_surface field will be updated using the projected point.Also the vect_at_grid_pt field will be updated using the vector at the upper left front vertex of the cell. When projecting the point and calculating the vector, using K nearest neighbors instead of using the union of input data point within the cell and pads.
index_1d | the index of the cell after flatting it's 3d index into a 1d array | |
index_3d | the index of the cell in (x,y,z) 3d format | |
cell_data | information stored in the cell |
Definition at line 588 of file grid_projection.hpp.
HashMap pcl::GridProjection< PointNT >::cell_hash_map_ [private] |
Map containing the set of leaves.
Definition at line 422 of file grid_projection.h.
PointCloudPtr pcl::GridProjection< PointNT >::data_ [private] |
The point cloud input (XYZ+Normals).
Definition at line 446 of file grid_projection.h.
int pcl::GridProjection< PointNT >::data_size_ [private] |
Data size.
Definition at line 434 of file grid_projection.h.
double pcl::GridProjection< PointNT >::gaussian_scale_ [private] |
Gaussian scale.
Definition at line 431 of file grid_projection.h.
int pcl::GridProjection< PointNT >::k_ [private] |
Number of neighbors (k) to use.
Definition at line 440 of file grid_projection.h.
double pcl::GridProjection< PointNT >::leaf_size_ [private] |
The size of a leaf.
Definition at line 428 of file grid_projection.h.
int pcl::GridProjection< PointNT >::max_binary_search_level_ [private] |
Max binary search level.
Definition at line 437 of file grid_projection.h.
Eigen::Vector4f pcl::GridProjection< PointNT >::max_p_ [private] |
Definition at line 425 of file grid_projection.h.
Eigen::Vector4f pcl::GridProjection< PointNT >::min_p_ [private] |
Min and max data points.
Definition at line 425 of file grid_projection.h.
boost::dynamic_bitset pcl::GridProjection< PointNT >::occupied_cell_list_ [private] |
Bit map which tells if there is any input data point in the cell.
Definition at line 455 of file grid_projection.h.
int pcl::GridProjection< PointNT >::padding_size_ [private] |
Padding size.
Definition at line 443 of file grid_projection.h.
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > pcl::GridProjection< PointNT >::surface_ [private] |
An array of points which lay on the output surface.
Definition at line 452 of file grid_projection.h.
std::vector<vector3d> pcl::GridProjection< PointNT >::vector_at_data_point_ [private] |
Store the surface normal(vector) at the each input data point.
Definition at line 449 of file grid_projection.h.