src
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
45
OcTreeNode::OcTreeNode
()
46
:
OcTreeDataNode
<float>(0.0)
47
{
48
}
49
50
OcTreeNode::~OcTreeNode
(){
51
}
52
53
54
// ============================================================
55
// = occupancy probability ==================================
56
// ============================================================
57
58
double
OcTreeNode::getMeanChildLogOdds
()
const
{
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
76
float
OcTreeNode::getMaxChildLogOdds
()
const
{
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 Tue Dec 12 2023 03:39:40