Simox  2.3.50
VirtualRobot::VoxelTreeND< T, N > Class Template Reference

Data Structures

struct  ElementIterator
 

Public Member Functions

 VoxelTreeND (float minExtend[N], float maxExtend[N], float discretization[N], bool verbose=false)
 
virtual ~VoxelTreeND ()
 
bool setEntry (float pos[N], const T &e)
 
T * getEntry (float pos[N])
 
void getSize (float storeSize[N])
 
void getMinExtend (float storeMin[N])
 
void getMaxExtend (float storeMin[N])
 
void getDiscretization (float storeDiscretization[N])
 
void getRealDiscretization (float storeDiscretization[N])
 
VoxelTreeNDElement< T, N > * getLeafElement (float pos[N])
 
VoxelTreeNDElement< T, N > * getMaxEntry (const Eigen::VectorXf &p)
 
std::vector< VoxelTreeNDElement< T, N > * > getAllLeafs (const Eigen::VectorXf &p)
 
bool save (std::ofstream &file)
 
bool save (const std::string &filename)
 
bool isCovered (float p[N])
 
void print ()
 
int getMaxLevels ()
 
VoxelTreeNDElement< T, N > * getRoot ()
 
void getMemoryConsumtion (long &storeMemStructure, long &storeMemData)
 
long getNumNodes ()
 

Static Public Member Functions

static VoxelTreeND< T, N > * load (std::ifstream &file)
 
static VoxelTreeND< T, N > * load (const std::string &filename)
 

Protected Member Functions

float getExtends (int level, int dim)
 
int getNumChildren ()
 
void setRoot (VoxelTreeNDElement< T, N > *e)
 
unsigned int getNextID ()
 

Protected Attributes

float minExtend [N]
 
float maxExtend [N]
 
float size [N]
 
float discretization [N]
 
int maxLevels
 
bool verbose
 
unsigned int currentElementID
 
int num_children
 
Eigen::MatrixXf elementExtends
 
VoxelTreeNDElement< T, N > * root
 

Friends

class VoxelTreeNDElement< T, N >
 

Detailed Description

template<typename T, unsigned int N>
class VirtualRobot::VoxelTreeND< T, N >

A binary tree covering an N-dimensional space with voxels. On each tree level, 2^N children exist, each of which is either a leaf (and stores an entry) or it is further divided. The elements are created on demand, so it can hold sparse information.

The space extends are given by minExtend / maxExtend and the maximum voxel sizes are determined by the discretization parameters. Note that the worst minExtend/maxExtend and discretization set is chosen to determine the number of levels of the tree.

The following example shows how a 6D workspace consisting of position(-100mm -> +100mm) and orientation(-PI -> +PI, radian) can be coverd by this class. Each voxel covers a 20cm sized cube and rotation intervals of PI/10 radian for each RPY dimension. The voxel can hold an unsigned char value. float minExtend[6] = { -100.0f, -100.0f, -100.0f, -M_PI, -M_PI_2, -M_PI}; float maxExtend[6] = { 100.0f, 100.0f, 100.0f, M_PI, M_PI_2, M_PI}; float discr[6] = { 20.0f, 20.0f, 20.0f, M_PI/10.0f, M_PI/10.0f, M_PI/10.0f }; VoxelTreeND<unsigned char, 6> v(minExtend,maxExtend,discr);

float pos[6] = {10.0f,20.0f,0.0f, 0.1f,0.3f,-0.4f}; v.setEntry(pos,255); unsigned char *e = v.getEntry(pos); check if entry is set if (e) cout << "Entry is set:" << *e << endl;

Constructor & Destructor Documentation

template<typename T, unsigned int N>
VirtualRobot::VoxelTreeND< T, N >::VoxelTreeND ( float  minExtend[N],
float  maxExtend[N],
float  discretization[N],
bool  verbose = false 
)
inline
template<typename T, unsigned int N>
virtual VirtualRobot::VoxelTreeND< T, N >::~VoxelTreeND ( )
inlinevirtual

Member Function Documentation

template<typename T, unsigned int N>
std::vector< VoxelTreeNDElement<T, N>* > VirtualRobot::VoxelTreeND< T, N >::getAllLeafs ( const Eigen::VectorXf &  p)
inline

Gets all leaf elements of all sub-tree elements at position p (size of Vector p defines which level of the tree is considered).

template<typename T, unsigned int N>
void VirtualRobot::VoxelTreeND< T, N >::getDiscretization ( float  storeDiscretization[N])
inline

Returns discretization vector as defined on construction. (The actual discretization may differ, depending on max tree depth)

template<typename T, unsigned int N>
T* VirtualRobot::VoxelTreeND< T, N >::getEntry ( float  pos[N])
inline

Returns entry at pos. If pos is outside the space representation or no data stored at pos, NULL is returned.

template<typename T, unsigned int N>
float VirtualRobot::VoxelTreeND< T, N >::getExtends ( int  level,
int  dim 
)
inlineprotected

Returns voxel extends of element in given level (0<=level<maxLevels) and dimension dim (0<=dim<N)

