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_);}
261 if ( !this->getData () || !node->
getData () )
264 this->getData ()->insertNeighbor (node);
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);
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;
408 inline std::vector<ORROctree::Node*>&
411 inline const std::vector<ORROctree::Node*>&
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);