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  // node definition
45  class ColorOcTreeNode : public OcTreeNode {
46  public:
47 
48  class Color {
49  public:
50  Color() : r(255), g(255), b(255) {}
51  Color(unsigned char _r, unsigned char _g, unsigned char _b)
52  : r(_r), g(_g), b(_b) {}
53  inline bool operator== (const Color &other) const {
54  return (r==other.r && g==other.g && b==other.b);
55  }
56  inline bool operator!= (const Color &other) const {
57  return (r!=other.r || g!=other.g || b!=other.b);
58  }
59  unsigned char r, g, b;
60  };
61 
62  public:
64 
66 
67  bool operator==(const ColorOcTreeNode& rhs) const{
68  return (rhs.value == value && rhs.color == color);
69  }
70 
71  // children
72  inline ColorOcTreeNode* getChild(unsigned int i) {
73  return static_cast<ColorOcTreeNode*> (OcTreeNode::getChild(i));
74  }
75  inline const ColorOcTreeNode* getChild(unsigned int i) const {
76  return static_cast<const ColorOcTreeNode*> (OcTreeNode::getChild(i));
77  }
78 
79  bool createChild(unsigned int i) {
80  if (children == NULL) allocChildren();
81  children[i] = new ColorOcTreeNode();
82  return true;
83  }
84 
85  bool pruneNode();
86  void expandNode();
87 
88  inline Color getColor() const { return color; }
89  inline void setColor(Color c) {this->color = c; }
90  inline void setColor(unsigned char r, unsigned char g, unsigned char b) {
91  this->color = Color(r,g,b);
92  }
93 
94  Color& getColor() { return color; }
95 
96  // has any color been integrated? (pure white is very unlikely...)
97  inline bool isColorSet() const {
98  return ((color.r != 255) || (color.g != 255) || (color.b != 255));
99  }
100 
101  void updateColorChildren();
102 
103 
105 
106  // file I/O
107  std::istream& readValue (std::istream &s);
108  std::ostream& writeValue(std::ostream &s) const;
109 
110  protected:
112  };
113 
114 
115  // tree definition
116  class ColorOcTree : public OccupancyOcTreeBase <ColorOcTreeNode> {
117 
118  public:
120  ColorOcTree(double resolution);
121 
124  ColorOcTree* create() const {return new ColorOcTree(resolution); }
125 
126  std::string getTreeType() const {return "ColorOcTree";}
127 
128  // set node color at given key or coordinate. Replaces previous color.
129  ColorOcTreeNode* setNodeColor(const OcTreeKey& key, const unsigned char& r,
130  const unsigned char& g, const unsigned char& b);
131 
132  ColorOcTreeNode* setNodeColor(const float& x, const float& y,
133  const float& z, const unsigned char& r,
134  const unsigned char& g, const unsigned char& b) {
135  OcTreeKey key;
136  if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
137  return setNodeColor(key,r,g,b);
138  }
139 
140  // integrate color measurement at given key or coordinate. Average with previous color
141  ColorOcTreeNode* averageNodeColor(const OcTreeKey& key, const unsigned char& r,
142  const unsigned char& g, const unsigned char& b);
143 
144  ColorOcTreeNode* averageNodeColor(const float& x, const float& y,
145  const float& z, const unsigned char& r,
146  const unsigned char& g, const unsigned char& b) {
147  OcTreeKey key;
148  if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
149  return averageNodeColor(key,r,g,b);
150  }
151 
152  // integrate color measurement at given key or coordinate. Average with previous color
153  ColorOcTreeNode* integrateNodeColor(const OcTreeKey& key, const unsigned char& r,
154  const unsigned char& g, const unsigned char& b);
155 
156  ColorOcTreeNode* integrateNodeColor(const float& x, const float& y,
157  const float& z, const unsigned char& r,
158  const unsigned char& g, const unsigned char& b) {
159  OcTreeKey key;
160  if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
161  return integrateNodeColor(key,r,g,b);
162  }
163 
164  // update inner nodes, sets color to average child color
165  void updateInnerOccupancy();
166 
167  // uses gnuplot to plot a RGB histogram in EPS format
168  void writeColorHistogram(std::string filename);
169 
170  protected:
171  void updateInnerOccupancyRecurs(ColorOcTreeNode* node, unsigned int depth);
172 
181  public:
183  ColorOcTree* tree = new ColorOcTree(0.1);
185  }
186 
192  void ensureLinking() {};
193  };
196 
197  };
198 
200  std::ostream& operator<<(std::ostream& out, ColorOcTreeNode::Color const& c);
201 
202 } // end namespace
203 
204 #endif
static void registerTreeType(AbstractOcTree *tree)
const ColorOcTreeNode * getChild(unsigned int i) const
Definition: ColorOcTree.h:75
ColorOcTreeNode::Color getAverageChildColor() const
Definition: ColorOcTree.cpp:78
T value
stored data (payload)
bool isColorSet() const
Definition: ColorOcTree.h:97
bool operator==(const Color &other) const
Definition: ColorOcTree.h:53
OcTreeNode * getChild(unsigned int i)
Definition: OcTreeNode.h:64
ColorOcTree * create() const
Definition: ColorOcTree.h:124
bool operator!=(const Color &other) const
Definition: ColorOcTree.h:56
std::string getTreeType() const
returns actual class name as string for identification
Definition: ColorOcTree.h:126
ColorOcTreeNode * getChild(unsigned int i)
Definition: ColorOcTree.h:72
void setColor(Color c)
Definition: ColorOcTree.h:89
std::ostream & operator<<(std::ostream &out, ColorOcTreeNode::Color const &c)
user friendly output in format (r g b)
OcTreeDataNode< float > ** children
pointer to array of children, may be NULL
Color(unsigned char _r, unsigned char _g, unsigned char _b)
Definition: ColorOcTree.h:51
std::istream & readValue(std::istream &s)
Definition: ColorOcTree.cpp:60
ColorOcTreeNode * integrateNodeColor(const float &x, const float &y, const float &z, const unsigned char &r, const unsigned char &g, const unsigned char &b)
Definition: ColorOcTree.h:156
ColorOcTreeNode * setNodeColor(const float &x, const float &y, const float &z, const unsigned char &r, const unsigned char &g, const unsigned char &b)
Definition: ColorOcTree.h:132
bool operator==(const ColorOcTreeNode &rhs) const
Definition: ColorOcTree.h:67
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:48
bool createChild(unsigned int i)
Definition: ColorOcTree.h:79
std::ostream & writeValue(std::ostream &s) const
Definition: ColorOcTree.cpp:40
ColorOcTreeNode * averageNodeColor(const float &x, const float &y, const float &z, const unsigned char &r, const unsigned char &g, const unsigned char &b)
Definition: ColorOcTree.h:144
static StaticMemberInitializer colorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: ColorOcTree.h:195
ColorOcTreeNode(const ColorOcTreeNode &rhs)
Definition: ColorOcTree.h:65
void setColor(unsigned char r, unsigned char g, unsigned char b)
Definition: ColorOcTree.h:90
Color getColor() const
Definition: ColorOcTree.h:88


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Fri Mar 25 2016 03:52:54