template<typename T, unsigned int N>
VoxelTreeNDElement<T, N>* VirtualRobot::VoxelTreeND< T, N >::getLeafElement ( float  pos[N])
inline

Returns leaf at position pos. NULL if no data stored.

template<typename T, unsigned int N>
VoxelTreeNDElement<T, N>* VirtualRobot::VoxelTreeND< T, N >::getMaxEntry ( const Eigen::VectorXf &  p)
inline

Gets leaf element with max entry T of all sub-tree elements at position p (size of Vector p defines which level of the tree is considered). If no entry is stored below p NULL is returned

template<typename T, unsigned int N>
void VirtualRobot::VoxelTreeND< T, N >::getMaxExtend ( float  storeMin[N])
inline

Returns max position of voxelized data structure (in local coordinate system)

template<typename T, unsigned int N>
int VirtualRobot::VoxelTreeND< T, N >::getMaxLevels ( )
inline
template<typename T, unsigned int N>
void VirtualRobot::VoxelTreeND< T, N >::getMemoryConsumtion ( long &  storeMemStructure,
long &  storeMemData 
)
inline
template<typename T, unsigned int N>
void VirtualRobot::VoxelTreeND< T, N >::getMinExtend ( float  storeMin[N])
inline

Returns min position of voxelized data structure (in local coordinate system)

template<typename T, unsigned int N>
unsigned int VirtualRobot::VoxelTreeND< T, N >::getNextID ( )
inlineprotected
template<typename T, unsigned int N>
int VirtualRobot::VoxelTreeND< T, N >::getNumChildren ( )
inlineprotected
template<typename T, unsigned int N>
long VirtualRobot::VoxelTreeND< T, N >::getNumNodes ( )
inline

Retruns number of all Nodes, including inner and leaf nodes.

template<typename T, unsigned int N>
void VirtualRobot::VoxelTreeND< T, N >::getRealDiscretization ( float  storeDiscretization[N])
inline

Get real min element sizes.

template<typename T, unsigned int N>
VoxelTreeNDElement<T, N>* VirtualRobot::VoxelTreeND< T, N >::getRoot ( )
inline
template<typename T, unsigned int N>
void VirtualRobot::VoxelTreeND< T, N >::getSize ( float  storeSize[N])
inline

Returns size of voxelized data structure (in local coordinate system)

template<typename T, unsigned int N>
bool VirtualRobot::VoxelTreeND< T, N >::isCovered ( float  p[N])
inline
template<typename T, unsigned int N>
static VoxelTreeND<T, N>* VirtualRobot::VoxelTreeND< T, N >::load ( std::ifstream &  file)
inlinestatic
template<typename T, unsigned int N>
static VoxelTreeND<T, N>* VirtualRobot::VoxelTreeND< T, N >::load ( const std::string &  filename)
inlinestatic
template<typename T, unsigned int N>
void VirtualRobot::VoxelTreeND< T, N >::print ( )
inline
template<typename T, unsigned int N>
bool VirtualRobot::VoxelTreeND< T, N >::save ( std::ofstream &  file)
inline
template<typename T, unsigned int N>
bool VirtualRobot::VoxelTreeND< T, N >::save ( const std::string &  filename)
inline
template<typename T, unsigned int N>
bool VirtualRobot::VoxelTreeND< T, N >::setEntry ( float  pos[N],
const T &  e 
)
inline

Store entry to this voxel grid. Creates a leaf if necessary. Existing entries are silently overwritten. A copy of e is created.

template<typename T, unsigned int N>
void VirtualRobot::VoxelTreeND< T, N >::setRoot ( VoxelTreeNDElement< T, N > *  e)
inlineprotected

Friends And Related Function Documentation

template<typename T, unsigned int N>
friend class VoxelTreeNDElement< T, N >
friend

Field Documentation

template<typename T, unsigned int N>
unsigned int VirtualRobot::VoxelTreeND< T, N >::currentElementID
protected
template<typename T, unsigned int N>
float VirtualRobot::VoxelTreeND< T, N >::discretization[N]
protected
template<typename T, unsigned int N>
Eigen::MatrixXf VirtualRobot::VoxelTreeND< T, N >::elementExtends
protected
template<typename T, unsigned int N>
float VirtualRobot::VoxelTreeND< T, N >::maxExtend[N]
protected
template<typename T, unsigned int N>
int VirtualRobot::VoxelTreeND< T, N >::maxLevels
protected
template<typename T, unsigned int N>
float VirtualRobot::VoxelTreeND< T, N >::minExtend[N]
protected
template<typename T, unsigned int N>
int VirtualRobot::VoxelTreeND< T, N >::num_children
protected
template<typename T, unsigned int N>
VoxelTreeNDElement<T, N>* VirtualRobot::VoxelTreeND< T, N >::root
protected
template<typename T, unsigned int N>
float VirtualRobot::VoxelTreeND< T, N >::size[N]
protected
template<typename T, unsigned int N>
bool VirtualRobot::VoxelTreeND< T, N >::verbose
protected