48 #include "auxiliary.h"
49 #include <pcl/point_types.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/pcl_exports.h>
83 Data (
int id_x,
int id_y,
int id_z,
int lin_id,
void* user_data =
nullptr)
89 user_data_ (user_data)
91 n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f;
99 p_[0] += x; p_[1] += y; p_[2] += z;
106 if ( num_points_ < 2 )
109 aux::mult3 (p_, 1.0f/static_cast<float> (num_points_));
114 addToNormal (
float x,
float y,
float z) { n_[0] += x; n_[1] += y; n_[2] += z;}
157 inline const std::set<Node*>&
175 this->deleteChildren ();
180 setCenter(
const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];}
183 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];}
195 float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])};
208 memcpy (b, bounds_, 6*
sizeof (
float));
230 hasData (){
return static_cast<bool> (data_);}
261 if ( !this->getData () || !node->
getData () )
264 this->getData ()->insertNeighbor (node);
285 build (
const PointCloudIn& points,
float voxel_size,
const PointCloudN* normals =
nullptr,
float enlarge_bounds = 0.00001f);
290 build (
const float* bounds,
float voxel_size);
300 if ( x < bounds_[0] || x > bounds_[1] ||
301 y < bounds_[2] || y > bounds_[3] ||
302 z < bounds_[4] || z > bounds_[5] )
312 for (
int l = 0 ; l < tree_levels_ ; ++l )
318 if ( x >= c[0] )
id |= 4;
319 if ( y >= c[1] )
id |= 2;
320 if ( z >= c[2] )
id |= 1;
328 static_cast<int> ((node->
getCenter ()[0] - bounds_[0])/voxel_size_),
329 static_cast<int> ((node->
getCenter ()[1] - bounds_[2])/voxel_size_),
330 static_cast<int> ((node->
getCenter ()[2] - bounds_[4])/voxel_size_),
331 static_cast<int> (full_leaves_.size ()));
334 this->insertNeighbors (node);
335 full_leaves_.push_back (node);
348 getFullLeavesIntersectedBySphere (
const float* p,
float radius, std::list<ORROctree::Node*>& out)
const;
353 getRandomFullLeafOnSphere (
const float* p,
float radius)
const;
360 float offset = 0.5f*voxel_size_;
361 float p[3] = {bounds_[0] + offset +
static_cast<float> (i)*voxel_size_,
362 bounds_[2] + offset + static_cast<float> (j)*voxel_size_,
363 bounds_[4] + offset +
static_cast<float> (k)*voxel_size_};
365 return (this->getLeaf (p[0], p[1], p[2]));
373 if ( x < bounds_[0] || x > bounds_[1] ||
374 y < bounds_[2] || y > bounds_[3] ||
375 z < bounds_[4] || z > bounds_[5] )
385 for (
int l = 0 ; l < tree_levels_ ; ++l )
393 if ( x >= c[0] )
id |= 4;
394 if ( y >= c[1] )
id |= 2;
395 if ( z >= c[2] )
id |= 1;
405 deleteBranch (Node* node);
408 inline std::vector<ORROctree::Node*>&
411 inline const std::vector<ORROctree::Node*>&
415 getFullLeavesPoints (PointCloudOut& out)
const;
418 getNormalsOfFullLeaves (PointCloudN& out)
const;
432 memcpy (b, bounds_, 6*
sizeof (
float));
442 float s = 0.5f*voxel_size_;
445 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
446 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
447 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
448 neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
449 neigh = this->getLeaf (c[0]+s, c[1] , c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
450 neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
451 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
452 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
453 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
455 neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
456 neigh = this->getLeaf (c[0] , c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
457 neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
458 neigh = this->getLeaf (c[0] , c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
460 neigh = this->getLeaf (c[0] , c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
461 neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
462 neigh = this->getLeaf (c[0] , c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
463 neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
465 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
466 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
467 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
468 neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
469 neigh = this->getLeaf (c[0]-s, c[1] , c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
470 neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
471 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
472 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
473 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)
const Node::Data * getData() const
void setUserData(void *user_data)
void mult3(T *v, T scalar)
v = scalar*v.
Data(int id_x, int id_y, int id_z, int lin_id, void *user_data=nullptr)
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
const float * getBounds() const
ORROctree::Node * getRoot()
void setData(Node::Data *data)
float getRadius() const
Computes the "radius" of the node which is half the diagonal length.
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
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)