43 for (
unsigned int i=0; i<8; i++) {
47 char children_char = (char) children.to_ulong();
52 s.write((
char*)&children_char,
sizeof(
char));
55 for (
unsigned int i=0; i<8; ++i)
65 s.read((
char*)&children_char,
sizeof(
char));
68 std::bitset<8>
children ((
unsigned long long) children_char);
69 for (
unsigned int i=0; i<8; i++) {
70 if (children[i] == 1){
79 int mr(0), mg(0), mb(0);
81 for (
int i=0; i<8; i++) {
93 return Color((
unsigned char) mr, (
unsigned char) mg, (
unsigned char) mb);
96 return Color(255, 255, 255);
115 for (
unsigned int i=0;i<8;i++) {
125 for (
unsigned int k=0; k<8; k++) {
139 const unsigned char& r,
140 const unsigned char& g,
141 const unsigned char& b) {
150 const unsigned char& r,
151 const unsigned char& g,
152 const unsigned char& b) {
157 n->
setColor((prev_color.
r + r)/2, (prev_color.
g + g)/2, (prev_color.
b + b)/2);
167 const unsigned char& r,
168 const unsigned char& g,
169 const unsigned char& b) {
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));
200 for (
unsigned int i=0; i<8; i++) {
214 fprintf(stderr,
"The color histogram uses gnuplot, this is not supported under windows.\n");
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(),
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");
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]);
248 for (
int i=0; i<256; ++i) fprintf(gui,
"%d %d\n", i, histogram_g[i]);
250 for (
int i=0; i<256; ++i) fprintf(gui,
"%d %d\n", i, histogram_b[i]);
257 return out <<
'(' << (
unsigned int)c.
r <<
' ' << (
unsigned int)c.
g <<
' ' << (
unsigned int)c.
b <<
')';
ColorOcTreeNode::Color getAverageChildColor() const
float value
stored data (payload)
const tree_iterator end_tree() const
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
void updateColorChildren()
const iterator end() const
ColorOcTreeNode * search(double x, double y, double z, unsigned int depth=0) const
void updateOccupancyChildren()
update this node's occupancy according to its children's maximum occupancy
bool childExists(unsigned int i) const
ColorOcTreeNode * getChild(unsigned int i)
void setLogOdds(float l)
sets log odds occupancy of node
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)
double getOccupancy() const
void updateInnerOccupancy()
ColorOcTree(double resolution)
Default constructor, sets resolution of leafs.
tree_iterator begin_tree(unsigned char maxDepth=0) const
bool createChild(unsigned int i)
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
static StaticMemberInitializer colorOcTreeMemberInit
static member to ensure static initialization (only once)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree's parameter for "occupancy" ...
ColorOcTreeNode * setNodeColor(const OcTreeKey &key, const unsigned char &r, const unsigned char &g, const unsigned char &b)