48 #include "auxiliary.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_);}
267 if ( !this->getData () || !node->
getData () )
270 this->getData ()->insertNeighbor (node);
291 build (
const PointCloudIn& points,
float voxel_size,
const PointCloudN* normals =
nullptr,
float enlarge_bounds = 0.00001f);
296 build (
const float* bounds,
float voxel_size);
306 if ( x < bounds_[0] || x > bounds_[1] ||
307 y < bounds_[2] || y > bounds_[3] ||
308 z < bounds_[4] || z > bounds_[5] )
318 for (
int l = 0 ; l < tree_levels_ ; ++l )
324 if ( x >= c[0] )
id |= 4;
325 if ( y >= c[1] )
id |= 2;
326 if ( z >= c[2] )
id |= 1;
334 static_cast<int> ((node->
getCenter ()[0] - bounds_[0])/voxel_size_),
335 static_cast<int> ((node->
getCenter ()[1] - bounds_[2])/voxel_size_),
336 static_cast<int> ((node->
getCenter ()[2] - bounds_[4])/voxel_size_),
337 static_cast<int> (full_leaves_.size ()));
340 this->insertNeighbors (node);
341 full_leaves_.push_back (node);
354 getFullLeavesIntersectedBySphere (
const float* p,
float radius, std::list<ORROctree::Node*>& out)
const;
359 getRandomFullLeafOnSphere (
const float* p,
float radius)
const;
366 float offset = 0.5f*voxel_size_;
367 float p[3] = {bounds_[0] + offset + static_cast<float> (i)*voxel_size_,
368 bounds_[2] + offset + static_cast<float> (j)*voxel_size_,
369 bounds_[4] + offset + static_cast<float> (k)*voxel_size_};
371 return (this->getLeaf (p[0], p[1], p[2]));
379 if ( x < bounds_[0] || x > bounds_[1] ||
380 y < bounds_[2] || y > bounds_[3] ||
381 z < bounds_[4] || z > bounds_[5] )
391 for (
int l = 0 ; l < tree_levels_ ; ++l )
399 if ( x >= c[0] )
id |= 4;
400 if ( y >= c[1] )
id |= 2;
401 if ( z >= c[2] )
id |= 1;
411 deleteBranch (Node* node);
414 inline std::vector<ORROctree::Node*>&
417 inline const std::vector<ORROctree::Node*>&
421 getFullLeavesPoints (PointCloudOut& out)
const;
424 getNormalsOfFullLeaves (PointCloudN& out)
const;
438 memcpy (b, bounds_, 6*
sizeof (
float));
448 float s = 0.5f*voxel_size_;
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);
454 neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
455 neigh = this->getLeaf (c[0]+s, c[1] , c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
456 neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
457 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
458 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
459 neigh = this->getLeaf (c[0]+s, c[1]-s, 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);
464 neigh = this->getLeaf (c[0] , c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
466 neigh = this->getLeaf (c[0] , c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
467 neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
468 neigh = this->getLeaf (c[0] , c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
469 neigh = this->getLeaf (c[0] , c[1]-s, 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);
474 neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
475 neigh = this->getLeaf (c[0]-s, c[1] , c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
476 neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
477 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
478 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
479 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);