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 #include <octomap/ColorOcTree.h>
00035
00036 namespace octomap {
00037
00038
00039
00040 std::ostream& ColorOcTreeNode::writeValue (std::ostream &s) const {
00041
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
00050 s.write((const char*) &value, sizeof(value));
00051 s.write((const char*) &color, sizeof(Color));
00052 s.write((char*)&children_char, sizeof(char));
00053
00054
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
00062 char children_char;
00063 s.read((char*) &value, sizeof(value));
00064 s.read((char*) &color, sizeof(Color));
00065 s.read((char*)&children_char, sizeof(char));
00066
00067
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 {
00096 return Color(255, 255, 255);
00097 }
00098 }
00099
00100
00101 void ColorOcTreeNode::updateColorChildren() {
00102 color = getAverageChildColor();
00103 }
00104
00105
00106
00107 bool ColorOcTreeNode::pruneNode() {
00108
00109 if (!this->collapsible()) return false;
00110
00111 setLogOdds(getChild(0)->getLogOdds());
00112
00113 if (isColorSet()) color = getAverageChildColor();
00114
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
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
00193 if (node->hasChildren()){
00194
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
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
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 }
00260