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 #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
00047 }
00048 #endif
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
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
00075
00076
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
00089 BayesHistosToLut::~BayesHistosToLut()
00090 {
00091 delete lut;
00092 }
00093
00094
00095
00096
00097 string
00098 BayesHistosToLut::getName()
00099 {
00100 return string("BayesHistosToLut");
00101 }
00102
00103
00104
00105
00106
00107 float
00108 BayesHistosToLut::getObjectProb(hint_t object)
00109 {
00110
00111
00112 if (fg_object == H_BALL) {
00113
00114
00115
00116
00117 return 0.2;
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
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
00159
00160
00161
00162
00163
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
00179
00180
00181
00182
00183
00184
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
00196
00197
00198
00199
00200
00201
00202 float
00203 BayesHistosToLut::getAPosterioriProb(hint_t object,
00204 unsigned int u,
00205 unsigned int v)
00206 {
00207
00208
00209
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
00224
00225
00226
00227
00228
00229
00230
00231 float
00232 BayesHistosToLut::getAPosterioriProb(hint_t object,
00233 unsigned int y,
00234 unsigned int u,
00235 unsigned int v)
00236 {
00237
00238
00239
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
00254
00255
00256
00257
00258 hint_t
00259 BayesHistosToLut::getMostLikelyObject(unsigned int u,
00260 unsigned int v)
00261 {
00262
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
00285
00286
00287
00288
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
00316 void
00317 BayesHistosToLut::calculateLutAllColors()
00318 {
00319
00320
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
00338
00339
00340
00341
00342
00343
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
00353 highest_prob = 0.0;
00354 color_with_highest_prob = H_UNKNOWN;
00355 map<hint_t, Histogram*>::iterator hit;
00356 for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00357
00358 if (numberOfOccurrences[ (hint_t)hit->first ] > 0) {
00359 current_prob = float( hit->second->get_value(u, v, y) ) / float( numberOfOccurrences[ hit->first ] );
00360
00361
00362 if ( current_prob > highest_prob &&
00363 current_prob > min_probability ) {
00364
00365 highest_prob = current_prob;
00366 color_with_highest_prob = hit->first;
00367 }
00368 }
00369 }
00370
00371
00372 lut->set(y_index, u, v, ColorObjectMap::get_instance().get(color_with_highest_prob));
00373 }
00374 }
00375 }
00376
00377 }
00378
00379
00380
00381
00382
00383 void
00384 BayesHistosToLut::calculateLutValues( bool penalty )
00385 {
00386
00387 unsigned int old_undo = 0;
00388
00389 if ( penalty ) {
00390
00391 Histogram *histo_fg = histograms[fg_object];
00392 Histogram *histo_bg = histograms[H_BACKGROUND];
00393
00394 if ( histo_bg->get_num_undos() < 2 ) {
00395
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
00424
00425
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
00448
00449
00450 for (hit = histograms.begin(); hit != histograms.end(); hit++) {
00451 object_probabilities[hit->first] = (float)hit->second->get_sum() / (float)total_count;
00452
00453
00454
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
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533 }
00534
00535
00536
00537
00538 void
00539 BayesHistosToLut::saveLut(char *file)
00540 {
00541 ColormapFile cmf;
00542 cmf.add_colormap(lut);
00543 cmf.write(file);
00544 }
00545
00546
00547
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
00559
00560
00561 void
00562 BayesHistosToLut::setMinProbability( float min_prob )
00563 {
00564 min_probability = min_prob;
00565 }
00566
00567
00568
00569
00570
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
00601
00602
00603 YuvColormap *
00604 BayesHistosToLut::get_colormap()
00605 {
00606 return lut;
00607 }
00608
00609 }