Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <cassert>
00035 #include <octomap/CountingOcTree.h>
00036
00037 namespace octomap {
00038
00039
00041
00042 CountingOcTreeNode::CountingOcTreeNode()
00043 : OcTreeDataNode<unsigned int>(0)
00044 {
00045 }
00046
00047 CountingOcTreeNode::~CountingOcTreeNode() {
00048
00049 }
00050
00051 void CountingOcTreeNode::expandNode(){
00052 assert(!hasChildren());
00053
00054
00055 unsigned int childCount = (unsigned int)(value/ 8.0 +0.5);
00056 for (unsigned int k=0; k<8; k++) {
00057 createChild(k);
00058 children[k]->setValue(childCount);
00059 }
00060 }
00061
00062 bool CountingOcTreeNode::createChild(unsigned int i) {
00063 if (children == NULL) {
00064 allocChildren();
00065 }
00066 assert (children[i] == NULL);
00067 children[i] = new CountingOcTreeNode();
00068 return true;
00069 }
00070
00071
00073
00074
00075 CountingOcTreeNode* CountingOcTree::updateNode(const point3d& value) {
00076
00077 OcTreeKey key;
00078 if (!coordToKeyChecked(value, key)) return NULL;
00079 return updateNode(key);
00080 }
00081
00082
00083
00084 CountingOcTreeNode* CountingOcTree::updateNode(const OcTreeKey& k) {
00085
00086 CountingOcTreeNode* curNode (root);
00087 curNode->increaseCount();
00088
00089
00090
00091 for (int i=(tree_depth-1); i>=0; i--) {
00092
00093 unsigned int pos = computeChildIdx(k, i);
00094
00095
00096 if (!curNode->childExists(pos)) {
00097 curNode->createChild(pos);
00098 tree_size++;
00099 }
00100
00101 curNode = static_cast<CountingOcTreeNode*> (curNode->getChild(pos));
00102 curNode->increaseCount();
00103 }
00104
00105 return curNode;
00106 }
00107
00108
00109 void CountingOcTree::getCentersMinHits(point3d_list& node_centers, unsigned int min_hits) const {
00110
00111 OcTreeKey root_key;
00112 root_key[0] = root_key[1] = root_key[2] = this->tree_max_val;
00113 getCentersMinHitsRecurs(node_centers, min_hits, this->tree_depth, this->root, 0, root_key);
00114 }
00115
00116
00117 void CountingOcTree::getCentersMinHitsRecurs( point3d_list& node_centers,
00118 unsigned int& min_hits,
00119 unsigned int max_depth,
00120 CountingOcTreeNode* node, unsigned int depth,
00121 const OcTreeKey& parent_key) const {
00122
00123 if (depth < max_depth && node->hasChildren()) {
00124
00125 unsigned short int center_offset_key = this->tree_max_val >> (depth + 1);
00126 OcTreeKey search_key;
00127
00128 for (unsigned int i=0; i<8; ++i) {
00129 if (node->childExists(i)) {
00130 computeChildKey(i, center_offset_key, parent_key, search_key);
00131 getCentersMinHitsRecurs(node_centers, min_hits, max_depth, node->getChild(i), depth+1, search_key);
00132 }
00133 }
00134 }
00135
00136 else {
00137
00138 if (node->getCount() >= min_hits) {
00139 node_centers.push_back(this->keyToCoord(parent_key, depth));
00140 }
00141 }
00142 }
00143
00144 CountingOcTree::StaticMemberInitializer CountingOcTree::countingOcTreeMemberInit;
00145
00146
00147 }