ColorOcTree.cpp
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 #include <octomap/ColorOcTree.h>
35 
36 namespace octomap {
37 
38 
39  // node implementation --------------------------------------
40  std::ostream& ColorOcTreeNode::writeData(std::ostream &s) const {
41  s.write((const char*) &value, sizeof(value)); // occupancy
42  s.write((const char*) &color, sizeof(Color)); // color
43 
44  return s;
45  }
46 
47  std::istream& ColorOcTreeNode::readData(std::istream &s) {
48  s.read((char*) &value, sizeof(value)); // occupancy
49  s.read((char*) &color, sizeof(Color)); // color
50 
51  return s;
52  }
53 
55  int mr = 0;
56  int mg = 0;
57  int mb = 0;
58  int c = 0;
59 
60  if (children != NULL){
61  for (int i=0; i<8; i++) {
62  ColorOcTreeNode* child = static_cast<ColorOcTreeNode*>(children[i]);
63 
64  if (child != NULL && child->isColorSet()) {
65  mr += child->getColor().r;
66  mg += child->getColor().g;
67  mb += child->getColor().b;
68  ++c;
69  }
70  }
71  }
72 
73  if (c > 0) {
74  mr /= c;
75  mg /= c;
76  mb /= c;
77  return Color((uint8_t) mr, (uint8_t) mg, (uint8_t) mb);
78  }
79  else { // no child had a color other than white
80  return Color(255, 255, 255);
81  }
82  }
83 
84 
87  }
88 
89 
90  // tree implementation --------------------------------------
91  ColorOcTree::ColorOcTree(double resolution)
92  : OccupancyOcTreeBase<ColorOcTreeNode>(resolution) {
94  };
95 
97  uint8_t r,
98  uint8_t g,
99  uint8_t b) {
100  ColorOcTreeNode* n = search (key);
101  if (n != 0) {
102  n->setColor(r, g, b);
103  }
104  return n;
105  }
106 
108  if (!isNodeCollapsible(node))
109  return false;
110 
111  // set value to children's values (all assumed equal)
112  node->copyData(*(getNodeChild(node, 0)));
113 
114  if (node->isColorSet()) // TODO check
115  node->setColor(node->getAverageChildColor());
116 
117  // delete children
118  for (unsigned int i=0;i<8;i++) {
119  deleteNodeChild(node, i);
120  }
121  delete[] node->children;
122  node->children = NULL;
123 
124  return true;
125  }
126 
128  // all children must exist, must not have children of
129  // their own and have the same occupancy probability
130  if (!nodeChildExists(node, 0))
131  return false;
132 
133  const ColorOcTreeNode* firstChild = getNodeChild(node, 0);
134  if (nodeHasChildren(firstChild))
135  return false;
136 
137  for (unsigned int i = 1; i<8; i++) {
138  // compare nodes only using their occupancy, ignoring color for pruning
139  if (!nodeChildExists(node, i) || nodeHasChildren(getNodeChild(node, i)) || !(getNodeChild(node, i)->getValue() == firstChild->getValue()))
140  return false;
141  }
142 
143  return true;
144  }
145 
147  uint8_t r,
148  uint8_t g,
149  uint8_t b) {
150  ColorOcTreeNode* n = search(key);
151  if (n != 0) {
152  if (n->isColorSet()) {
153  ColorOcTreeNode::Color prev_color = n->getColor();
154  n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2);
155  }
156  else {
157  n->setColor(r, g, b);
158  }
159  }
160  return n;
161  }
162 
164  uint8_t r,
165  uint8_t g,
166  uint8_t b) {
167  ColorOcTreeNode* n = search (key);
168  if (n != 0) {
169  if (n->isColorSet()) {
170  ColorOcTreeNode::Color prev_color = n->getColor();
171  double node_prob = n->getOccupancy();
172  uint8_t new_r = (uint8_t) ((double) prev_color.r * node_prob
173  + (double) r * (0.99-node_prob));
174  uint8_t new_g = (uint8_t) ((double) prev_color.g * node_prob
175  + (double) g * (0.99-node_prob));
176  uint8_t new_b = (uint8_t) ((double) prev_color.b * node_prob
177  + (double) b * (0.99-node_prob));
178  n->setColor(new_r, new_g, new_b);
179  }
180  else {
181  n->setColor(r, g, b);
182  }
183  }
184  return n;
185  }
186 
187 
189  this->updateInnerOccupancyRecurs(this->root, 0);
190  }
191 
193  // only recurse and update for inner nodes:
194  if (nodeHasChildren(node)){
195  // return early for last level:
196  if (depth < this->tree_depth){
197  for (unsigned int i=0; i<8; i++) {
198  if (nodeChildExists(node, i)) {
199  updateInnerOccupancyRecurs(getNodeChild(node, i), depth+1);
200  }
201  }
202  }
203  node->updateOccupancyChildren();
204  node->updateColorChildren();
205  }
206  }
207 
208  void ColorOcTree::writeColorHistogram(std::string filename) {
209 
210 #ifdef _MSC_VER
211  fprintf(stderr, "The color histogram uses gnuplot, this is not supported under windows.\n");
212 #else
213  // build RGB histogram
214  std::vector<int> histogram_r (256,0);
215  std::vector<int> histogram_g (256,0);
216  std::vector<int> histogram_b (256,0);
217  for(ColorOcTree::tree_iterator it = this->begin_tree(),
218  end=this->end_tree(); it!= end; ++it) {
219  if (!it.isLeaf() || !this->isNodeOccupied(*it)) continue;
220  ColorOcTreeNode::Color& c = it->getColor();
221  ++histogram_r[c.r];
222  ++histogram_g[c.g];
223  ++histogram_b[c.b];
224  }
225  // plot data
226  FILE *gui = popen("gnuplot ", "w");
227  fprintf(gui, "set term postscript eps enhanced color\n");
228  fprintf(gui, "set output \"%s\"\n", filename.c_str());
229  fprintf(gui, "plot [-1:256] ");
230  fprintf(gui,"'-' w filledcurve lt 1 lc 1 tit \"r\",");
231  fprintf(gui, "'-' w filledcurve lt 1 lc 2 tit \"g\",");
232  fprintf(gui, "'-' w filledcurve lt 1 lc 3 tit \"b\",");
233  fprintf(gui, "'-' w l lt 1 lc 1 tit \"\",");
234  fprintf(gui, "'-' w l lt 1 lc 2 tit \"\",");
235  fprintf(gui, "'-' w l lt 1 lc 3 tit \"\"\n");
236 
237  for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_r[i]);
238  fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
239  for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_g[i]);
240  fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
241  for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_b[i]);
242  fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
243  for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_r[i]);
244  fprintf(gui, "e\n");
245  for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_g[i]);
246  fprintf(gui, "e\n");
247  for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_b[i]);
248  fprintf(gui, "e\n");
249  fflush(gui);
250 #endif
251  }
252 
253  std::ostream& operator<<(std::ostream& out, ColorOcTreeNode::Color const& c) {
254  return out << '(' << (unsigned int)c.r << ' ' << (unsigned int)c.g << ' ' << (unsigned int)c.b << ')';
255  }
256 
257 
258  ColorOcTree::StaticMemberInitializer ColorOcTree::colorOcTreeMemberInit;
259 
260 } // end namespace
261 
tree_iterator begin_tree(unsigned char maxDepth=0) const
float value
stored data (payload)
ColorOcTreeNode * integrateNodeColor(const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
const unsigned int tree_depth
Maximum tree depth is fixed to 16 currently.
void writeColorHistogram(std::string filename)
void updateInnerOccupancyRecurs(ColorOcTreeNode *node, unsigned int depth)
double getOccupancy() const
Definition: OcTreeNode.h:65
void deleteNodeChild(ColorOcTreeNode *node, unsigned int childIdx)
Deletes the i-th child of the node.
ColorOcTreeNode * getNodeChild(ColorOcTreeNode *node, unsigned int childIdx) const
bool isColorSet() const
Definition: ColorOcTree.h:89
ColorOcTreeNode * setNodeColor(const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.cpp:96
void updateOccupancyChildren()
update this node&#39;s occupancy according to its children&#39;s maximum occupancy
Definition: OcTreeNode.h:83
void setColor(Color c)
Definition: ColorOcTree.h:81
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 * search(double x, double y, double z, unsigned int depth=0) const
AbstractOcTreeNode ** children
ColorOcTreeNode * averageNodeColor(const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
ColorOcTreeNode::Color getAverageChildColor() const
Definition: ColorOcTree.cpp:54
virtual bool pruneNode(ColorOcTreeNode *node)
virtual bool isNodeCollapsible(const ColorOcTreeNode *node) const
ColorOcTree(double resolution)
Default constructor, sets resolution of leafs.
Definition: ColorOcTree.cpp:91
ColorOcTreeNode * root
Pointer to the root NODE, NULL for empty tree.
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
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
bool nodeChildExists(const ColorOcTreeNode *node, unsigned int childIdx) const
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