octomap 1.9.8
ColorOcTree.h
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#ifndef OCTOMAP_COLOR_OCTREE_H
35#define OCTOMAP_COLOR_OCTREE_H
36
37
38#include <iostream>
39#include <octomap/OcTreeNode.h>
41
42namespace octomap {
43
44 // forward declaraton for "friend"
45 class ColorOcTree;
46
47 // node definition
48 class ColorOcTreeNode : public OcTreeNode {
49 public:
50 friend class ColorOcTree; // needs access to node children (inherited)
51
52 class Color {
53 public:
54 Color() : r(255), g(255), b(255) {}
55 Color(uint8_t _r, uint8_t _g, uint8_t _b)
56 : r(_r), g(_g), b(_b) {}
57 inline bool operator== (const Color &other) const {
58 return (r==other.r && g==other.g && b==other.b);
59 }
60 inline bool operator!= (const Color &other) const {
61 return (r!=other.r || g!=other.g || b!=other.b);
62 }
63 uint8_t r, g, b;
64 };
65
66 public:
68
70
71 bool operator==(const ColorOcTreeNode& rhs) const{
72 return (rhs.value == value && rhs.color == color);
73 }
74
75 void copyData(const ColorOcTreeNode& from){
77 this->color = from.getColor();
78 }
79
80 inline Color getColor() const { return color; }
81 inline void setColor(Color c) {this->color = c; }
82 inline void setColor(uint8_t r, uint8_t g, uint8_t b) {
83 this->color = Color(r,g,b);
84 }
85
86 Color& getColor() { return color; }
87
88 // has any color been integrated? (pure white is very unlikely...)
89 inline bool isColorSet() const {
90 return ((color.r != 255) || (color.g != 255) || (color.b != 255));
91 }
92
94
95
97
98 // file I/O
99 std::istream& readData(std::istream &s);
100 std::ostream& writeData(std::ostream &s) const;
101
102 protected:
104 };
105
106
107 // tree definition
108 class ColorOcTree : public OccupancyOcTreeBase <ColorOcTreeNode> {
109
110 public:
112 ColorOcTree(double resolution);
113
116 ColorOcTree* create() const {return new ColorOcTree(resolution); }
117
118 std::string getTreeType() const {return "ColorOcTree";}
119
126 virtual bool pruneNode(ColorOcTreeNode* node);
127
128 virtual bool isNodeCollapsible(const ColorOcTreeNode* node) const;
129
130 // set node color at given key or coordinate. Replaces previous color.
131 ColorOcTreeNode* setNodeColor(const OcTreeKey& key, uint8_t r,
132 uint8_t g, uint8_t b);
133
134 ColorOcTreeNode* setNodeColor(float x, float y,
135 float z, uint8_t r,
136 uint8_t g, uint8_t b) {
137 OcTreeKey key;
138 if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
139 return setNodeColor(key,r,g,b);
140 }
141
142 // integrate color measurement at given key or coordinate. Average with previous color
143 ColorOcTreeNode* averageNodeColor(const OcTreeKey& key, uint8_t r,
144 uint8_t g, uint8_t b);
145
147 float z, uint8_t r,
148 uint8_t g, uint8_t b) {
149 OcTreeKey key;
150 if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
151 return averageNodeColor(key,r,g,b);
152 }
153
154 // integrate color measurement at given key or coordinate. Average with previous color
155 ColorOcTreeNode* integrateNodeColor(const OcTreeKey& key, uint8_t r,
156 uint8_t g, uint8_t b);
157
159 float z, uint8_t r,
160 uint8_t g, uint8_t b) {
161 OcTreeKey key;
162 if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
163 return integrateNodeColor(key,r,g,b);
164 }
165
166 // update inner nodes, sets color to average child color
168
169 // uses gnuplot to plot a RGB histogram in EPS format
170 void writeColorHistogram(std::string filename);
171
172 protected:
173 void updateInnerOccupancyRecurs(ColorOcTreeNode* node, unsigned int depth);
174
183 public:
185 ColorOcTree* tree = new ColorOcTree(0.1);
186 tree->clearKeyRays();
188 }
189
195 void ensureLinking() {};
196 };
199
200 };
201
203 std::ostream& operator<<(std::ostream& out, ColorOcTreeNode::Color const& c);
204
205} // end namespace
206
207#endif
static void registerTreeType(AbstractOcTree *tree)
Definition: AbstractOcTree.cpp:205
Definition: ColorOcTree.h:52
Color(uint8_t _r, uint8_t _g, uint8_t _b)
Definition: ColorOcTree.h:55
bool operator==(const Color &other) const
Definition: ColorOcTree.h:57
Color()
Definition: ColorOcTree.h:54
uint8_t g
Definition: ColorOcTree.h:63
uint8_t b
Definition: ColorOcTree.h:63
uint8_t r
Definition: ColorOcTree.h:63
bool operator!=(const Color &other) const
Definition: ColorOcTree.h:60
Definition: ColorOcTree.h:48
Color getColor() const
Definition: ColorOcTree.h:80
bool isColorSet() const
Definition: ColorOcTree.h:89
void setColor(Color c)
Definition: ColorOcTree.h:81
ColorOcTreeNode::Color getAverageChildColor() const
Definition: ColorOcTree.cpp:54
std::istream & readData(std::istream &s)
Definition: ColorOcTree.cpp:47
Color & getColor()
Definition: ColorOcTree.h:86
void updateColorChildren()
Definition: ColorOcTree.cpp:85
ColorOcTreeNode()
Definition: ColorOcTree.h:67
Color color
Definition: ColorOcTree.h:103
ColorOcTreeNode(const ColorOcTreeNode &rhs)
Definition: ColorOcTree.h:69
bool operator==(const ColorOcTreeNode &rhs) const
Definition: ColorOcTree.h:71
void setColor(uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.h:82
std::ostream & writeData(std::ostream &s) const
Definition: ColorOcTree.cpp:40
void copyData(const ColorOcTreeNode &from)
Definition: ColorOcTree.h:75
Static member object which ensures that this OcTree's prototype ends up in the classIDMapping only on...
Definition: ColorOcTree.h:182
StaticMemberInitializer()
Definition: ColorOcTree.h:184
void ensureLinking()
Dummy function to ensure that MSVC does not drop the StaticMemberInitializer, causing this tree faili...
Definition: ColorOcTree.h:195
Definition: ColorOcTree.h:108
std::string getTreeType() const
returns actual class name as string for identification
Definition: ColorOcTree.h:118
void updateInnerOccupancy()
Definition: ColorOcTree.cpp:188
ColorOcTreeNode * integrateNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.h:158
ColorOcTreeNode * averageNodeColor(const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.cpp:146
virtual bool isNodeCollapsible(const ColorOcTreeNode *node) const
Definition: ColorOcTree.cpp:127
ColorOcTree * create() const
virtual constructor: creates a new object of same type (Covariant return type requires an up-to-date ...
Definition: ColorOcTree.h:116
void writeColorHistogram(std::string filename)
Definition: ColorOcTree.cpp:208
ColorOcTreeNode * setNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.h:134
void updateInnerOccupancyRecurs(ColorOcTreeNode *node, unsigned int depth)
Definition: ColorOcTree.cpp:192
virtual bool pruneNode(ColorOcTreeNode *node)
Prunes a node when it is collapsible.
Definition: ColorOcTree.cpp:107
ColorOcTreeNode * integrateNodeColor(const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.cpp:163
ColorOcTree(double resolution)
Default constructor, sets resolution of leafs.
Definition: ColorOcTree.cpp:91
static StaticMemberInitializer colorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: ColorOcTree.h:198
ColorOcTreeNode * setNodeColor(const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.cpp:96
ColorOcTreeNode * averageNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.h:146
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking.
Definition: OcTreeBaseImpl.hxx:340
void clearKeyRays()
Clear KeyRay vector to minimize unneeded memory.
Definition: OcTreeBaseImpl.h:120
double resolution
in meters
Definition: OcTreeBaseImpl.h:547
T value
stored data (payload)
Definition: OcTreeDataNode.h:128
void copyData(const OcTreeDataNode &from)
Copy the payload (data in "value") from rhs into this node Opposed to copy ctor, this does not clone ...
Definition: OcTreeDataNode.hxx:73
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:70
Nodes to be used in OcTree.
Definition: OcTreeNode.h:55
Base implementation for Occupancy Octrees (e.g.
Definition: OccupancyOcTreeBase.h:69
Namespace the OctoMap library and visualization tools.
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
std::ostream & operator<<(std::ostream &out, ColorOcTreeNode::Color const &c)
user friendly output in format (r g b)
Definition: ColorOcTree.cpp:253