bayes_histos_to_lut.cpp

00001 
00002 /**************************************************************************
00003  *  bayes_histos_to_lut.cpp - This file implements a class
00004  *                            that takes color histograms of objects as input,
00005  *                            and, together with probabilities of objects,
00006  *                            generates all the values for a lookup-table
00007  *                            that maps from colors to objects
00008  *
00009  *  Generated: Mon Jun 27 14:16:52 2005
00010  *  Copyright  2005       Martin Heracles
00011  *             2005-2008  Tim Niemueller [www.niemueller.de]
00012  *             2007-2008  Daniel Beck
00013  *
00014  ***************************************************************************/
00015 
00016 /*  This program is free software; you can redistribute it and/or modify
00017  *  it under the terms of the GNU General Public License as published by
00018  *  the Free Software Foundation; either version 2 of the License, or
00019  *  (at your option) any later version. A runtime exception applies to
00020  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00021  *
00022  *  This program is distributed in the hope that it will be useful,
00023  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00024  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00025  *  GNU Library General Public License for more details.
00026  *
00027  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00028  */
00029 
00030 #include <fvutils/colormap/bayes/bayes_histos_to_lut.h>
00031 #include <fvutils/statistical/histogram.h>
00032 #include <fvutils/colormap/yuvcm.h>
00033 #include <fvutils/colormap/cmfile.h>
00034 #include <core/exception.h>
00035 
00036 #include <fvutils/color/color_object_map.h>
00037 
00038 #include <iostream>
00039 #include <string>
00040 #include <cstdlib>
00041 #include <cstdio>
00042 
00043 using namespace std;
00044 
00045 namespace firevision {
00046 #if 0 /* just to make Emacs auto-indent happy */
00047 }
00048 #endif
00049 
00050 /** @class BayesHistosToLut <fvutils/colormap/bayes/bayes_histos_to_lut.h>
00051  * LUT generation by using Bayesian method on histograms.
00052  * Generates a YUV colormap.
00053  * @author Martin Herakles.
00054  * @author Tim Niemueller
00055  * @author Daniel Beck
00056  */
00057 
00058 /** Constructor.
00059  * @param histos histograms
00060  * @param d depth of lookup table
00061  * @param object type of the foreground object
00062  * @param w the width of the lookup table (u-resolution)
00063  * @param h the height of the lookup table (v-resolution)
00064  */
00065 BayesHistosToLut::BayesHistosToLut(std::map<hint_t, Histogram*> &histos,
00066                                    unsigned int d, hint_t object, unsigned int w, unsigned int h)
00067   : histograms(histos)
00068 {
00069   width  = w;
00070   height = h;
00071   depth  = d;
00072 
00073   fg_object  = object;
00074   //  histograms = histos;
00075 
00076   // no as shmem segment
00077   lut = new YuvColormap(depth, width, height);
00078 
00079   min_probability = 0.3;
00080   min_prob_ball = 0.0;
00081   min_prob_green = 0.0;
00082   min_prob_yellow = 0.0;
00083   min_prob_blue = 0.0;
00084   min_prob_white = 0.0;
00085   min_prob_black = 0.0;
00086 }
00087 
00088 /** Destructor. */
00089 BayesHistosToLut::~BayesHistosToLut()
00090 {
00091   delete lut;
00092 }
00093 
00094 /** Get name.
00095  * @return BayesHistosToLut
00096  */
00097 string
00098 BayesHistosToLut::getName()
00099 {
00100   return string("BayesHistosToLut");
00101 }
00102 
00103 /** Get object probability.
00104  * @param object object
00105  * @return probability.
00106  */
00107 float
00108 BayesHistosToLut::getObjectProb(hint_t object)
00109 {
00110   // These object probabilities should better be read from config file.
00111 
00112   if (fg_object == H_BALL) {
00113     /*
00114     switch (object) {
00115     case H_BALL:
00116     */
00117       return 0.2;
00118     /*
00119       break;
00120     case H_BACKGROUND:
00121       return 0.8;
00122       break;
00123     case H_ROBOT:
00124       return 0.0;
00125       break;
00126     case H_FIELD:
00127       return 0.0;
00128       break;
00129     case H_GOAL_BLUE:
00130       return 0.0;
00131       break;
00132     case H_GOAL_YELLOW:
00133       return 0.0;
00134       break;
00135     case H_LINE:
00136       return 0.0;
00137       break;
00138     case H_UNKNOWN:
00139       return 0.0;
00140       break;
00141     default:
00142       cout << "(BayesHistosToLut::getObjectProb): Invalid object." << endl;
00143       exit(-1);
00144       return 0.0f;
00145       break;
00146     }
00147     */
00148   } else {
00149     if ( object_probabilities.find(object) != object_probabilities.end() ) {
00150       return object_probabilities[object];
00151     } else {
00152       cout << "returning 0" << endl;
00153       return 0.f;
00154     }
00155   }
00156 }
00157 
00158 /** P(u, v| object).
00159  * Get a-priori probability.
00160  * @param u YUV U-value
00161  * @param v YUV V-value
00162  * @param object object.
00163  * @return probability
00164  */
00165 float
00166 BayesHistosToLut::getAPrioriProb(unsigned int u,
00167                                  unsigned int v,
00168                                  hint_t object)
00169 {
00170   unsigned int sum = 0;
00171   for (unsigned int y = 0; y < depth; ++y) {
00172     sum += histograms[object]->get_value(u, v, y);
00173   }
00174 
00175   return ( float(sum) / float(numberOfOccurrences[object]) );
00176 }
00177 
00178 /** P(u, v| object).
00179  * Get a-priori probability.
00180  * @param y YUV Y-value
00181  * @param u YUV U-value
00182  * @param v YUV V-value
00183  * @param object object.
00184  * @return probability
00185  */
00186 float
00187 BayesHistosToLut::getAPrioriProb(unsigned int y,
00188                                  unsigned int u,
00189                                  unsigned int v,
00190                                  hint_t object)
00191 {
00192   return ( float(histograms[object]->get_value(u, v, y)) / float(numberOfOccurrences[object]) );
00193 }
00194 
00195 /** P(object| u, v).
00196  * Get a-posteriori probability.
00197  * @param object objcet
00198  * @param u YUV U-value
00199  * @param v YUV V-value
00200  * @return a posteriori probability
00201  */
00202 float
00203 BayesHistosToLut::getAPosterioriProb(hint_t object,
00204                                      unsigned int u,
00205                                      unsigned int v)
00206 {
00207   /* calculate "nenner" for bayes-formula,
00208      i.e. sum up the probabilities P(u, v| object) * P(object)
00209      over all objects */
00210   float sumOfProbabilities = 0.0;
00211   map<hint_t, Histogram*>::iterator hit;
00212   for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00213     sumOfProbabilities += ( getAPrioriProb(u, v, (hint_t)hit->first) * getObjectProb((hint_t)hit->first) );
00214   }
00215 
00216   if (sumOfProbabilities != 0) {
00217     return getAPrioriProb(u, v, object) * getObjectProb(object) / sumOfProbabilities;
00218   }
00219   else
00220     return 0;
00221 }
00222 
00223 /** P(object| u, v).
00224  * Get a-posteriori probability.
00225  * @param object objcet
00226  * @param y YUV Y-value
00227  * @param u YUV U-value
00228  * @param v YUV V-value
00229  * @return a posteriori probability
00230  */
00231 float
00232 BayesHistosToLut::getAPosterioriProb(hint_t object,
00233                                      unsigned int y,
00234                                      unsigned int u,
00235                                      unsigned int v)
00236 {
00237   /* calculate "nenner" for bayes-formula,
00238      i.e. sum up the probabilities P(u, v| object) * P(object)
00239      over all objects */
00240   float sumOfProbabilities = 0.0;
00241   map<hint_t, Histogram*>::iterator hit;
00242   for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00243     sumOfProbabilities += ( getAPrioriProb(y, u, v, (hint_t)hit->first) * getObjectProb((hint_t)hit->first) );
00244   }
00245 
00246   if (sumOfProbabilities != 0) {
00247     return getAPrioriProb(y, u, v, object) * getObjectProb(object) / sumOfProbabilities;
00248   }
00249   else
00250     return 0;
00251 }
00252 
00253 /** Get most likely object.
00254  * @param u YUV U-value
00255  * @param v YUV V-value
00256  * @return most likely object for this color
00257  */
00258 hint_t
00259 BayesHistosToLut::getMostLikelyObject(unsigned int u,
00260                                       unsigned int v)
00261 {
00262   // TODO sum over all y-values
00263 
00264   hint_t mostLikelyObject = H_UNKNOWN;
00265   float probOfMostLikelyObject = 0.0;
00266   map<hint_t, Histogram*>::iterator hit;
00267   for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00268     float tmp = getAPosterioriProb((hint_t)hit->first, u, v);
00269 
00270     if (tmp > probOfMostLikelyObject) {
00271       probOfMostLikelyObject = tmp;
00272       mostLikelyObject = (hint_t)hit->first;
00273     }
00274   }
00275 
00276   if (probOfMostLikelyObject > min_probability) {
00277     return mostLikelyObject;
00278   }
00279   else {
00280     return H_UNKNOWN;
00281   }
00282 }
00283 
00284 /** Get most likely object.
00285  * @param y YUV Y-value
00286  * @param u YUV U-value
00287  * @param v YUV V-value
00288  * @return most likely object for this color
00289  */
00290 hint_t
00291 BayesHistosToLut::getMostLikelyObject(unsigned int y,
00292                                       unsigned int u,
00293                                       unsigned int v)
00294 {
00295   hint_t mostLikelyObject = H_UNKNOWN;
00296   float probOfMostLikelyObject = 0.0;
00297   map<hint_t, Histogram*>::iterator hit;
00298   for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00299     float tmp = getAPosterioriProb((hint_t)hit->first, y, u, v);
00300 
00301     if (tmp > probOfMostLikelyObject) {
00302       probOfMostLikelyObject = tmp;
00303       mostLikelyObject = (hint_t)hit->first;
00304     }
00305   }
00306 
00307   if (probOfMostLikelyObject > min_probability) {
00308     return mostLikelyObject;
00309   }
00310   else {
00311     return H_UNKNOWN;
00312   }
00313 }
00314 
00315 /** Calculate all LUT colors. */
00316 void
00317 BayesHistosToLut::calculateLutAllColors()
00318 {
00319   // for each histogram, sum up all of its entries
00320   //  numberOfOccurrences.resize( histograms.size() );
00321   map<hint_t, Histogram*>::iterator hit;
00322   for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00323     unsigned int total = 0;
00324     for (unsigned int v = 0; v < height; ++v) {
00325       for (unsigned int u = 0; u < width; ++u) {
00326         for (unsigned int y = 0; y < depth; ++y) {
00327           unsigned int tmp = ((Histogram*)(hit->second))->get_value(u, v, y);
00328           if (tmp > 0)
00329             total += tmp;
00330         }
00331       }
00332     }
00333     numberOfOccurrences[ (hint_t)hit->first ] = total;
00334   }
00335 
00336   /*
00337   cout << "histo-BALL : " << numberOfOccurrences[0] << " counts." << endl
00338        << "histo-GREEN: " << numberOfOccurrences[3] << " counts." << endl
00339        << "histo-BLUE : " << numberOfOccurrences[5] << " counts." << endl;
00340   */
00341 
00342   // for each color, mark it (in lut) as the color
00343   // that has the highest probability (among all histograms)
00344   hint_t   color_with_highest_prob;
00345   float    highest_prob;
00346   float    current_prob;
00347   for (unsigned int y = 0; y < depth; ++y) {
00348     unsigned int y_index = y * lut->deepness() / lut->depth();
00349     for (unsigned int v = 0; v < height; ++v) {
00350       for (unsigned int u = 0; u < width; ++u) {
00351 
00352         // find most probable color for (u, v)
00353         highest_prob = 0.0;
00354         color_with_highest_prob = H_UNKNOWN; // ...maybe it is better to have default = H_BACKGROUND...
00355         map<hint_t, Histogram*>::iterator hit;
00356         for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00357           // if current histogram is not empty...
00358           if (numberOfOccurrences[ (hint_t)hit->first ] > 0) {
00359             current_prob = float( hit->second->get_value(u, v, y) ) / float( numberOfOccurrences[ hit->first ] );
00360             // if current histogram has higher probability for color (u, v),
00361             // _and_ is above min_prob-threshold...
00362             if ( current_prob > highest_prob &&
00363                  current_prob > min_probability ) {
00364               // ...update color information
00365               highest_prob = current_prob;
00366               color_with_highest_prob = hit->first;
00367             }
00368           }
00369         }
00370 
00371         // set lut value for color (u, v) to most probable color
00372         lut->set(y_index, u, v, ColorObjectMap::get_instance().get(color_with_highest_prob));
00373       }
00374     }
00375   }
00376 
00377 }
00378 
00379 
00380 /** Calculate LUT values.
00381  * @param penalty if true, non-ball colors are penalized
00382  */
00383 void
00384 BayesHistosToLut::calculateLutValues( bool penalty )
00385 {
00386 
00387   unsigned int old_undo = 0;
00388 
00389   if ( penalty ) {
00390     // We penalize all values, that have NOT been classified as ball
00391     Histogram *histo_fg = histograms[fg_object];
00392     Histogram *histo_bg = histograms[H_BACKGROUND];
00393 
00394     if ( histo_bg->get_num_undos() < 2 ) {
00395       // No undo available for us
00396       cout << "Histogram::calculateLutValues: There are not enough undos possible for background histogram, not penalizing" << endl;
00397     } else {
00398       unsigned int bg_median  = histo_bg->get_median();
00399       unsigned int bg_average = histo_bg->get_average();
00400        unsigned int bg_val = 0;
00401 
00402       old_undo = histo_bg->switch_undo( 1 );
00403 
00404       cout << "Histogram: Setting low bg vals to median. median=" << bg_median
00405            << "  avg=" << bg_average << endl;
00406 
00407       for (unsigned int v = 0; v < height; ++v) {
00408         for (unsigned int u = 0; u < width; ++u) {
00409           for (unsigned int y = 0; y < depth; ++y) {
00410 
00411             if ( histo_fg->get_value(u, v, y) == 0 ) {
00412               bg_val = histo_bg->get_value(u, v, y);
00413               if (bg_val < bg_average) {
00414                 histo_bg->set_value(u, v, y, bg_average);
00415               }
00416             }
00417           }
00418         }
00419       }
00420     }
00421   }
00422 
00423   /* count for each object
00424      how many non-zero values its histogram has in total */
00425   //  numberOfOccurrences.resize(histograms.size());
00426 
00427   map<hint_t, Histogram*>::iterator hit;
00428   for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00429     unsigned int total = 0;
00430     for (unsigned int y = 0; y < depth; ++y) {
00431       for (unsigned int v = 0; v < height; ++v) {
00432         for (unsigned int u = 0; u < width; ++u) {
00433           unsigned int tmp = hit->second->get_value(u, v, y);
00434           if (tmp > 0)
00435             total += tmp;
00436         }
00437       }
00438     }
00439     numberOfOccurrences[hit->first] = total;
00440     cout << "[" << hit->first << "]: " << numberOfOccurrences[hit->first] << " occurences" << endl;
00441   }
00442 
00443   unsigned int total_count = 0;
00444   for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00445     total_count += hit->second->get_sum();
00446   }
00447   //  cout << "Total count: " << total_count << endl;
00448 
00449   // Calculate overall object probabilities
00450   for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00451     object_probabilities[hit->first] = (float)hit->second->get_sum() / (float)total_count;
00452 
00453     //     cout << "Setting a-priori probability for histogram " << hit->first << " to "
00454     //   << object_probabilities[hit->first] << endl;
00455   }
00456 
00457 
00458   unsigned int count_ball       = 0;
00459   unsigned int count_field      = 0;
00460   unsigned int count_line       = 0;
00461   unsigned int count_robot      = 0;
00462   unsigned int count_background = 0;
00463   unsigned int count_goal       = 0;
00464   unsigned int count_unknown    = 0;
00465 
00466   lut->reset();
00467 
00468   for (unsigned int y = 0; y < depth; ++y) {
00469     unsigned int y_index = y * lut->deepness() / lut->depth();
00470     for (unsigned int u = 0; u < width; ++u) {
00471       unsigned int u_index = u * lut->deepness() / lut->width();
00472       for (unsigned int v = 0; v < height; ++v) {
00473         unsigned int v_index = v * lut->deepness() / lut->height();
00474         hint_t mostLikelyObject = getMostLikelyObject(y, u, v);
00475 
00476         switch(mostLikelyObject) {
00477         case H_BALL:
00478           count_ball++;
00479           break;
00480         case H_BACKGROUND:
00481           count_background++;
00482           break;
00483         case H_ROBOT:
00484         case H_ROBOT_OPP:
00485           count_robot++;
00486           break;
00487         case H_FIELD:
00488           count_field++;
00489           break;
00490         case H_LINE:
00491           count_line++;
00492           break;
00493         case H_GOAL_YELLOW:
00494         case H_GOAL_BLUE:
00495           count_goal++;
00496           break;
00497         case H_UNKNOWN:
00498           count_unknown++;
00499           break;
00500         default:
00501           cout << "(BayesHistosToLut::calculateLutValues(): Invalid object." << endl;
00502           throw fawkes::Exception("BayesHistosToLut::calculateLutValues(): Invalid object.");
00503         }
00504         lut->set(y_index, u_index, v_index, ColorObjectMap::get_instance().get(mostLikelyObject));
00505       }
00506     }
00507   }
00508 
00509         printf("d/w/h: %u/%u/%u  ball: %d  field: %d  line: %d  robot: %d  goal: %d  background: %d  unknown: %d\n",
00510                depth, width, height, count_ball, count_field, count_line,
00511                count_robot, count_goal, count_background, count_unknown);
00512 
00513   if ( penalty ) {
00514     Histogram *histo_bg   = histograms[H_BACKGROUND];
00515     if ( histo_bg->get_num_undos() >= 2 ) {
00516       histo_bg->undo();
00517       histo_bg->switch_undo( old_undo );
00518     }
00519   }
00520 
00521 
00522   /*
00523   // for testing: output ball colors
00524   cout << " ============" << endl;
00525   for (unsigned int v = 0; v < height; v++) {
00526     for (unsigned int u = 0; u < width; u++) {
00527       if (lut->determine(128, u, v) == BACKGROUND)
00528         cout << "lut says that (" << u << ", " << v << ") is background color." << endl;
00529     }
00530   }
00531   cout << "===============" << endl;
00532   */
00533 }
00534 
00535 /** Save LUT to file.
00536  * @param file file name
00537  */
00538 void
00539 BayesHistosToLut::saveLut(char *file)
00540 {
00541   ColormapFile cmf;
00542   cmf.add_colormap(lut);
00543   cmf.write(file);
00544 }
00545 
00546 /** Save LUT to file.
00547  * @param filename file name
00548  */
00549 void
00550 BayesHistosToLut::save(std::string filename)
00551 {
00552   ColormapFile cmf;
00553   cmf.add_colormap(lut);
00554   cmf.write(filename.c_str());
00555 }
00556 
00557 
00558 /** Set min probability.
00559  * @param min_prob minimum probability
00560  */
00561 void
00562 BayesHistosToLut::setMinProbability( float min_prob )
00563 {
00564   min_probability = min_prob;
00565 }
00566 
00567 
00568 /** Set min probability for color.
00569  * @param min_prob minimum probability
00570  * @param hint color hint
00571  */
00572 void
00573 BayesHistosToLut::setMinProbForColor( float min_prob, hint_t hint ) {
00574   switch( hint ) {
00575   case H_BALL:
00576     min_prob_ball = min_prob;
00577     break;
00578   case H_FIELD:
00579     min_prob_green = min_prob;
00580     break;
00581   case H_GOAL_YELLOW:
00582     min_prob_yellow = min_prob;
00583     break;
00584   case H_GOAL_BLUE:
00585     min_prob_blue = min_prob;
00586     break;
00587   case H_LINE:
00588     min_prob_white = min_prob;
00589     break;
00590   case H_ROBOT:
00591     min_prob_black = min_prob;
00592     break;
00593   default:
00594     /**/
00595     break;
00596   }
00597 }
00598 
00599 
00600 /** Get generated color model.
00601  * @return generated color model
00602  */
00603 YuvColormap *
00604 BayesHistosToLut::get_colormap()
00605 {
00606   return lut;
00607 }
00608 
00609 } // end namespace firevision

Generated on Tue Feb 22 13:31:16 2011 for Fawkes API by  doxygen 1.4.7