ColorOcTree.h
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 #ifndef OCTOMAP_COLOR_OCTREE_H
00035 #define OCTOMAP_COLOR_OCTREE_H
00036 
00037 
00038 #include <iostream>
00039 #include <octomap/OcTreeNode.h>
00040 #include <octomap/OccupancyOcTreeBase.h>
00041 
00042 namespace octomap {
00043   
00044   // node definition
00045   class ColorOcTreeNode : public OcTreeNode {    
00046   public:
00047     
00048     class Color {
00049     public:
00050     Color() : r(255), g(255), b(255) {}
00051     Color(unsigned char _r, unsigned char _g, unsigned char _b) 
00052       : r(_r), g(_g), b(_b) {}
00053       inline bool operator== (const Color &other) const {
00054         return (r==other.r && g==other.g && b==other.b);
00055       }
00056       inline bool operator!= (const Color &other) const {
00057         return (r!=other.r || g!=other.g || b!=other.b);
00058       }
00059       unsigned char r, g, b;
00060     };
00061 
00062   public:
00063     ColorOcTreeNode() : OcTreeNode() {}
00064 
00065     ColorOcTreeNode(const ColorOcTreeNode& rhs) : OcTreeNode(rhs), color(rhs.color) {}
00066 
00067     bool operator==(const ColorOcTreeNode& rhs) const{
00068       return (rhs.value == value && rhs.color == color);
00069     }
00070     
00071     // children
00072     inline ColorOcTreeNode* getChild(unsigned int i) {
00073       return static_cast<ColorOcTreeNode*> (OcTreeNode::getChild(i));
00074     }
00075     inline const ColorOcTreeNode* getChild(unsigned int i) const {
00076       return static_cast<const ColorOcTreeNode*> (OcTreeNode::getChild(i));
00077     }
00078 
00079     bool createChild(unsigned int i) {
00080       if (children == NULL) allocChildren();
00081       children[i] = new ColorOcTreeNode();
00082       return true;
00083     }
00084 
00085     bool pruneNode();
00086     void expandNode();
00087     
00088     inline Color getColor() const { return color; }
00089     inline void  setColor(Color c) {this->color = c; }
00090     inline void  setColor(unsigned char r, unsigned char g, unsigned char b) {
00091       this->color = Color(r,g,b); 
00092     }
00093 
00094     Color& getColor() { return color; }
00095 
00096     // has any color been integrated? (pure white is very unlikely...)
00097     inline bool isColorSet() const { 
00098       return ((color.r != 255) || (color.g != 255) || (color.b != 255)); 
00099     }
00100 
00101     void updateColorChildren();
00102 
00103 
00104     ColorOcTreeNode::Color getAverageChildColor() const;
00105   
00106     // file I/O
00107     std::istream& readValue (std::istream &s);
00108     std::ostream& writeValue(std::ostream &s) const;
00109     
00110   protected:
00111     Color color;
00112   };
00113 
00114 
00115   // tree definition
00116   class ColorOcTree : public OccupancyOcTreeBase <ColorOcTreeNode> {
00117 
00118   public:
00120     ColorOcTree(double resolution) : OccupancyOcTreeBase<ColorOcTreeNode>(resolution) {};  
00121       
00124     ColorOcTree* create() const {return new ColorOcTree(resolution); }
00125 
00126     std::string getTreeType() const {return "ColorOcTree";}
00127    
00128     // set node color at given key or coordinate. Replaces previous color.
00129     ColorOcTreeNode* setNodeColor(const OcTreeKey& key, const unsigned char& r, 
00130                                  const unsigned char& g, const unsigned char& b);
00131 
00132     ColorOcTreeNode* setNodeColor(const float& x, const float& y, 
00133                                  const float& z, const unsigned char& r, 
00134                                  const unsigned char& g, const unsigned char& b) {
00135       OcTreeKey key;
00136       if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
00137       return setNodeColor(key,r,g,b);
00138     }
00139 
00140     // integrate color measurement at given key or coordinate. Average with previous color
00141     ColorOcTreeNode* averageNodeColor(const OcTreeKey& key, const unsigned char& r, 
00142                                   const unsigned char& g, const unsigned char& b);
00143     
00144     ColorOcTreeNode* averageNodeColor(const float& x, const float& y, 
00145                                       const float& z, const unsigned char& r, 
00146                                       const unsigned char& g, const unsigned char& b) {
00147       OcTreeKey key;
00148       if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
00149       return averageNodeColor(key,r,g,b);
00150     }
00151 
00152     // integrate color measurement at given key or coordinate. Average with previous color
00153     ColorOcTreeNode* integrateNodeColor(const OcTreeKey& key, const unsigned char& r, 
00154                                   const unsigned char& g, const unsigned char& b);
00155     
00156     ColorOcTreeNode* integrateNodeColor(const float& x, const float& y, 
00157                                       const float& z, const unsigned char& r, 
00158                                       const unsigned char& g, const unsigned char& b) {
00159       OcTreeKey key;
00160       if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
00161       return integrateNodeColor(key,r,g,b);
00162     }
00163 
00164     // update inner nodes, sets color to average child color
00165     void updateInnerOccupancy();
00166 
00167     // uses gnuplot to plot a RGB histogram in EPS format
00168     void writeColorHistogram(std::string filename);
00169     
00170   protected:
00171     void updateInnerOccupancyRecurs(ColorOcTreeNode* node, unsigned int depth);
00172 
00177     class StaticMemberInitializer{
00178        public:
00179          StaticMemberInitializer() {
00180            ColorOcTree* tree = new ColorOcTree(0.1);
00181            AbstractOcTree::registerTreeType(tree);
00182          }
00183     };
00185     static StaticMemberInitializer colorOcTreeMemberInit;
00186 
00187   };
00188 
00190   std::ostream& operator<<(std::ostream& out, ColorOcTreeNode::Color const& c);
00191 
00192 } // end namespace
00193 
00194 #endif


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