Classes | Public Member Functions | Protected Member Functions | Static Protected Attributes | List of all members
octomap::ColorOcTree Class Reference

#include <ColorOcTree.h>

Inheritance diagram for octomap::ColorOcTree:
Inheritance graph
[legend]

Classes

class  StaticMemberInitializer
 

Public Member Functions

ColorOcTreeNodeaverageNodeColor (const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
 
ColorOcTreeNodeaverageNodeColor (float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
 
 ColorOcTree (double resolution)
 Default constructor, sets resolution of leafs. More...
 
ColorOcTreecreate () const
 
std::string getTreeType () const
 returns actual class name as string for identification More...
 
ColorOcTreeNodeintegrateNodeColor (const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
 
ColorOcTreeNodeintegrateNodeColor (float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
 
virtual bool isNodeCollapsible (const ColorOcTreeNode *node) const
 
virtual bool pruneNode (ColorOcTreeNode *node)
 
ColorOcTreeNodesetNodeColor (const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
 
ColorOcTreeNodesetNodeColor (float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
 
void updateInnerOccupancy ()
 
void writeColorHistogram (std::string filename)
 
- Public Member Functions inherited from octomap::OccupancyOcTreeBase< ColorOcTreeNode >
bool bbxSet () const
 
virtual bool castRay (const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
 
KeyBoolMap::const_iterator changedKeysBegin () const
 
KeyBoolMap::const_iterator changedKeysEnd () const
 Iterator to traverse all keys of changed nodes. More...
 
void computeDiscreteUpdate (const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
 
void computeUpdate (const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
 
void enableChangeDetection (bool enable)
 track or ignore changes while inserting scans (default: ignore) More...
 
point3d getBBXBounds () const
 
point3d getBBXCenter () const
 
point3d getBBXMax () const
 
point3d getBBXMin () const
 
bool getNormals (const point3d &point, std::vector< point3d > &normals, bool unknownStatus=true) const
 
virtual bool getRayIntersection (const point3d &origin, const point3d &direction, const point3d &center, point3d &intersection, double delta=0.0) const
 
bool inBBX (const point3d &p) const
 
bool inBBX (const OcTreeKey &key) const
 
virtual void insertPointCloud (const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
 
virtual void insertPointCloud (const Pointcloud &scan, const point3d &sensor_origin, const pose6d &frame_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
 
virtual void insertPointCloud (const ScanNode &scan, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
 
virtual void insertPointCloudRays (const Pointcloud &scan, const point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false)
 
virtual bool insertRay (const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
 
virtual void integrateHit (ColorOcTreeNode *occupancyNode) const
 integrate a "hit" measurement according to the tree's sensor model More...
 
virtual void integrateMiss (ColorOcTreeNode *occupancyNode) const
 integrate a "miss" measurement according to the tree's sensor model More...
 
bool isChangeDetectionEnabled () const
 
virtual void nodeToMaxLikelihood (ColorOcTreeNode *occupancyNode) const
 converts the node to the maximum likelihood value according to the tree's parameter for "occupancy" More...
 
virtual void nodeToMaxLikelihood (ColorOcTreeNode &occupancyNode) const
 converts the node to the maximum likelihood value according to the tree's parameter for "occupancy" More...
 
size_t numChangesDetected () const
 Number of changes since last reset. More...
 
 OccupancyOcTreeBase (double resolution)
 Default constructor, sets resolution of leafs. More...
 
 OccupancyOcTreeBase (const OccupancyOcTreeBase< ColorOcTreeNode > &rhs)
 Copy constructor. More...
 
std::istream & readBinaryData (std::istream &s)
 
std::istream & readBinaryNode (std::istream &s, ColorOcTreeNode *node)
 
void resetChangeDetection ()
 Reset the set of changed keys. Call this after you obtained all changed nodes. More...
 
void setBBXMax (point3d &max)
 sets the maximum for a query bounding box to use More...
 
void setBBXMin (point3d &min)
 sets the minimum for a query bounding box to use More...
 
virtual ColorOcTreeNodesetNodeValue (const OcTreeKey &key, float log_odds_value, bool lazy_eval=false)
 
virtual ColorOcTreeNodesetNodeValue (const point3d &value, float log_odds_value, bool lazy_eval=false)
 
virtual ColorOcTreeNodesetNodeValue (double x, double y, double z, float log_odds_value, bool lazy_eval=false)
 
virtual void toMaxLikelihood ()
 
void updateInnerOccupancy ()
 
virtual ColorOcTreeNodeupdateNode (const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
 
virtual ColorOcTreeNodeupdateNode (const point3d &value, float log_odds_update, bool lazy_eval=false)
 
virtual ColorOcTreeNodeupdateNode (double x, double y, double z, float log_odds_update, bool lazy_eval=false)
 
virtual ColorOcTreeNodeupdateNode (const OcTreeKey &key, bool occupied, bool lazy_eval=false)
 
virtual ColorOcTreeNodeupdateNode (const point3d &value, bool occupied, bool lazy_eval=false)
 
virtual ColorOcTreeNodeupdateNode (double x, double y, double z, bool occupied, bool lazy_eval=false)
 
virtual void updateNodeLogOdds (ColorOcTreeNode *occupancyNode, const float &update) const
 update logodds value of node by adding to the current value. More...
 
void useBBXLimit (bool enable)
 use or ignore BBX limit (default: ignore) More...
 
std::ostream & writeBinaryData (std::ostream &s) const
 
std::ostream & writeBinaryNode (std::ostream &s, const ColorOcTreeNode *node) const
 
virtual ~OccupancyOcTreeBase ()
 
- Public Member Functions inherited from octomap::OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree >
OcTreeKey adjustKeyAtDepth (const OcTreeKey &key, unsigned int depth) const
 
key_type adjustKeyAtDepth (key_type key, unsigned int depth) const
 
iterator begin (unsigned char maxDepth=0) const
 
leaf_iterator begin_leafs (unsigned char maxDepth=0) const
 
leaf_bbx_iterator begin_leafs_bbx (const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
 
leaf_bbx_iterator begin_leafs_bbx (const point3d &min, const point3d &max, unsigned char maxDepth=0) const
 
tree_iterator begin_tree (unsigned char maxDepth=0) const
 
size_t calcNumNodes () const
 Traverses the tree to calculate the total number of nodes. More...
 
void clear ()
 Deletes the complete tree structure. More...
 
void clearKeyRays ()
 
bool computeRay (const point3d &origin, const point3d &end, std::vector< point3d > &ray)
 
bool computeRayKeys (const point3d &origin, const point3d &end, KeyRay &ray) const
 
key_type coordToKey (double coordinate) const
 Converts from a single coordinate into a discrete key. More...
 
key_type coordToKey (double coordinate, unsigned depth) const
 Converts from a single coordinate into a discrete key at a given depth. More...
 
OcTreeKey coordToKey (const point3d &coord) const
 Converts from a 3D coordinate into a 3D addressing key. More...
 
OcTreeKey coordToKey (double x, double y, double z) const
 Converts from a 3D coordinate into a 3D addressing key. More...
 
OcTreeKey coordToKey (const point3d &coord, unsigned depth) const
 Converts from a 3D coordinate into a 3D addressing key at a given depth. More...
 
OcTreeKey coordToKey (double x, double y, double z, unsigned depth) const
 Converts from a 3D coordinate into a 3D addressing key at a given depth. More...
 
bool coordToKeyChecked (const point3d &coord, OcTreeKey &key) const
 
bool coordToKeyChecked (const point3d &coord, unsigned depth, OcTreeKey &key) const
 
bool coordToKeyChecked (double x, double y, double z, OcTreeKey &key) const
 
bool coordToKeyChecked (double x, double y, double z, unsigned depth, OcTreeKey &key) const
 
bool coordToKeyChecked (double coordinate, key_type &key) const
 
bool coordToKeyChecked (double coordinate, unsigned depth, key_type &key) const
 
ColorOcTreeNodecreateNodeChild (ColorOcTreeNode *node, unsigned int childIdx)
 Creates (allocates) the i-th child of the node. More...
 
bool deleteNode (double x, double y, double z, unsigned int depth=0)
 
bool deleteNode (const point3d &value, unsigned int depth=0)
 
bool deleteNode (const OcTreeKey &key, unsigned int depth=0)
 
void deleteNodeChild (ColorOcTreeNode *node, unsigned int childIdx)
 Deletes the i-th child of the node. More...
 
const iterator end () const
 
const leaf_iterator end_leafs () const
 
const leaf_bbx_iterator end_leafs_bbx () const
 
const tree_iterator end_tree () const
 
virtual void expand ()
 
virtual void expandNode (ColorOcTreeNode *node)
 
virtual void getMetricMax (double &x, double &y, double &z)
 maximum value of the bounding box of all known space in x, y, z More...
 
void getMetricMax (double &x, double &y, double &z) const
 maximum value of the bounding box of all known space in x, y, z More...
 
virtual void getMetricMin (double &x, double &y, double &z)
 minimum value of the bounding box of all known space in x, y, z More...
 
void getMetricMin (double &x, double &y, double &z) const
 minimum value of the bounding box of all known space in x, y, z More...
 
virtual void getMetricSize (double &x, double &y, double &z)
 Size of OcTree (all known space) in meters for x, y and z dimension. More...
 
virtual void getMetricSize (double &x, double &y, double &z) const
 Size of OcTree (all known space) in meters for x, y and z dimension. More...
 
ColorOcTreeNodegetNodeChild (ColorOcTreeNode *node, unsigned int childIdx) const
 
const ColorOcTreeNodegetNodeChild (const ColorOcTreeNode *node, unsigned int childIdx) const
 
double getNodeSize (unsigned depth) const
 
size_t getNumLeafNodes () const
 Traverses the tree to calculate the total number of leaf nodes. More...
 
double getResolution () const
 
ColorOcTreeNodegetRoot () const
 
unsigned int getTreeDepth () const
 
std::string getTreeType () const
 
void getUnknownLeafCenters (point3d_list &node_centers, point3d pmin, point3d pmax, unsigned int depth=0) const
 return centers of leafs that do NOT exist (but could) in a given bounding box More...
 
double keyToCoord (key_type key, unsigned depth) const
 
double keyToCoord (key_type key) const
 
point3d keyToCoord (const OcTreeKey &key) const
 
point3d keyToCoord (const OcTreeKey &key, unsigned depth) const
 
unsigned long long memoryFullGrid () const
 
virtual size_t memoryUsage () const
 
virtual size_t memoryUsageNode () const
 
bool nodeChildExists (const ColorOcTreeNode *node, unsigned int childIdx) const
 
bool nodeHasChildren (const ColorOcTreeNode *node) const
 
 OcTreeBaseImpl (double resolution)
 
 OcTreeBaseImpl (const OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree > &rhs)
 Deep copy constructor. More...
 
bool operator== (const OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree > &rhs) const
 
virtual void prune ()
 
std::istream & readData (std::istream &s)
 
ColorOcTreeNodesearch (double x, double y, double z, unsigned int depth=0) const
 
ColorOcTreeNodesearch (const point3d &value, unsigned int depth=0) const
 
ColorOcTreeNodesearch (const OcTreeKey &key, unsigned int depth=0) const
 
void setResolution (double r)
 
virtual size_t size () const
 
void swapContent (OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree > &rhs)
 
double volume ()
 
std::ostream & writeData (std::ostream &s) const
 
virtual ~OcTreeBaseImpl ()
 
- Public Member Functions inherited from octomap::AbstractOccupancyOcTree
 AbstractOccupancyOcTree ()
 
double getClampingThresMax () const
 
float getClampingThresMaxLog () const
 
double getClampingThresMin () const
 
float getClampingThresMinLog () const
 
double getOccupancyThres () const
 
float getOccupancyThresLog () const
 
double getProbHit () const
 
float getProbHitLog () const
 
double getProbMiss () const
 
float getProbMissLog () const
 
bool isNodeAtThreshold (const OcTreeNode *occupancyNode) const
 queries whether a node is at the clamping threshold according to the tree's parameter More...
 
bool isNodeAtThreshold (const OcTreeNode &occupancyNode) const
 queries whether a node is at the clamping threshold according to the tree's parameter More...
 
bool isNodeOccupied (const OcTreeNode *occupancyNode) const
 queries whether a node is occupied according to the tree's parameter for "occupancy" More...
 
bool isNodeOccupied (const OcTreeNode &occupancyNode) const
 queries whether a node is occupied according to the tree's parameter for "occupancy" More...
 
bool readBinary (std::istream &s)
 
bool readBinary (const std::string &filename)
 
void setClampingThresMax (double thresProb)
 sets the maximum threshold for occupancy clamping (sensor model) More...
 
void setClampingThresMin (double thresProb)
 sets the minimum threshold for occupancy clamping (sensor model) More...
 
void setOccupancyThres (double prob)
 sets the threshold for occupancy (sensor model) More...
 
void setProbHit (double prob)
 sets the probability for a "hit" (will be converted to logodds) - sensor model More...
 
void setProbMiss (double prob)
 sets the probability for a "miss" (will be converted to logodds) - sensor model More...
 
bool writeBinary (const std::string &filename)
 
bool writeBinary (std::ostream &s)
 
bool writeBinaryConst (const std::string &filename) const
 
bool writeBinaryConst (std::ostream &s) const
 
virtual ~AbstractOccupancyOcTree ()
 
- Public Member Functions inherited from octomap::AbstractOcTree
 AbstractOcTree ()
 
bool write (const std::string &filename) const
 Write file header and complete tree to file (serialization) More...
 
bool write (std::ostream &s) const
 Write file header and complete tree to stream (serialization) More...
 
virtual ~AbstractOcTree ()
 

Protected Member Functions

void updateInnerOccupancyRecurs (ColorOcTreeNode *node, unsigned int depth)
 
- Protected Member Functions inherited from octomap::OccupancyOcTreeBase< ColorOcTreeNode >
bool integrateMissOnRay (const point3d &origin, const point3d &end, bool lazy_eval=false)
 
 OccupancyOcTreeBase (double resolution, unsigned int tree_depth, unsigned int tree_max_val)
 
ColorOcTreeNodesetNodeValueRecurs (ColorOcTreeNode *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_value, bool lazy_eval=false)
 
void toMaxLikelihoodRecurs (ColorOcTreeNode *node, unsigned int depth, unsigned int max_depth)
 
void updateInnerOccupancyRecurs (ColorOcTreeNode *node, unsigned int depth)
 
ColorOcTreeNodeupdateNodeRecurs (ColorOcTreeNode *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_update, bool lazy_eval=false)
 
- Protected Member Functions inherited from octomap::OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree >
void allocNodeChildren (ColorOcTreeNode *node)
 
void calcMinMax ()
 recalculates min and max in x, y, z. Does nothing when tree size didn't change. More...
 
void calcNumNodesRecurs (ColorOcTreeNode *node, size_t &num_nodes) const
 
void deleteNodeRecurs (ColorOcTreeNode *node)
 recursive delete of node and all children (deallocates memory) More...
 
bool deleteNodeRecurs (ColorOcTreeNode *node, unsigned int depth, unsigned int max_depth, const OcTreeKey &key)
 recursive call of deleteNode() More...
 
void expandRecurs (ColorOcTreeNode *node, unsigned int depth, unsigned int max_depth)
 recursive call of expand() More...
 
size_t getNumLeafNodesRecurs (const ColorOcTreeNode *parent) const
 
void init ()
 initialize non-trivial members, helper for constructors More...
 
 OcTreeBaseImpl (double resolution, unsigned int tree_depth, unsigned int tree_max_val)
 
void pruneRecurs (ColorOcTreeNode *node, unsigned int depth, unsigned int max_depth, unsigned int &num_pruned)
 recursive call of prune() More...
 
std::istream & readNodesRecurs (ColorOcTreeNode *, std::istream &s)
 recursive call of readData() More...
 
std::ostream & writeNodesRecurs (const ColorOcTreeNode *, std::ostream &s) const
 recursive call of writeData() More...
 
- Protected Member Functions inherited from octomap::AbstractOccupancyOcTree
bool readBinaryLegacyHeader (std::istream &s, unsigned int &size, double &res)
 Try to read the old binary format for conversion, will be removed in the future. More...
 

Static Protected Attributes

static StaticMemberInitializer colorOcTreeMemberInit
 static member to ensure static initialization (only once) More...
 
- Static Protected Attributes inherited from octomap::AbstractOccupancyOcTree
static const std::string binaryFileHeader = "# Octomap OcTree binary file"
 
- Static Protected Attributes inherited from octomap::AbstractOcTree
static const std::string fileHeader = "# Octomap OcTree file"
 

Additional Inherited Members

- Public Types inherited from octomap::OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree >
typedef leaf_iterator iterator
 
typedef ColorOcTreeNode NodeType
 Make the templated NODE type available from the outside. More...
 
- Static Public Member Functions inherited from octomap::AbstractOcTree
static AbstractOcTreecreateTree (const std::string id, double res)
 
static AbstractOcTreeread (const std::string &filename)
 
static AbstractOcTreeread (std::istream &s)
 
- Static Protected Member Functions inherited from octomap::AbstractOcTree
static bool readHeader (std::istream &s, std::string &id, unsigned &size, double &res)
 
static void registerTreeType (AbstractOcTree *tree)
 
- Protected Attributes inherited from octomap::OccupancyOcTreeBase< ColorOcTreeNode >
point3d bbx_max
 
OcTreeKey bbx_max_key
 
point3d bbx_min
 
OcTreeKey bbx_min_key
 
KeyBoolMap changed_keys
 Set of leaf keys (lowest level) which changed since last resetChangeDetection. More...
 
bool use_bbx_limit
 use bounding box for queries (needs to be set)? More...
 
bool use_change_detection
 
- Protected Attributes inherited from octomap::OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree >
std::vector< KeyRaykeyrays
 data structure for ray casting, array for multithreading More...
 
const leaf_bbx_iterator leaf_iterator_bbx_end
 
const leaf_iterator leaf_iterator_end
 
double max_value [3]
 max in x, y, z More...
 
double min_value [3]
 
double resolution
 in meters More...
 
double resolution_factor
 = 1. / resolution More...
 
ColorOcTreeNoderoot
 Pointer to the root NODE, NULL for empty tree. More...
 
bool size_changed
 flag to denote whether the octree extent changed (for lazy min/max eval) More...
 
std::vector< double > sizeLookupTable
 contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0) More...
 
point3d tree_center
 
const unsigned int tree_depth
 Maximum tree depth is fixed to 16 currently. More...
 
const tree_iterator tree_iterator_end
 
const unsigned int tree_max_val
 
size_t tree_size
 
- Protected Attributes inherited from octomap::AbstractOccupancyOcTree
float clamping_thres_max
 
float clamping_thres_min
 
float occ_prob_thres_log
 
float prob_hit_log
 
float prob_miss_log
 

Detailed Description

Definition at line 108 of file ColorOcTree.h.

Constructor & Destructor Documentation

octomap::ColorOcTree::ColorOcTree ( double  resolution)

Default constructor, sets resolution of leafs.

Definition at line 91 of file ColorOcTree.cpp.

Member Function Documentation

ColorOcTreeNode * octomap::ColorOcTree::averageNodeColor ( const OcTreeKey key,
uint8_t  r,
uint8_t  g,
uint8_t  b 
)

Definition at line 146 of file ColorOcTree.cpp.

ColorOcTreeNode* octomap::ColorOcTree::averageNodeColor ( float  x,
float  y,
float  z,
uint8_t  r,
uint8_t  g,
uint8_t  b 
)
inline

Definition at line 146 of file ColorOcTree.h.

ColorOcTree* octomap::ColorOcTree::create ( ) const
inlinevirtual

virtual constructor: creates a new object of same type (Covariant return type requires an up-to-date compiler)

Implements octomap::AbstractOcTree.

Definition at line 116 of file ColorOcTree.h.

std::string octomap::ColorOcTree::getTreeType ( ) const
inlinevirtual

returns actual class name as string for identification

Implements octomap::AbstractOcTree.

Definition at line 118 of file ColorOcTree.h.

ColorOcTreeNode * octomap::ColorOcTree::integrateNodeColor ( const OcTreeKey key,
uint8_t  r,
uint8_t  g,
uint8_t  b 
)

Definition at line 163 of file ColorOcTree.cpp.

ColorOcTreeNode* octomap::ColorOcTree::integrateNodeColor ( float  x,
float  y,
float  z,
uint8_t  r,
uint8_t  g,
uint8_t  b 
)
inline

Definition at line 158 of file ColorOcTree.h.

bool octomap::ColorOcTree::isNodeCollapsible ( const ColorOcTreeNode node) const
virtual

A node is collapsible if all children exist, don't have children of their own and have the same occupancy value

Reimplemented from octomap::OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree >.

Definition at line 127 of file ColorOcTree.cpp.

bool octomap::ColorOcTree::pruneNode ( ColorOcTreeNode node)
virtual

Prunes a node when it is collapsible. This overloaded version only considers the node occupancy for pruning, different colors of child nodes are ignored.

Returns
true if pruning was successful

Reimplemented from octomap::OcTreeBaseImpl< ColorOcTreeNode, AbstractOccupancyOcTree >.

Definition at line 107 of file ColorOcTree.cpp.

ColorOcTreeNode * octomap::ColorOcTree::setNodeColor ( const OcTreeKey key,
uint8_t  r,
uint8_t  g,
uint8_t  b 
)

Definition at line 96 of file ColorOcTree.cpp.

ColorOcTreeNode* octomap::ColorOcTree::setNodeColor ( float  x,
float  y,
float  z,
uint8_t  r,
uint8_t  g,
uint8_t  b 
)
inline

Definition at line 134 of file ColorOcTree.h.

void octomap::ColorOcTree::updateInnerOccupancy ( )

Definition at line 188 of file ColorOcTree.cpp.

void octomap::ColorOcTree::updateInnerOccupancyRecurs ( ColorOcTreeNode node,
unsigned int  depth 
)
protected

Definition at line 192 of file ColorOcTree.cpp.

void octomap::ColorOcTree::writeColorHistogram ( std::string  filename)

Definition at line 208 of file ColorOcTree.cpp.

Member Data Documentation

ColorOcTree::StaticMemberInitializer octomap::ColorOcTree::colorOcTreeMemberInit
staticprotected

static member to ensure static initialization (only once)

Definition at line 198 of file ColorOcTree.h.


The documentation for this class was generated from the following files:


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:27