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


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Jun 10 2019 14:00:13