CountingOcTree.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * https://octomap.github.io/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #include <cassert>
35 #include <octomap/CountingOcTree.h>
36 
37 namespace octomap {
38 
39 
41 
43  : OcTreeDataNode<unsigned int>(0)
44  {
45  }
46 
48 
49  }
50 
52  CountingOcTree::CountingOcTree(double in_resolution)
53  : OcTreeBase<CountingOcTreeNode>(in_resolution) {
55  }
56 
58 
59  OcTreeKey key;
60  if (!coordToKeyChecked(value, key)) return NULL;
61  return updateNode(key);
62  }
63 
64 
65  // Note: do not inline this method, will decrease speed (KMW)
67 
68  if (root == NULL) {
69  root = new CountingOcTreeNode();
70  tree_size++;
71  }
72  CountingOcTreeNode* curNode (root);
73  curNode->increaseCount();
74 
75 
76  // follow or construct nodes down to last level...
77  for (int i=(tree_depth-1); i>=0; i--) {
78 
79  unsigned int pos = computeChildIdx(k, i);
80 
81  // requested node does not exist
82  if (!nodeChildExists(curNode, pos)) {
83  createNodeChild(curNode, pos);
84  }
85  // descent tree
86  curNode = getNodeChild(curNode, pos);
87  curNode->increaseCount(); // modify traversed nodes
88  }
89 
90  return curNode;
91  }
92 
93 
94  void CountingOcTree::getCentersMinHits(point3d_list& node_centers, unsigned int min_hits) const {
95 
96  OcTreeKey root_key;
97  root_key[0] = root_key[1] = root_key[2] = this->tree_max_val;
98  getCentersMinHitsRecurs(node_centers, min_hits, this->tree_depth, this->root, 0, root_key);
99  }
100 
101 
103  unsigned int& min_hits,
104  unsigned int max_depth,
105  CountingOcTreeNode* node, unsigned int depth,
106  const OcTreeKey& parent_key) const {
107 
108  if (depth < max_depth && nodeHasChildren(node)) {
109 
110  key_type center_offset_key = this->tree_max_val >> (depth + 1);
111  OcTreeKey search_key;
112 
113  for (unsigned int i=0; i<8; ++i) {
114  if (nodeChildExists(node,i)) {
115  computeChildKey(i, center_offset_key, parent_key, search_key);
116  getCentersMinHitsRecurs(node_centers, min_hits, max_depth, getNodeChild(node,i), depth+1, search_key);
117  }
118  }
119  }
120 
121  else { // max level reached
122 
123  if (node->getCount() >= min_hits) {
124  node_centers.push_back(this->keyToCoord(parent_key, depth));
125  }
126  }
127  }
128 
130 
131 
132 } // namespace
octomap::CountingOcTreeNode::getCount
unsigned int getCount() const
Definition: CountingOcTree.h:60
octomap::CountingOcTree::StaticMemberInitializer
Definition: CountingOcTree.h:100
octomap::key_type
uint16_t key_type
Definition: OcTreeKey.h:63
CountingOcTree.h
octomap::OcTreeDataNode
Definition: OcTreeDataNode.h:63
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::tree_max_val
const unsigned int tree_max_val
Definition: OcTreeBaseImpl.h:546
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::tree_depth
const unsigned int tree_depth
Maximum tree depth is fixed to 16 currently.
Definition: OcTreeBaseImpl.h:545
octomap::CountingOcTree::getCentersMinHitsRecurs
void getCentersMinHitsRecurs(point3d_list &node_centers, unsigned int &min_hits, unsigned int max_depth, CountingOcTreeNode *node, unsigned int depth, const OcTreeKey &parent_key) const
Definition: CountingOcTree.cpp:102
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::root
CountingOcTreeNode * root
Pointer to the root NODE, NULL for empty tree.
Definition: OcTreeBaseImpl.h:542
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomap::CountingOcTree::CountingOcTree
CountingOcTree(double resolution)
Default constructor, sets resolution of leafs.
Definition: CountingOcTree.cpp:52
octomap::CountingOcTreeNode::CountingOcTreeNode
CountingOcTreeNode()
implementation of CountingOcTreeNode -------------------------------—
Definition: CountingOcTree.cpp:42
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::nodeChildExists
bool nodeChildExists(const CountingOcTreeNode *node, unsigned int childIdx) const
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::nodeHasChildren
bool nodeHasChildren(const CountingOcTreeNode *node) const
octomap::CountingOcTree::countingOcTreeMemberInit
static StaticMemberInitializer countingOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: CountingOcTree.h:116
octomap::CountingOcTreeNode::~CountingOcTreeNode
~CountingOcTreeNode()
Definition: CountingOcTree.cpp:47
octomap::OcTreeKey
Definition: OcTreeKey.h:70
octomap::computeChildIdx
uint8_t computeChildIdx(const OcTreeKey &key, int depth)
generate child index (between 0 and 7) from key at given tree depth
Definition: OcTreeKey.h:207
octomap::CountingOcTree::StaticMemberInitializer::ensureLinking
void ensureLinking()
Definition: CountingOcTree.h:113
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::coordToKeyChecked
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::createNodeChild
CountingOcTreeNode * createNodeChild(CountingOcTreeNode *node, unsigned int childIdx)
Creates (allocates) the i-th child of the node.
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::keyToCoord
double keyToCoord(key_type key, unsigned depth) const
octomap::point3d_list
std::list< octomath::Vector3 > point3d_list
Definition: octomap_types.h:54
octomap::OcTreeBase
Definition: OcTreeBase.h:44
octomap::CountingOcTreeNode
Definition: CountingOcTree.h:53
octomap::CountingOcTree::getCentersMinHits
void getCentersMinHits(point3d_list &node_centers, unsigned int min_hits) const
Definition: CountingOcTree.cpp:94
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::tree_size
size_t tree_size
Definition: OcTreeBaseImpl.h:550
octomap::CountingOcTreeNode::increaseCount
void increaseCount()
Definition: CountingOcTree.h:61
octomap::OcTreeBaseImpl< CountingOcTreeNode, AbstractOcTree >::getNodeChild
CountingOcTreeNode * getNodeChild(CountingOcTreeNode *node, unsigned int childIdx) const
octomap::CountingOcTree::updateNode
virtual CountingOcTreeNode * updateNode(const point3d &value)
Definition: CountingOcTree.cpp:57
octomap
octomap::computeChildKey
void computeChildKey(unsigned int pos, key_type center_offset_key, const OcTreeKey &parent_key, OcTreeKey &child_key)
Definition: OcTreeKey.h:193


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Jun 6 2022 02:39:55