Fawkes API
Fawkes Development Version
|
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