ColorOcTree.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #include <octomap/ColorOcTree.h>
00035 
00036 namespace octomap {
00037 
00038 
00039   // node implementation  --------------------------------------
00040   std::ostream& ColorOcTreeNode::writeValue (std::ostream &s) const {
00041     // 1 bit for each children; 0: empty, 1: allocated
00042     std::bitset<8> children;
00043     for (unsigned int i=0; i<8; i++) {
00044       if (childExists(i)) children[i] = 1;
00045       else                children[i] = 0;
00046     }
00047     char children_char = (char) children.to_ulong();
00048     
00049     // write node data
00050     s.write((const char*) &value, sizeof(value)); // occupancy
00051     s.write((const char*) &color, sizeof(Color)); // color
00052     s.write((char*)&children_char, sizeof(char)); // child existence
00053 
00054     // write existing children
00055     for (unsigned int i=0; i<8; ++i) 
00056       if (children[i] == 1) this->getChild(i)->writeValue(s);    
00057     return s;
00058   }
00059 
00060   std::istream& ColorOcTreeNode::readValue (std::istream &s) {
00061     // read node data
00062     char children_char;
00063     s.read((char*) &value, sizeof(value)); // occupancy
00064     s.read((char*) &color, sizeof(Color)); // color
00065     s.read((char*)&children_char, sizeof(char)); // child existence
00066 
00067     // read existing children
00068     std::bitset<8> children ((unsigned long long) children_char);
00069     for (unsigned int i=0; i<8; i++) {
00070       if (children[i] == 1){
00071         createChild(i);
00072         getChild(i)->readValue(s);
00073       }
00074     }
00075     return s;
00076   }
00077 
00078   ColorOcTreeNode::Color ColorOcTreeNode::getAverageChildColor() const {
00079     int mr(0), mg(0), mb(0);
00080     int c(0);
00081     for (int i=0; i<8; i++) {
00082       if (childExists(i) && getChild(i)->isColorSet()) {
00083         mr += getChild(i)->getColor().r;
00084         mg += getChild(i)->getColor().g;
00085         mb += getChild(i)->getColor().b;
00086         ++c;
00087       }
00088     }
00089     if (c) {
00090       mr /= c;
00091       mg /= c;
00092       mb /= c;
00093       return Color((unsigned char) mr, (unsigned char) mg, (unsigned char) mb);
00094     }
00095     else { // no child had a color other than white
00096       return Color(255, 255, 255);
00097     }
00098   }
00099 
00100 
00101   void ColorOcTreeNode::updateColorChildren() {      
00102     color = getAverageChildColor();
00103   }
00104 
00105   // pruning =============
00106 
00107   bool ColorOcTreeNode::pruneNode() {
00108     // checks for equal occupancy only, color ignored
00109     if (!this->collapsible()) return false;
00110     // set occupancy value 
00111     setLogOdds(getChild(0)->getLogOdds());
00112     // set color to average color
00113     if (isColorSet()) color = getAverageChildColor();
00114     // delete children
00115     for (unsigned int i=0;i<8;i++) {
00116       delete children[i];
00117     }
00118     delete[] children;
00119     children = NULL;
00120     return true;
00121   }
00122 
00123   void ColorOcTreeNode::expandNode() {
00124     assert(!hasChildren());
00125     for (unsigned int k=0; k<8; k++) {
00126       createChild(k);
00127       children[k]->setValue(value);
00128       getChild(k)->setColor(color);
00129     }
00130   }
00131 
00132   // tree implementation  --------------------------------------
00133 
00134   ColorOcTreeNode* ColorOcTree::setNodeColor(const OcTreeKey& key, 
00135                                              const unsigned char& r, 
00136                                              const unsigned char& g, 
00137                                              const unsigned char& b) {
00138     ColorOcTreeNode* n = search (key);
00139     if (n != 0) {
00140       n->setColor(r, g, b); 
00141     }
00142     return n;
00143   }
00144 
00145   ColorOcTreeNode* ColorOcTree::averageNodeColor(const OcTreeKey& key, 
00146                                                  const unsigned char& r, 
00147                                                  const unsigned char& g, 
00148                                                  const unsigned char& b) {
00149     ColorOcTreeNode* n = search (key);
00150     if (n != 0) {
00151       if (n->isColorSet()) {
00152         ColorOcTreeNode::Color prev_color = n->getColor();
00153         n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2); 
00154       }
00155       else {
00156         n->setColor(r, g, b);
00157       }
00158     }
00159     return n;
00160   }
00161 
00162   ColorOcTreeNode* ColorOcTree::integrateNodeColor(const OcTreeKey& key, 
00163                                                    const unsigned char& r, 
00164                                                    const unsigned char& g, 
00165                                                    const unsigned char& b) {
00166     ColorOcTreeNode* n = search (key);
00167     if (n != 0) {
00168       if (n->isColorSet()) {
00169         ColorOcTreeNode::Color prev_color = n->getColor();
00170         double node_prob = n->getOccupancy();
00171         unsigned char new_r = (unsigned char) ((double) prev_color.r * node_prob 
00172                                                +  (double) r * (0.99-node_prob));
00173         unsigned char new_g = (unsigned char) ((double) prev_color.g * node_prob 
00174                                                +  (double) g * (0.99-node_prob));
00175         unsigned char new_b = (unsigned char) ((double) prev_color.b * node_prob 
00176                                                +  (double) b * (0.99-node_prob));
00177         n->setColor(new_r, new_g, new_b); 
00178       }
00179       else {
00180         n->setColor(r, g, b);
00181       }
00182     }
00183     return n;
00184   }
00185   
00186   
00187   void ColorOcTree::updateInnerOccupancy() {
00188     this->updateInnerOccupancyRecurs(this->root, 0);
00189   }
00190 
00191   void ColorOcTree::updateInnerOccupancyRecurs(ColorOcTreeNode* node, unsigned int depth) {
00192     // only recurse and update for inner nodes:
00193     if (node->hasChildren()){
00194       // return early for last level:
00195       if (depth < this->tree_depth){
00196         for (unsigned int i=0; i<8; i++) {
00197           if (node->childExists(i)) {
00198             updateInnerOccupancyRecurs(node->getChild(i), depth+1);
00199           }
00200         }
00201       }
00202       node->updateOccupancyChildren();
00203       node->updateColorChildren();
00204     }
00205   }
00206 
00207   void ColorOcTree::writeColorHistogram(std::string filename) {
00208 
00209 #ifdef _MSC_VER
00210     fprintf(stderr, "The color histogram uses gnuplot, this is not supported under windows.\n");
00211 #else
00212     // build RGB histogram
00213     std::vector<int> histogram_r (256,0);
00214     std::vector<int> histogram_g (256,0);
00215     std::vector<int> histogram_b (256,0);
00216     for(ColorOcTree::tree_iterator it = this->begin_tree(),
00217           end=this->end_tree(); it!= end; ++it) {
00218       if (!it.isLeaf() || !this->isNodeOccupied(*it)) continue;
00219       ColorOcTreeNode::Color& c = it->getColor();
00220       ++histogram_r[c.r];
00221       ++histogram_g[c.g];
00222       ++histogram_b[c.b];
00223     }
00224     // plot data
00225     FILE *gui = popen("gnuplot ", "w");
00226     fprintf(gui, "set term postscript eps enhanced color\n");
00227     fprintf(gui, "set output \"%s\"\n", filename.c_str());
00228     fprintf(gui, "plot [-1:256] ");
00229     fprintf(gui,"'-' w filledcurve lt 1 lc 1 tit \"r\",");
00230     fprintf(gui, "'-' w filledcurve lt 1 lc 2 tit \"g\",");
00231     fprintf(gui, "'-' w filledcurve lt 1 lc 3 tit \"b\",");
00232     fprintf(gui, "'-' w l lt 1 lc 1 tit \"\",");
00233     fprintf(gui, "'-' w l lt 1 lc 2 tit \"\",");
00234     fprintf(gui, "'-' w l lt 1 lc 3 tit \"\"\n");
00235 
00236     for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_r[i]);    
00237     fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
00238     for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_g[i]);    
00239     fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
00240     for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_b[i]);    
00241     fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
00242     for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_r[i]);    
00243     fprintf(gui, "e\n");
00244     for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_g[i]);    
00245     fprintf(gui, "e\n");
00246     for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_b[i]);    
00247     fprintf(gui, "e\n");
00248     fflush(gui);
00249 #endif
00250   }
00251 
00252   std::ostream& operator<<(std::ostream& out, ColorOcTreeNode::Color const& c) {
00253     return out << '(' << (unsigned int)c.r << ' ' << (unsigned int)c.g << ' ' << (unsigned int)c.b << ')';
00254   }
00255 
00256 
00257   ColorOcTree::StaticMemberInitializer ColorOcTree::colorOcTreeMemberInit;
00258 
00259 } // end namespace
00260 


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Jun 6 2019 17:31:45