48 #include "auxiliary.h"
50 #include <pcl/point_cloud.h>
51 #include <pcl/pcl_exports.h>
81 Data (
int id_x,
int id_y,
int id_z,
int lin_id,
void* user_data =
nullptr)
87 user_data_ (user_data)
89 n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f;
97 p_[0] += x; p_[1] += y; p_[2] += z;
104 if ( num_points_ < 2 )
107 aux::mult3 (p_, 1.0f/
static_cast<float> (num_points_));
112 addToNormal (
float x,
float y,
float z) { n_[0] += x; n_[1] += y; n_[2] += z;}
155 inline const std::set<Node*>&
173 this->deleteChildren ();
178 setCenter(
const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];}
181 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];}
193 float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])};
206 memcpy (b, bounds_, 6*
sizeof (
float));
228 hasData (){
return static_cast<bool> (data_);}
259 if ( !this->getData () || !node->
getData () )
262 this->getData ()->insertNeighbor (node);
283 build (
const PointCloudIn& points,
float voxel_size,
const PointCloudN* normals =
nullptr,
float enlarge_bounds = 0.00001f);
288 build (
const float* bounds,
float voxel_size);
298 if ( x < bounds_[0] || x > bounds_[1] ||
299 y < bounds_[2] || y > bounds_[3] ||
300 z < bounds_[4] || z > bounds_[5] )
310 for (
int l = 0 ; l < tree_levels_ ; ++l )
316 if ( x >= c[0] )
id |= 4;
317 if ( y >= c[1] )
id |= 2;
318 if ( z >= c[2] )
id |= 1;
326 static_cast<int> ((node->
getCenter ()[0] - bounds_[0])/voxel_size_),
327 static_cast<int> ((node->
getCenter ()[1] - bounds_[2])/voxel_size_),
328 static_cast<int> ((node->
getCenter ()[2] - bounds_[4])/voxel_size_),
329 static_cast<int> (full_leaves_.size ()));
332 this->insertNeighbors (node);
333 full_leaves_.push_back (node);
346 getFullLeavesIntersectedBySphere (
const float* p,
float radius, std::list<ORROctree::Node*>& out)
const;
351 getRandomFullLeafOnSphere (
const float* p,
float radius)
const;
358 float offset = 0.5f*voxel_size_;
359 float p[3] = {bounds_[0] + offset +
static_cast<float> (i)*voxel_size_,
360 bounds_[2] + offset +
static_cast<float> (j)*voxel_size_,
361 bounds_[4] + offset +
static_cast<float> (k)*voxel_size_};
363 return (this->getLeaf (p[0], p[1], p[2]));
371 if ( x < bounds_[0] || x > bounds_[1] ||
372 y < bounds_[2] || y > bounds_[3] ||
373 z < bounds_[4] || z > bounds_[5] )
383 for (
int l = 0 ; l < tree_levels_ ; ++l )
391 if ( x >= c[0] )
id |= 4;
392 if ( y >= c[1] )
id |= 2;
393 if ( z >= c[2] )
id |= 1;
403 deleteBranch (Node* node);
406 inline std::vector<ORROctree::Node*>&
409 inline const std::vector<ORROctree::Node*>&
413 getFullLeavesPoints (PointCloudOut& out)
const;
416 getNormalsOfFullLeaves (PointCloudN& out)
const;
430 memcpy (b, bounds_, 6*
sizeof (
float));
440 float s = 0.5f*voxel_size_;
443 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
444 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
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] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
447 neigh = this->getLeaf (c[0]+s, c[1] , c[2] );
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]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
450 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
451 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
453 neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
454 neigh = this->getLeaf (c[0] , c[1]+s, c[2] );
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] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
458 neigh = this->getLeaf (c[0] , c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
459 neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
460 neigh = this->getLeaf (c[0] , c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
461 neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
463 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
464 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] );
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] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
467 neigh = this->getLeaf (c[0]-s, c[1] , c[2] );
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]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
470 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
471 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);