00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
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
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
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
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
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
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
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
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
00165 void updateInnerOccupancy();
00166
00167
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 }
00193
00194 #endif