Main MRPT website > C++ reference for MRPT 1.3.2
ColorOcTree.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef OCTOMAP_COLOR_OCTREE_H
10 #define OCTOMAP_COLOR_OCTREE_H
11 
12 // $Id: ColorOcTree.h 402 2012-08-06 13:39:42Z ahornung $
13 
14 /**
15  * OctoMap:
16  * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
17  * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009-2011
18  * @see http://octomap.sourceforge.net/
19  * License: New BSD License
20  */
21 
22 /*
23  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
24  * All rights reserved.
25  *
26  * Redistribution and use in source and binary forms, with or without
27  * modification, are permitted provided that the following conditions are met:
28  *
29  * * Redistributions of source code must retain the above copyright
30  * notice, this list of conditions and the following disclaimer.
31  * * Redistributions in binary form must reproduce the above copyright
32  * notice, this list of conditions and the following disclaimer in the
33  * documentation and/or other materials provided with the distribution.
34  * * Neither the name of the University of Freiburg nor the names of its
35  * contributors may be used to endorse or promote products derived from
36  * this software without specific prior written permission.
37  *
38  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
39  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
40  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
41  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
42  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
43  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
44  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
45  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
46  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
47  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
48  * POSSIBILITY OF SUCH DAMAGE.
49  */
50 
51 #include <iostream>
54 #include <mrpt/maps/link_pragmas.h> // For DLL export within mrpt-maps via the MAPS_IMPEXP macro
55 
56 namespace octomap {
57 
58  // node definition
59  class /*MAPS_IMPEXP*/ ColorOcTreeNode : public OcTreeNode {
60  public:
61 
62  class Color {
63  public:
64  Color() : r(255), g(255), b(255) {}
65  Color(unsigned char _r, unsigned char _g, unsigned char _b)
66  : r(_r), g(_g), b(_b) {}
67  inline bool operator== (const Color &other) const {
68  return (r==other.r && g==other.g && b==other.b);
69  }
70  inline bool operator!= (const Color &other) const {
71  return (r!=other.r || g!=other.g || b!=other.b);
72  }
73  unsigned char r, g, b;
74  };
75 
76  public:
78 
80 
81  bool operator==(const ColorOcTreeNode& rhs) const{
82  return (rhs.value == value && rhs.color == color);
83  }
84 
85  // children
86  inline ColorOcTreeNode* getChild(unsigned int i) {
87  return static_cast<ColorOcTreeNode*> (OcTreeNode::getChild(i));
88  }
89  inline const ColorOcTreeNode* getChild(unsigned int i) const {
90  return static_cast<const ColorOcTreeNode*> (OcTreeNode::getChild(i));
91  }
92 
93  bool createChild(unsigned int i) {
94  if (children == NULL) allocChildren();
95  children[i] = new ColorOcTreeNode();
96  return true;
97  }
98 
99  bool pruneNode();
100  void expandNode();
101 
102  inline Color getColor() const { return color; }
103  inline void setColor(Color c) {this->color = c; }
104  inline void setColor(unsigned char r, unsigned char g, unsigned char b) {
105  this->color = Color(r,g,b);
106  }
107 
108  Color& getColor() { return color; }
109 
110  // has any color been integrated? (pure white is very unlikely...)
111  inline bool isColorSet() const {
112  return ((color.r != 255) || (color.g != 255) || (color.b != 255));
113  }
114 
115  void updateColorChildren();
116 
117 
119 
120  // file I/O
121  std::istream& readValue (std::istream &s);
122  std::ostream& writeValue(std::ostream &s) const;
123 
124  protected:
126  };
127 
128 
129  // tree definition
130  class /*MAPS_IMPEXP*/ ColorOcTree : public OccupancyOcTreeBase <ColorOcTreeNode> {
131 
132  public:
133  /// Default constructor, sets resolution of leafs
135 
136  /// virtual constructor: creates a new object of same type
137  /// (Covariant return type requires an up-to-date compiler)
138  ColorOcTree* create() const {return new ColorOcTree(resolution); }
139 
140  std::string getTreeType() const {return "ColorOcTree";}
141 
142  // set node color at given key or coordinate. Replaces previous color.
143  ColorOcTreeNode* setNodeColor(const OcTreeKey& key, const unsigned char& r,
144  const unsigned char& g, const unsigned char& b);
145 
146  ColorOcTreeNode* setNodeColor(const float& x, const float& y,
147  const float& z, const unsigned char& r,
148  const unsigned char& g, const unsigned char& b) {
149  OcTreeKey key;
150  if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
151  return setNodeColor(key,r,g,b);
152  }
153 
154  // integrate color measurement at given key or coordinate. Average with previous color
155  ColorOcTreeNode* averageNodeColor(const OcTreeKey& key, const unsigned char& r,
156  const unsigned char& g, const unsigned char& b);
157 
158  ColorOcTreeNode* averageNodeColor(const float& x, const float& y,
159  const float& z, const unsigned char& r,
160  const unsigned char& g, const unsigned char& b) {
161  OcTreeKey key;
162  if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
163  return averageNodeColor(key,r,g,b);
164  }
165 
166  // integrate color measurement at given key or coordinate. Average with previous color
167  ColorOcTreeNode* integrateNodeColor(const OcTreeKey& key, const unsigned char& r,
168  const unsigned char& g, const unsigned char& b);
169 
170  ColorOcTreeNode* integrateNodeColor(const float& x, const float& y,
171  const float& z, const unsigned char& r,
172  const unsigned char& g, const unsigned char& b) {
173  OcTreeKey key;
174  if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
175  return integrateNodeColor(key,r,g,b);
176  }
177 
178  // update inner nodes, sets color to average child color
179  void updateInnerOccupancy();
180 
181  // uses gnuplot to plot a RGB histogram in EPS format
182  void writeColorHistogram(std::string filename);
183 
184  protected:
185  void updateInnerOccupancyRecurs(ColorOcTreeNode* node, unsigned int depth);
186 
187  /**
188  * Static member object which ensures that this OcTree's prototype
189  * ends up in the classIDMapping only once
190  */
192  public:
194  ColorOcTree* tree = new ColorOcTree(0.1);
196  }
197  };
198  /// static member to ensure static initialization (only once)
200 
201  };
202 
203  //! user friendly output in format (r g b)
204  std::ostream & operator<<(std::ostream& out, ColorOcTreeNode::Color const& c);
205 
206 } // end namespace
207 
208 #endif
bool operator==(const ColorOcTreeNode &rhs) const
Definition: ColorOcTree.h:81
Base implementation for Occupancy Octrees (e.g.
ColorOcTreeNode * averageNodeColor(const OcTreeKey &key, const unsigned char &r, const unsigned char &g, const unsigned char &b)
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
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:170
ColorOcTree(double resolution)
Default constructor, sets resolution of leafs.
Definition: ColorOcTree.h:134
bool operator!=(const Color &other) const
Definition: ColorOcTree.h:70
ColorOcTreeNode * getChild(unsigned int i)
Definition: ColorOcTree.h:86
std::istream & readValue(std::istream &s)
std::ostream & writeValue(std::ostream &s) const
ColorOcTreeNode::Color getAverageChildColor() const
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:146
Color(unsigned char _r, unsigned char _g, unsigned char _b)
Definition: ColorOcTree.h:65
bool createChild(unsigned int i)
Definition: ColorOcTree.h:93
Static member object which ensures that this OcTree's prototype ends up in the classIDMapping only on...
Definition: ColorOcTree.h:191
OcTreeDataNode< float > ** children
pointer to array of children, may be NULL
void writeColorHistogram(std::string filename)
bool isColorSet() const
Definition: ColorOcTree.h:111
void updateInnerOccupancyRecurs(ColorOcTreeNode *node, unsigned int depth)
ColorOcTreeNode * integrateNodeColor(const OcTreeKey &key, const unsigned char &r, const unsigned char &g, const unsigned char &b)
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:158
bool operator==(const Color &other) const
Definition: ColorOcTree.h:67
std::ostream & operator<<(std::ostream &out, ColorOcTreeNode::Color const &c)
user friendly output in format (r g b)
ColorOcTreeNode * setNodeColor(const OcTreeKey &key, const unsigned char &r, const unsigned char &g, const unsigned char &b)
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking.
std::string getTreeType() const
returns actual class name as string for identification
Definition: ColorOcTree.h:140
void setColor(Color c)
Definition: ColorOcTree.h:103
ColorOcTree * create() const
virtual constructor: creates a new object of same type (Covariant return type requires an up-to-date ...
Definition: ColorOcTree.h:138
OcTreeNode * getChild(unsigned int i)
Definition: OcTreeNode.h:76
T value
stored data (payload)
static void registerTreeType(AbstractOcTree *tree)
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:68
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:63
void setColor(unsigned char r, unsigned char g, unsigned char b)
Definition: ColorOcTree.h:104
Color getColor() const
Definition: ColorOcTree.h:102
ColorOcTreeNode(const ColorOcTreeNode &rhs)
Definition: ColorOcTree.h:79
Nodes to be used in OcTree.
Definition: OcTreeNode.h:67
static StaticMemberInitializer colorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: ColorOcTree.h:199
const ColorOcTreeNode * getChild(unsigned int i) const
Definition: ColorOcTree.h:89



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Thu Dec 10 00:07:55 UTC 2015