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