CountingOcTree.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
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     // divide "counts" evenly to children
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   // Note: do not inline this method, will decrease speed (KMW)
00084   CountingOcTreeNode* CountingOcTree::updateNode(const OcTreeKey& k) {
00085 
00086     CountingOcTreeNode* curNode (root);
00087     curNode->increaseCount();
00088 
00089 
00090     // follow or construct nodes down to last level...
00091     for (int i=(tree_depth-1); i>=0; i--) {
00092 
00093       unsigned int pos = computeChildIdx(k, i);
00094 
00095       // requested node does not exist
00096       if (!curNode->childExists(pos)) {
00097         curNode->createChild(pos);
00098         tree_size++;
00099       }
00100       // descent tree
00101       curNode = static_cast<CountingOcTreeNode*> (curNode->getChild(pos));
00102       curNode->increaseCount(); // modify traversed nodes
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 { // max level reached
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 } // namespace


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14