ColorOcTree.h
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
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 
42 namespace 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 
93  void updateColorChildren();
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 
146  ColorOcTreeNode* averageNodeColor(float x, float y,
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
167  void updateInnerOccupancy();
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)
T value
stored data (payload)
void setColor(uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.h:82
ColorOcTree * create() const
Definition: ColorOcTree.h:116
bool operator!=(const Color &other) const
Definition: ColorOcTree.h:60
bool isColorSet() const
Definition: ColorOcTree.h:89
bool operator==(const Color &other) const
Definition: ColorOcTree.h:57
void setColor(Color c)
Definition: ColorOcTree.h:81
bool operator==(const ColorOcTreeNode &rhs) const
Definition: ColorOcTree.h:71
Color getColor() const
Definition: ColorOcTree.h:80
std::ostream & operator<<(std::ostream &out, ColorOcTreeNode::Color const &c)
user friendly output in format (r g b)
ColorOcTreeNode * averageNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.h:146
ColorOcTreeNode::Color getAverageChildColor() const
Definition: ColorOcTree.cpp:54
std::string getTreeType() const
returns actual class name as string for identification
Definition: ColorOcTree.h:118
void copyData(const OcTreeDataNode &from)
ColorOcTreeNode * setNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.h:134
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
Color(uint8_t _r, uint8_t _g, uint8_t _b)
Definition: ColorOcTree.h:55
friend class ColorOcTree
Definition: ColorOcTree.h:50
std::ostream & writeData(std::ostream &s) const
Definition: ColorOcTree.cpp:40
std::istream & readData(std::istream &s)
Definition: ColorOcTree.cpp:47
static StaticMemberInitializer colorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: ColorOcTree.h:198
ColorOcTreeNode * integrateNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.h:158
ColorOcTreeNode(const ColorOcTreeNode &rhs)
Definition: ColorOcTree.h:69
void copyData(const ColorOcTreeNode &from)
Definition: ColorOcTree.h:75


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Fri Jun 7 2019 21:17:32