46 #ifndef PCL_RECOGNITION_ORR_OCTREE_H_ 47 #define PCL_RECOGNITION_ORR_OCTREE_H_ 49 #include "auxiliary.h" 50 #include <pcl/point_types.h> 51 #include <pcl/point_cloud.h> 52 #include <pcl/pcl_exports.h> 84 Data (
int id_x,
int id_y,
int id_z,
int lin_id,
void* user_data = NULL)
90 user_data_ (user_data)
92 n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f;
100 p_[0] += x; p_[1] += y; p_[2] += z;
107 if ( num_points_ < 2 )
110 aux::mult3 (p_, 1.0f/static_cast<float> (num_points_));
115 addToNormal (
float x,
float y,
float z) { n_[0] += x; n_[1] += y; n_[2] += z;}
158 inline const std::set<Node*>&
176 this->deleteChildren ();
181 setCenter(
const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];}
184 setBounds(
const float *b) { bounds_[0] = b[0]; bounds_[1] = b[1]; bounds_[2] = b[2]; bounds_[3] = b[3]; bounds_[4] = b[4]; bounds_[5] = b[5];}
196 float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])};
209 memcpy (b, bounds_, 6*
sizeof (
float));
231 hasData (){
return static_cast<bool> (data_);}
268 if ( !this->getData () || !node->
getData () )
271 this->getData ()->insertNeighbor (node);
292 build (
const PointCloudIn& points,
float voxel_size,
const PointCloudN* normals = NULL,
float enlarge_bounds = 0.00001f);
297 build (
const float* bounds,
float voxel_size);
307 if ( x < bounds_[0] || x > bounds_[1] ||
308 y < bounds_[2] || y > bounds_[3] ||
309 z < bounds_[4] || z > bounds_[5] )
319 for (
int l = 0 ; l < tree_levels_ ; ++l )
325 if ( x >= c[0] )
id |= 4;
326 if ( y >= c[1] )
id |= 2;
327 if ( z >= c[2] )
id |= 1;
335 static_cast<int> ((node->
getCenter ()[0] - bounds_[0])/voxel_size_),
336 static_cast<int> ((node->
getCenter ()[1] - bounds_[2])/voxel_size_),
337 static_cast<int> ((node->
getCenter ()[2] - bounds_[4])/voxel_size_),
338 static_cast<int> (full_leaves_.size ()));
341 this->insertNeighbors (node);
342 full_leaves_.push_back (node);
355 getFullLeavesIntersectedBySphere (
const float* p,
float radius, std::list<ORROctree::Node*>& out)
const;
360 getRandomFullLeafOnSphere (
const float* p,
float radius)
const;
367 float offset = 0.5f*voxel_size_;
368 float p[3] = {bounds_[0] + offset +
static_cast<float> (i)*voxel_size_,
369 bounds_[2] + offset + static_cast<float> (j)*voxel_size_,
370 bounds_[4] + offset +
static_cast<float> (k)*voxel_size_};
372 return (this->getLeaf (p[0], p[1], p[2]));
380 if ( x < bounds_[0] || x > bounds_[1] ||
381 y < bounds_[2] || y > bounds_[3] ||
382 z < bounds_[4] || z > bounds_[5] )
392 for (
int l = 0 ; l < tree_levels_ ; ++l )
400 if ( x >= c[0] )
id |= 4;
401 if ( y >= c[1] )
id |= 2;
402 if ( z >= c[2] )
id |= 1;
412 deleteBranch (
Node* node);
415 inline std::vector<ORROctree::Node*>&
418 inline const std::vector<ORROctree::Node*>&
422 getFullLeavesPoints (PointCloudOut& out)
const;
425 getNormalsOfFullLeaves (PointCloudN& out)
const;
439 memcpy (b, bounds_, 6*
sizeof (
float));
449 float s = 0.5f*voxel_size_;
452 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
453 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
454 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
455 neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
456 neigh = this->getLeaf (c[0]+s, c[1] , c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
457 neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
458 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
459 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
460 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
462 neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
463 neigh = this->getLeaf (c[0] , c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
464 neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
465 neigh = this->getLeaf (c[0] , c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
467 neigh = this->getLeaf (c[0] , c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
468 neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
469 neigh = this->getLeaf (c[0] , c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
470 neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
472 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
473 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
474 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
475 neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
476 neigh = this->getLeaf (c[0]-s, c[1] , c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
477 neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
478 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
479 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
480 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
const float * getNormal() const
void insertNeighbor(Node *node)
float getRadius()
Computes the "radius" of the node which is half the diagonal length.
const Node::Data * getData() const
void setUserData(void *user_data)
void mult3(T *v, T scalar)
v = scalar*v.
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.
ORROctree::Node * createLeaf(float x, float y, float z)
Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within...
void getBounds(float b[6]) const
const std::set< Node * > & getNeighbors() const
const float * getCenter() const
float getVoxelSize() const
std::vector< ORROctree::Node * > & getFullLeaves()
Returns a vector with all octree leaves which contain at least one point.
void setCenter(const float *c)
const float * getBounds() const
pcl::PointCloud< pcl::PointXYZ > PointCloudOut
Data(int id_x, int id_y, int id_z, int lin_id, void *user_data=NULL)
const float * getBounds() const
ORROctree::Node * getRoot()
pcl::PointCloud< pcl::PointXYZ > PointCloudIn
void setData(Node::Data *data)
void insertNeighbors(Node *node)
void * getUserData() const
void setUserData(void *user_data)
std::vector< Node * > full_leaves_
void setBounds(const float *b)
ORROctree::Node * getLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.
T length3(const T v[3])
Returns the length of v.
ORROctree::Node * getLeaf(float x, float y, float z)
Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists.
void getBounds(float b[6]) const
pcl::PointCloud< pcl::Normal > PointCloudN
void computeAveragePoint()
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set...
const float * getPoint() const
void get3dId(int id[3]) const
const std::vector< ORROctree::Node * > & getFullLeaves() const
void addToNormal(float x, float y, float z)
std::set< Node * > neighbors_
void setParent(Node *parent)
That's a very specialized and simple octree class.
void addToPoint(float x, float y, float z)