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 #ifndef OCTOMAP_COUNTING_OCTREE_HH 00035 #define OCTOMAP_COUNTING_OCTREE_HH 00036 00037 00038 #include <stdio.h> 00039 #include "OcTreeBase.h" 00040 #include "OcTreeDataNode.h" 00041 00042 namespace octomap { 00043 00053 class CountingOcTreeNode : public OcTreeDataNode<unsigned int> { 00054 00055 public: 00056 00057 CountingOcTreeNode(); 00058 ~CountingOcTreeNode(); 00059 bool createChild(unsigned int i); 00060 00061 inline CountingOcTreeNode* getChild(unsigned int i) { 00062 return static_cast<CountingOcTreeNode*> (OcTreeDataNode<unsigned int>::getChild(i)); 00063 } 00064 00065 inline const CountingOcTreeNode* getChild(unsigned int i) const { 00066 return static_cast<const CountingOcTreeNode*> (OcTreeDataNode<unsigned int>::getChild(i)); 00067 } 00068 00069 inline unsigned int getCount() const { return getValue(); } 00070 inline void increaseCount() { value++; } 00071 inline void setCount(unsigned c) {this->setValue(c); } 00072 00073 // overloaded: 00074 void expandNode(); 00075 }; 00076 00077 00078 00088 class CountingOcTree : public OcTreeBase <CountingOcTreeNode> { 00089 00090 public: 00092 CountingOcTree(double resolution) : OcTreeBase<CountingOcTreeNode>(resolution) {}; 00093 virtual CountingOcTreeNode* updateNode(const point3d& value); 00094 CountingOcTreeNode* updateNode(const OcTreeKey& k); 00095 void getCentersMinHits(point3d_list& node_centers, unsigned int min_hits) const; 00096 00097 protected: 00098 00099 void getCentersMinHitsRecurs( point3d_list& node_centers, 00100 unsigned int& min_hits, 00101 unsigned int max_depth, 00102 CountingOcTreeNode* node, unsigned int depth, 00103 const OcTreeKey& parent_key) const; 00104 00109 class StaticMemberInitializer{ 00110 public: 00111 StaticMemberInitializer() { 00112 CountingOcTree* tree = new CountingOcTree(0.1); 00113 AbstractOcTree::registerTreeType(tree); 00114 } 00115 }; 00117 static StaticMemberInitializer countingOcTreeMemberInit; 00118 }; 00119 00120 00121 } 00122 00123 00124 #endif