OcTreeNode.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 <bitset>
35 #include <cassert>
36 #include <math.h>
37 #include <fstream>
38 #include <stdlib.h>
39 #include <inttypes.h>
40 
41 #include <octomap/OcTreeNode.h>
42 
43 namespace octomap {
44 
46  : OcTreeDataNode<float>(0.0)
47  {
48  }
49 
51  }
52 
53 
54  // ============================================================
55  // = occupancy probability ==================================
56  // ============================================================
57 
59  double mean = 0;
60  uint8_t c = 0;
61  if (children !=NULL){
62  for (unsigned int i=0; i<8; i++) {
63  if (children[i] != NULL) {
64  mean += static_cast<OcTreeNode*>(children[i])->getOccupancy(); // TODO check if works generally
65  ++c;
66  }
67  }
68  }
69 
70  if (c > 0)
71  mean /= (double) c;
72 
73  return log(mean/(1-mean));
74  }
75 
77  float max = -std::numeric_limits<float>::max();
78 
79  if (children !=NULL){
80  for (unsigned int i=0; i<8; i++) {
81  if (children[i] != NULL) {
82  float l = static_cast<OcTreeNode*>(children[i])->getLogOdds(); // TODO check if works generally
83  if (l > max)
84  max = l;
85  }
86  }
87  }
88  return max;
89  }
90 
91  void OcTreeNode::addValue(const float& logOdds) {
92  value += logOdds;
93  }
94 
95 } // end namespace
96 
97 
octomap::OcTreeDataNode
Definition: OcTreeDataNode.h:63
octomap::OcTreeDataNode< float >::value
float value
stored data (payload)
Definition: OcTreeDataNode.h:128
octomap::OcTreeNode::getLogOdds
float getLogOdds() const
Definition: OcTreeNode.h:68
octomap::OcTreeNode::getMaxChildLogOdds
float getMaxChildLogOdds() const
Definition: OcTreeNode.cpp:76
octomap::OcTreeNode::~OcTreeNode
~OcTreeNode()
Definition: OcTreeNode.cpp:50
octomap::OcTreeNode
Definition: OcTreeNode.h:55
octomap::OcTreeNode::OcTreeNode
OcTreeNode()
Definition: OcTreeNode.cpp:45
OcTreeNode.h
octomap::OcTreeNode::addValue
void addValue(const float &p)
adds p to the node's logOdds value (with no boundary / threshold checking!)
Definition: OcTreeNode.cpp:91
octomap::OcTreeNode::getMeanChildLogOdds
double getMeanChildLogOdds() const
Definition: OcTreeNode.cpp:58
octomap::OcTreeNode::getOccupancy
double getOccupancy() const
Definition: OcTreeNode.h:65
octomap
octomap::OcTreeDataNode< float >::children
AbstractOcTreeNode ** children
Definition: OcTreeDataNode.h:126


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Mar 21 2024 02:40:30