Fawkes API  Fawkes Development Version
bayes_histos_to_lut.cpp
1 
2 /**************************************************************************
3  * bayes_histos_to_lut.cpp - This file implements a class
4  * that takes color histograms of objects as input,
5  * and, together with probabilities of objects,
6  * generates all the values for a lookup-table
7  * that maps from colors to objects
8  *
9  * Generated: Mon Jun 27 14:16:52 2005
10  * Copyright 2005 Martin Heracles
11  * 2005-2008 Tim Niemueller [www.niemueller.de]
12  * 2007-2008 Daniel Beck
13  *
14  ***************************************************************************/
15 
16 /* This program is free software; you can redistribute it and/or modify
17  * it under the terms of the GNU General Public License as published by
18  * the Free Software Foundation; either version 2 of the License, or
19  * (at your option) any later version. A runtime exception applies to
20  * this software (see LICENSE.GPL_WRE file mentioned below for details).
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU Library General Public License for more details.
26  *
27  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
28  */
29 
30 #include <core/exception.h>
31 #include <fvutils/color/color_object_map.h>
32 #include <fvutils/colormap/bayes/bayes_histos_to_lut.h>
33 #include <fvutils/colormap/cmfile.h>
34 #include <fvutils/colormap/yuvcm.h>
35 #include <fvutils/statistical/histogram.h>
36 
37 #include <cstdio>
38 #include <cstdlib>
39 #include <iostream>
40 #include <string>
41 
42 using namespace std;
43 
44 namespace firevision {
45 
46 /** @class BayesHistosToLut <fvutils/colormap/bayes/bayes_histos_to_lut.h>
47  * LUT generation by using Bayesian method on histograms.
48  * Generates a YUV colormap.
49  * @author Martin Herakles.
50  * @author Tim Niemueller
51  * @author Daniel Beck
52  */
53 
54 /** Constructor.
55  * @param histos histograms
56  * @param d depth of lookup table
57  * @param fg_object type of the foreground object
58  * @param w the width of the lookup table (u-resolution)
59  * @param h the height of the lookup table (v-resolution)
60  */
61 BayesHistosToLut::BayesHistosToLut(std::map<hint_t, Histogram *> &histos,
62  unsigned int d,
63  hint_t fg_object,
64  unsigned int w,
65  unsigned int h)
66 : histograms(histos), fg_object(fg_object)
67 {
68  width = w;
69  height = h;
70  depth = d;
71 
72  // no as shmem segment
73  lut = new YuvColormap(depth, width, height);
74 
75  min_probability = 0.3;
76  min_prob_ball = 0.0;
77  min_prob_green = 0.0;
78  min_prob_yellow = 0.0;
79  min_prob_blue = 0.0;
80  min_prob_white = 0.0;
81  min_prob_black = 0.0;
82 }
83 
84 /** Destructor. */
86 {
87  delete lut;
88 }
89 
90 /** Get name.
91  * @return BayesHistosToLut
92  */
93 string
95 {
96  return string("BayesHistosToLut");
97 }
98 
99 /** Get object probability.
100  * @param object object
101  * @return probability.
102  */
103 float
105 {
106  // These object probabilities should better be read from config file.
107 
108  if (fg_object == H_BALL) {
109  /*
110  switch (object) {
111  case H_BALL:
112  */
113  return 0.2;
114  /*
115  break;
116  case H_BACKGROUND:
117  return 0.8;
118  break;
119  case H_ROBOT:
120  return 0.0;
121  break;
122  case H_FIELD:
123  return 0.0;
124  break;
125  case H_GOAL_BLUE:
126  return 0.0;
127  break;
128  case H_GOAL_YELLOW:
129  return 0.0;
130  break;
131  case H_LINE:
132  return 0.0;
133  break;
134  case H_UNKNOWN:
135  return 0.0;
136  break;
137  default:
138  cout << "(BayesHistosToLut::getObjectProb): Invalid object." << endl;
139  exit(-1);
140  return 0.0f;
141  break;
142  }
143  */
144  } else {
145  if (object_probabilities.find(object) != object_probabilities.end()) {
146  return object_probabilities[object];
147  } else {
148  cout << "returning 0" << endl;
149  return 0.f;
150  }
151  }
152 }
153 
154 /** P(u, v| object).
155  * Get a-priori probability.
156  * @param u YUV U-value
157  * @param v YUV V-value
158  * @param object object.
159  * @return probability
160  */
161 float
162 BayesHistosToLut::getAPrioriProb(unsigned int u, unsigned int v, hint_t object)
163 {
164  unsigned int sum = 0;
165  for (unsigned int y = 0; y < depth; ++y) {
166  sum += histograms[object]->get_value(u, v, y);
167  }
168 
169  return (float(sum) / float(numberOfOccurrences[object]));
170 }
171 
172 /** P(u, v| object).
173  * Get a-priori probability.
174  * @param y YUV Y-value
175  * @param u YUV U-value
176  * @param v YUV V-value
177  * @param object object.
178  * @return probability
179  */
180 float
181 BayesHistosToLut::getAPrioriProb(unsigned int y, unsigned int u, unsigned int v, hint_t object)
182 {
183  return (float(histograms[object]->get_value(u, v, y)) / float(numberOfOccurrences[object]));
184 }
185 
186 /** P(object| u, v).
187  * Get a-posteriori probability.
188  * @param object objcet
189  * @param u YUV U-value
190  * @param v YUV V-value
191  * @return a posteriori probability
192  */
193 float
194 BayesHistosToLut::getAPosterioriProb(hint_t object, unsigned int u, unsigned int v)
195 {
196  /* calculate "nenner" for bayes-formula,
197  i.e. sum up the probabilities P(u, v| object) * P(object)
198  over all objects */
199  float sumOfProbabilities = 0.0;
200  map<hint_t, Histogram *>::iterator hit;
201  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
202  sumOfProbabilities +=
203  (getAPrioriProb(u, v, (hint_t)hit->first) * getObjectProb((hint_t)hit->first));
204  }
205 
206  if (sumOfProbabilities != 0) {
207  return getAPrioriProb(u, v, object) * getObjectProb(object) / sumOfProbabilities;
208  } else
209  return 0;
210 }
211 
212 /** P(object| u, v).
213  * Get a-posteriori probability.
214  * @param object objcet
215  * @param y YUV Y-value
216  * @param u YUV U-value
217  * @param v YUV V-value
218  * @return a posteriori probability
219  */
220 float
221 BayesHistosToLut::getAPosterioriProb(hint_t object, unsigned int y, unsigned int u, unsigned int v)
222 {
223  /* calculate "nenner" for bayes-formula,
224  i.e. sum up the probabilities P(u, v| object) * P(object)
225  over all objects */
226  float sumOfProbabilities = 0.0;
227  map<hint_t, Histogram *>::iterator hit;
228  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
229  sumOfProbabilities +=
230  (getAPrioriProb(y, u, v, (hint_t)hit->first) * getObjectProb((hint_t)hit->first));
231  }
232 
233  if (sumOfProbabilities != 0) {
234  return getAPrioriProb(y, u, v, object) * getObjectProb(object) / sumOfProbabilities;
235  } else
236  return 0;
237 }
238 
239 /** Get most likely object.
240  * @param u YUV U-value
241  * @param v YUV V-value
242  * @return most likely object for this color
243  */
244 hint_t
245 BayesHistosToLut::getMostLikelyObject(unsigned int u, unsigned int v)
246 {
247  // TODO sum over all y-values
248 
249  hint_t mostLikelyObject = H_UNKNOWN;
250  float probOfMostLikelyObject = 0.0;
251  map<hint_t, Histogram *>::iterator hit;
252  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
253  float tmp = getAPosterioriProb((hint_t)hit->first, u, v);
254 
255  if (tmp > probOfMostLikelyObject) {
256  probOfMostLikelyObject = tmp;
257  mostLikelyObject = (hint_t)hit->first;
258  }
259  }
260 
261  if (probOfMostLikelyObject > min_probability) {
262  return mostLikelyObject;
263  } else {
264  return H_UNKNOWN;
265  }
266 }
267 
268 /** Get most likely object.
269  * @param y YUV Y-value
270  * @param u YUV U-value
271  * @param v YUV V-value
272  * @return most likely object for this color
273  */
274 hint_t
275 BayesHistosToLut::getMostLikelyObject(unsigned int y, unsigned int u, unsigned int v)
276 {
277  hint_t mostLikelyObject = H_UNKNOWN;
278  float probOfMostLikelyObject = 0.0;
279  map<hint_t, Histogram *>::iterator hit;
280  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
281  float tmp = getAPosterioriProb((hint_t)hit->first, y, u, v);
282 
283  if (tmp > probOfMostLikelyObject) {
284  probOfMostLikelyObject = tmp;
285  mostLikelyObject = (hint_t)hit->first;
286  }
287  }
288 
289  if (probOfMostLikelyObject > min_probability) {
290  return mostLikelyObject;
291  } else {
292  return H_UNKNOWN;
293  }
294 }
295 
296 /** Calculate all LUT colors. */
297 void
299 {
300  // for each histogram, sum up all of its entries
301  // numberOfOccurrences.resize( histograms.size() );
302  map<hint_t, Histogram *>::iterator hit;
303  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
304  unsigned int total = 0;
305  for (unsigned int v = 0; v < height; ++v) {
306  for (unsigned int u = 0; u < width; ++u) {
307  for (unsigned int y = 0; y < depth; ++y) {
308  unsigned int tmp = ((Histogram *)(hit->second))->get_value(u, v, y);
309  if (tmp > 0)
310  total += tmp;
311  }
312  }
313  }
314  numberOfOccurrences[(hint_t)hit->first] = total;
315  }
316 
317  /*
318  cout << "histo-BALL : " << numberOfOccurrences[0] << " counts." << endl
319  << "histo-GREEN: " << numberOfOccurrences[3] << " counts." << endl
320  << "histo-BLUE : " << numberOfOccurrences[5] << " counts." << endl;
321  */
322 
323  // for each color, mark it (in lut) as the color
324  // that has the highest probability (among all histograms)
325  hint_t color_with_highest_prob;
326  float highest_prob;
327  float current_prob;
328  for (unsigned int y = 0; y < depth; ++y) {
329  unsigned int y_index = y * lut->deepness() / lut->depth();
330  for (unsigned int v = 0; v < height; ++v) {
331  for (unsigned int u = 0; u < width; ++u) {
332  // find most probable color for (u, v)
333  highest_prob = 0.0;
334  color_with_highest_prob =
335  H_UNKNOWN; // ...maybe it is better to have default = H_BACKGROUND...
336  map<hint_t, Histogram *>::iterator hit;
337  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
338  // if current histogram is not empty...
339  if (numberOfOccurrences[(hint_t)hit->first] > 0) {
340  current_prob =
341  float(hit->second->get_value(u, v, y)) / float(numberOfOccurrences[hit->first]);
342  // if current histogram has higher probability for color (u, v),
343  // _and_ is above min_prob-threshold...
344  if (current_prob > highest_prob && current_prob > min_probability) {
345  // ...update color information
346  highest_prob = current_prob;
347  color_with_highest_prob = hit->first;
348  }
349  }
350  }
351 
352  // set lut value for color (u, v) to most probable color
353  lut->set(y_index, u, v, ColorObjectMap::get_instance().get(color_with_highest_prob));
354  }
355  }
356  }
357 }
358 
359 /** Calculate LUT values.
360  * @param penalty if true, non-ball colors are penalized
361  */
362 void
364 {
365  unsigned int old_undo = 0;
366 
367  if (penalty) {
368  // We penalize all values, that have NOT been classified as ball
369  Histogram *histo_fg = histograms[fg_object];
370  Histogram *histo_bg = histograms[H_BACKGROUND];
371 
372  if (histo_bg->get_num_undos() < 2) {
373  // No undo available for us
374  cout << "Histogram::calculateLutValues: There are not enough undos possible for background "
375  "histogram, not penalizing"
376  << endl;
377  } else {
378  unsigned int bg_median = histo_bg->get_median();
379  unsigned int bg_average = histo_bg->get_average();
380  unsigned int bg_val = 0;
381 
382  old_undo = histo_bg->switch_undo(1);
383 
384  cout << "Histogram: Setting low bg vals to median. median=" << bg_median
385  << " avg=" << bg_average << endl;
386 
387  for (unsigned int v = 0; v < height; ++v) {
388  for (unsigned int u = 0; u < width; ++u) {
389  for (unsigned int y = 0; y < depth; ++y) {
390  if (histo_fg->get_value(u, v, y) == 0) {
391  bg_val = histo_bg->get_value(u, v, y);
392  if (bg_val < bg_average) {
393  histo_bg->set_value(u, v, y, bg_average);
394  }
395  }
396  }
397  }
398  }
399  }
400  }
401 
402  /* count for each object
403  how many non-zero values its histogram has in total */
404  // numberOfOccurrences.resize(histograms.size());
405 
406  map<hint_t, Histogram *>::iterator hit;
407  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
408  unsigned int total = 0;
409  for (unsigned int y = 0; y < depth; ++y) {
410  for (unsigned int v = 0; v < height; ++v) {
411  for (unsigned int u = 0; u < width; ++u) {
412  unsigned int tmp = hit->second->get_value(u, v, y);
413  if (tmp > 0)
414  total += tmp;
415  }
416  }
417  }
418  numberOfOccurrences[hit->first] = total;
419  cout << "[" << hit->first << "]: " << numberOfOccurrences[hit->first] << " occurences" << endl;
420  }
421 
422  unsigned int total_count = 0;
423  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
424  total_count += hit->second->get_sum();
425  }
426  // cout << "Total count: " << total_count << endl;
427 
428  // Calculate overall object probabilities
429  for (hit = histograms.begin(); hit != histograms.end(); hit++) {
430  object_probabilities[hit->first] = (float)hit->second->get_sum() / (float)total_count;
431 
432  // cout << "Setting a-priori probability for histogram " << hit->first << " to "
433  // << object_probabilities[hit->first] << endl;
434  }
435 
436  unsigned int count_ball = 0;
437  unsigned int count_field = 0;
438  unsigned int count_line = 0;
439  unsigned int count_robot = 0;
440  unsigned int count_background = 0;
441  unsigned int count_goal = 0;
442  unsigned int count_unknown = 0;
443 
444  lut->reset();
445 
446  for (unsigned int y = 0; y < depth; ++y) {
447  unsigned int y_index = y * lut->deepness() / lut->depth();
448  for (unsigned int u = 0; u < width; ++u) {
449  unsigned int u_index = u * lut->deepness() / lut->width();
450  for (unsigned int v = 0; v < height; ++v) {
451  unsigned int v_index = v * lut->deepness() / lut->height();
452  hint_t mostLikelyObject = getMostLikelyObject(y, u, v);
453 
454  switch (mostLikelyObject) {
455  case H_BALL: count_ball++; break;
456  case H_BACKGROUND: count_background++; break;
457  case H_ROBOT:
458  case H_ROBOT_OPP: count_robot++; break;
459  case H_FIELD: count_field++; break;
460  case H_LINE: count_line++; break;
461  case H_GOAL_YELLOW:
462  case H_GOAL_BLUE: count_goal++; break;
463  case H_UNKNOWN: count_unknown++; break;
464  default:
465  cout << "(BayesHistosToLut::calculateLutValues(): Invalid object." << endl;
466  throw fawkes::Exception("BayesHistosToLut::calculateLutValues(): Invalid object.");
467  }
468  lut->set(y_index, u_index, v_index, ColorObjectMap::get_instance().get(mostLikelyObject));
469  }
470  }
471  }
472 
473  printf("d/w/h: %u/%u/%u ball: %d field: %d line: %d robot: %d goal: %d background: %d "
474  "unknown: %d\n",
475  depth,
476  width,
477  height,
478  count_ball,
479  count_field,
480  count_line,
481  count_robot,
482  count_goal,
483  count_background,
484  count_unknown);
485 
486  if (penalty) {
487  Histogram *histo_bg = histograms[H_BACKGROUND];
488  if (histo_bg->get_num_undos() >= 2) {
489  histo_bg->undo();
490  histo_bg->switch_undo(old_undo);
491  }
492  }
493 
494  /*
495  // for testing: output ball colors
496  cout << " ============" << endl;
497  for (unsigned int v = 0; v < height; v++) {
498  for (unsigned int u = 0; u < width; u++) {
499  if (lut->determine(128, u, v) == BACKGROUND)
500  cout << "lut says that (" << u << ", " << v << ") is background color." << endl;
501  }
502  }
503  cout << "===============" << endl;
504  */
505 }
506 
507 /** Save LUT to file.
508  * @param file file name
509  */
510 void
512 {
513  ColormapFile cmf;
514  cmf.add_colormap(lut);
515  cmf.write(file);
516 }
517 
518 /** Save LUT to file.
519  * @param filename file name
520  */
521 void
522 BayesHistosToLut::save(std::string filename)
523 {
524  ColormapFile cmf;
525  cmf.add_colormap(lut);
526  cmf.write(filename.c_str());
527 }
528 
529 /** Set min probability.
530  * @param min_prob minimum probability
531  */
532 void
534 {
535  min_probability = min_prob;
536 }
537 
538 /** Set min probability for color.
539  * @param min_prob minimum probability
540  * @param hint color hint
541  */
542 void
543 BayesHistosToLut::setMinProbForColor(float min_prob, hint_t hint)
544 {
545  switch (hint) {
546  case H_BALL: min_prob_ball = min_prob; break;
547  case H_FIELD: min_prob_green = min_prob; break;
548  case H_GOAL_YELLOW: min_prob_yellow = min_prob; break;
549  case H_GOAL_BLUE: min_prob_blue = min_prob; break;
550  case H_LINE: min_prob_white = min_prob; break;
551  case H_ROBOT: min_prob_black = min_prob; break;
552  default:
553  /**/
554  break;
555  }
556 }
557 
558 /** Get generated color model.
559  * @return generated color model
560  */
561 YuvColormap *
563 {
564  return lut;
565 }
566 
567 } // end namespace firevision
firevision::BayesHistosToLut::calculateLutValues
void calculateLutValues(bool penalty=false)
Calculate LUT values.
Definition: bayes_histos_to_lut.cpp:363
firevision::YuvColormap::depth
virtual unsigned int depth() const
Get depth of colormap.
Definition: yuvcm.cpp:336
firevision::BayesHistosToLut::saveLut
void saveLut(char *file)
Save LUT to file.
Definition: bayes_histos_to_lut.cpp:511
firevision::ColorObjectMap::get_instance
static const ColorObjectMap & get_instance()
ColorObjectMap getter.
Definition: color_object_map.h:40
firevision::BayesHistosToLut::calculateLutAllColors
void calculateLutAllColors()
Calculate all LUT colors.
Definition: bayes_histos_to_lut.cpp:298
firevision::YuvColormap::reset
virtual void reset()
Reset colormap.
Definition: yuvcm.cpp:200
firevision::Histogram::get_num_undos
unsigned int get_num_undos()
Get number of undos.
Definition: histogram.cpp:468
firevision::YuvColormap
YUV Colormap.
Definition: yuvcm.h:36
firevision::BayesHistosToLut::save
void save(std::string filename)
Save LUT to file.
Definition: bayes_histos_to_lut.cpp:522
firevision::Histogram::get_average
unsigned int get_average()
Get average of all values.
Definition: histogram.cpp:503
firevision::Histogram::set_value
void set_value(unsigned int x, unsigned int y, unsigned int value)
Set value in histogram.
Definition: histogram.cpp:233
firevision::Histogram::get_median
unsigned int get_median()
Get median of all values.
Definition: histogram.cpp:477
firevision::BayesHistosToLut::getMostLikelyObject
hint_t getMostLikelyObject(unsigned int u, unsigned int v)
Get most likely object.
Definition: bayes_histos_to_lut.cpp:245
firevision::YuvColormap::width
virtual unsigned int width() const
Get width of colormap.
Definition: yuvcm.cpp:324
firevision::BayesHistosToLut::getAPosterioriProb
float getAPosterioriProb(hint_t object, unsigned int u, unsigned int v)
P(object| u, v).
Definition: bayes_histos_to_lut.cpp:194
firevision::FireVisionDataFile::write
virtual void write(const char *file_name)
Write file.
Definition: fvfile.cpp:243
firevision::BayesHistosToLut::getAPrioriProb
float getAPrioriProb(unsigned int u, unsigned int v, hint_t object)
P(u, v| object).
Definition: bayes_histos_to_lut.cpp:162
firevision::BayesHistosToLut::get_colormap
YuvColormap * get_colormap()
Get generated color model.
Definition: bayes_histos_to_lut.cpp:562
firevision::BayesHistosToLut::setMinProbability
void setMinProbability(float min_prob)
Set min probability.
Definition: bayes_histos_to_lut.cpp:533
firevision::Histogram::switch_undo
unsigned int switch_undo(unsigned int undo_id)
Switch undo to another undo buffer.
Definition: histogram.cpp:451
firevision::BayesHistosToLut::setMinProbForColor
void setMinProbForColor(float min_prob, hint_t hint)
Set min probability for color.
Definition: bayes_histos_to_lut.cpp:543
firevision::ColormapFile::add_colormap
void add_colormap(Colormap *colormap)
Add colormap.
Definition: cmfile.cpp:89
firevision::YuvColormap::height
virtual unsigned int height() const
Get height of colormap.
Definition: yuvcm.cpp:330
firevision::Histogram
Histogram.
Definition: histogram.h:36
firevision::BayesHistosToLut::getObjectProb
float getObjectProb(hint_t object)
Get object probability.
Definition: bayes_histos_to_lut.cpp:104
firevision::Histogram::get_value
unsigned int get_value(unsigned int x, unsigned int y)
Get value from histogram.
Definition: histogram.cpp:210
firevision::Histogram::undo
void undo()
Undo.
Definition: histogram.cpp:432
firevision::ColormapFile
Colormap file.
Definition: cmfile.h:55
firevision::YuvColormap::set
virtual void set(unsigned int y, unsigned int u, unsigned int v, color_t c)
Set color class for given YUV value.
Definition: yuvcm.cpp:194
firevision::BayesHistosToLut::getName
std::string getName()
Get name.
Definition: bayes_histos_to_lut.cpp:94
firevision::BayesHistosToLut::~BayesHistosToLut
~BayesHistosToLut()
Destructor.
Definition: bayes_histos_to_lut.cpp:85
firevision::YuvColormap::deepness
virtual unsigned int deepness() const
Get deepness of colormap.
Definition: yuvcm.cpp:342
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36