Fawkes API  Fawkes Development Version
zauberstab.cpp
1 
2 /***************************************************************************
3  * zauberstab.cpp - Implementation of class "Zauberstab"
4  * which offers methods for finding
5  * maximal, color-contiguous region
6  * around a seed pixel
7  *
8  * Created: Mon Jul 02 2005
9  * Copyright 2005 Martin Heracles <Martin.Heracles@rwth-aachen.de>
10  * 2005-2008 Tim Niemueller [www.niemueller.de]
11  *
12  ****************************************************************************/
13 
14 /* This program is free software; you can redistribute it and/or modify
15  * it under the terms of the GNU General Public License as published by
16  * the Free Software Foundation; either version 2 of the License, or
17  * (at your option) any later version. A runtime exception applies to
18  * this software (see LICENSE.GPL_WRE file mentioned below for details).
19  *
20  * This program is distributed in the hope that it will be useful,
21  * but WITHOUT ANY WARRANTY; without even the implied warranty of
22  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
23  * GNU Library General Public License for more details.
24  *
25  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
26  */
27 
28 #include <fvutils/color/zauberstab.h>
29 #include <fvutils/color/yuv.h>
30 #include <core/macros.h>
31 
32 #include <cstdlib>
33 #include <iostream>
34 
35 using namespace std;
36 using namespace fawkes;
37 
38 namespace firevision {
39 #if 0 /* just to make Emacs auto-indent happy */
40 }
41 #endif
42 
43 /** @class Zauberstab <fvutils/color/zauberstab.h>
44  * Zaubertab selection utility.
45  */
46 
47 /** Constructor. */
48 ZRegion::ZRegion()
49 {
50  topSliceY = 0;
51  slices = new vector<ZSlice*>();
52  slices->clear();
53 }
54 
55 /** Constructor. */
56 ZRegion::~ZRegion()
57 {
58  for (std::vector<ZSlice*>::iterator it = slices->begin(); it != slices->end(); ++it)
59  {
60  delete (*it);
61  }
62 
63  delete slices;
64 }
65 
66 /** Clears all slices.
67  */
68 void
69 ZRegion::clear()
70 {
71  for (std::vector<ZSlice*>::iterator it = slices->begin(); it != slices->end(); ++it)
72  {
73  delete (*it);
74  }
75 
76  slices->clear();
77 }
78 
79 /** @class Zauberstab <fvutils/color/zauberstab.h>
80  * Zaubertab selection utility.
81  */
82 
83 /** Constructor. */
84 Zauberstab::Zauberstab() {
85  // create empty region
86  region = new ZRegion();
87 
88  buffer = NULL;
89  width = 0;
90  height = 0;
91 
92  /* by default, "Zauberstab" does not allow
93  any deviation from seed color */
94  this->threshold = 0 ;
95 }
96 
97 
98 /** Destructor. */
99 Zauberstab::~Zauberstab() {
100  delete(region);
101 }
102 
103 
104 /** Set threshold.
105  * @param t new threshold
106  */
107 void
108 Zauberstab::setThreshold(unsigned int t) {
109  this->threshold = t;
110 }
111 
112 
113 /** Get threshold.
114  * @return threshold
115  */
116 unsigned int
117 Zauberstab::getThreshold() {
118  return this->threshold;
119 }
120 
121 
122 /** Set buffer to work on.
123  * @param b buffer
124  * @param w width of image
125  * @param h height of buffer
126  */
127 void
128 Zauberstab::setBuffer(unsigned char *b,
129  unsigned int w,
130  unsigned int h) {
131  this->buffer = b;
132  this->width = w;
133  this->height = h;
134 }
135 
136 
137 /** Check if region is empty.
138  * @return true if empty
139  */
140 bool
141 Zauberstab::isEmptyRegion() {
142  return (region->slices->size() == 0);
143 }
144 
145 
146 /** Delete all regions. */
147 void
148 Zauberstab::deleteRegion() {
149  region->clear();
150 }
151 
152 /** Delete region.
153  * @param seedX seed x
154  * @param seedY seed y
155  */
156 void
157 Zauberstab::deleteRegion(unsigned int seedX, unsigned int seedY)
158 {
159  // STEP 1:
160  // find the region
161  ZRegion* region2 = privFindRegion (seedX, seedY);
162 
163  // STEP 2:
164  // now delete the newly found region2 from the original region
165  deleteRegion(region2);
166 
167  delete region2;
168 }
169 
170 
171 /** Delete region.
172  * @param region2 region to delete
173  */
174 void
175 Zauberstab::deleteRegion(ZRegion *region2)
176 {
177  ZSlice* nSlice; //A slice to be deleted
178  ZSlice* oSlice; //A slice currently in the region
179 
180  // delete each slice of region 2 from region
181  while (region2->slices->size())
182  {
183  /* check if current slice from region 2 is at
184  at a height different from all slices at region */
185  nSlice = region2->slices->back();
186  region2->slices->pop_back();
187  int heightOfSlice = nSlice->y;
188 
189  unsigned int i = 0;
190  unsigned int size = region->slices->size();
191 
192  while(i < size) //for all existing slices (but not the newly added)
193  {
194  oSlice = region->slices->at(i);
195  if (oSlice->y == heightOfSlice) //same height check for overlapping
196  {
197  if ((oSlice->leftX >= nSlice->leftX)
198  && (oSlice->leftX < nSlice->rightX))
199  {
200  //The slice to delete starts before the slice to be deleted
201  if (oSlice->rightX > nSlice->rightX) //A part of the region remains
202  {
203  oSlice->leftX = nSlice->rightX;
204  }
205  else //The whole slice dissapears
206  {
207  region->slices->erase(region->slices->begin() + i);
208  size--;
209  delete oSlice;
210 
211  //The index now points to the next element in the region->slices vector
212  continue;
213  }
214  }
215 
216  if ((nSlice->leftX >= oSlice->leftX)
217  && (nSlice->leftX < oSlice->rightX))
218  {
219  //The slice to be deleted starts before the part that should be deleted
220  if (oSlice->rightX <= nSlice->rightX)
221  {//just truncate the old slices
222  oSlice->rightX = nSlice->leftX;
223  }
224  else //split the old spice
225  {
226  ZSlice* newPart = new ZSlice;
227  newPart->rightX = oSlice->rightX;
228  newPart->leftX = nSlice->rightX;
229  newPart->y = heightOfSlice;
230 
231  oSlice->rightX = nSlice->leftX;
232  region->slices->push_back(newPart);
233  }
234  }
235  }
236 
237  i++;
238  }
239  }
240 }
241 
242 /** A private region finder
243  * @param seedX seed x
244  * @param seedY seed y
245  * @return a ZRegion pointer (has to be deleted by the caller)
246  */
247 ZRegion*
248 Zauberstab::privFindRegion (unsigned int seedX, unsigned int seedY)
249 {
250  unsigned char py __unused;
251  unsigned char pu = 0;
252  unsigned char pv = 0;
253 
254  // STEP 1:
255  // first of all find the region around (seedX, seedY)
256  // (this is analogously to method "findRegion")
257  // (could be done more elegantly without the following redundant code)
258 
259  // create empty region
260  ZRegion *region2 = new ZRegion();
261 
262  /* find seed pixel's v-value
263  (consider seed pixel's neighborhood
264  and take average v-value) */
265  unsigned int uSeed = 0;
266  unsigned int vSeed = 0;
267  unsigned int cnt = 0;
268 
269  for (int x = seedX - 2; x <= (int)seedX + 2; ++x) {
270  if (x < 0) continue;
271  if ((unsigned int )x >= width) continue;
272  for (int y = seedY - 2; y <= (int)seedY + 2; ++y) {
273  if (y < 0) continue;
274  if ((unsigned int)y >= height) continue;
275  YUV422_PLANAR_YUV(buffer, width, height, x, y, py, pu, pv);
276  uSeed += pu;
277  vSeed += pv;
278  ++cnt;
279  }
280  }
281 
282  if (cnt)
283  {
284  uSeed = uSeed / cnt;
285  vSeed = vSeed / cnt;
286  }
287  /* initial slice
288  thru seed pixel (seedX, seedY) */
289  ZSlice *tmp = NULL;
290  tmp = findSlice(seedX, seedY, vSeed, uSeed);
291  region2->slices->push_back(tmp);
292 
293  /* NOTE: The following search works fine for
294  objects that are convex (such as ball, goal, ...).
295  For non-convex objects it may miss parts
296  (e.g. for a U-shaped object it can only find right or left half). */
297 
298  // search downwards for more slices
299  tmp = region2->slices->front();
300  int tmpY = ((int)seedY >= (int)(height - 1)) ? height -1 : seedY + 1;
301  // new "seed" pixel has x-coordinate in the middle of initial slice
302  int tmpX = int(float(tmp->leftX + tmp->rightX) / 2.0);
303 
304  YUV422_PLANAR_YUV(buffer, width, height, tmpX, tmpY, py, pu, pv);
305  while (isSimilarUV(pu, uSeed, pv, vSeed)) {
306  tmp = findSlice(tmpX, tmpY, vSeed, uSeed);
307  region2->slices->push_back(tmp);
308  // new "seed" pixel has x-coordinate in the middle of previous slice
309  tmpX = int(float(tmp->leftX + tmp->rightX) / 2.0);
310  tmpY++;
311  if (tmpY >= (int)this->height) {
312  break;
313  } else {
314  YUV422_PLANAR_YUV(buffer, width, height, tmpX, tmpY, py, pu, pv);
315  }
316  }
317 
318  /* search upwards for more slices
319  (start search from initial slice again) */
320  tmp = region2->slices->front();
321  tmpY = (seedY == 0) ? 0 : seedY - 1;
322  // new "seed" pixel has x-coordinate in the middle of initial slice
323  tmpX = int(float(tmp->leftX + tmp->rightX) / 2.0);
324 
325  YUV422_PLANAR_YUV(buffer, width, height, tmpX, tmpY, py, pu, pv);
326  while (isSimilarUV(pu, uSeed, pv, vSeed)) {
327  tmp = findSlice(tmpX, tmpY, vSeed, uSeed);
328  region2->slices->push_back(tmp);
329  // new "seed" pixel has x-coordinate in the middle of previous slice
330  tmpX = int(float(tmp->leftX + tmp->rightX) / 2.0);
331  tmpY--;
332  if (tmpY < 0) {
333  break;
334  } else {
335  YUV422_PLANAR_YUV(buffer, width, height, tmpX, tmpY, py, pu, pv);
336  }
337  }
338 
339  region2->topSliceY = tmpY + 1;
340 
341  for (std::vector<ZSlice*>::iterator it = region2->slices->begin(); it != region2->slices->end(); ++it)
342  {
343  cout << "start x: " << ((*it)->leftX) << " end x: " << ((*it)->rightX) << " y: " << ((*it)->y) << endl;
344  }
345  cout << endl;
346  return region2;
347 }
348 
349 /** Find region.
350  * @param seedX seed x
351  * @param seedY seed y
352  */
353 void
354 Zauberstab::findRegion(unsigned int seedX, unsigned int seedY) {
355  if (buffer == NULL) return;
356  if (width == 0) return;
357  if (height == 0) return;
358 
359  delete region;
360  region = privFindRegion(seedX, seedY);
361 }
362 
363 
364 /** Add region.
365  * @param seedX seed x
366  * @param seedY seed y
367  */
368 void
369 Zauberstab::addRegion(unsigned int seedX, unsigned int seedY)
370 {
371  // STEP 1:
372  // find the region
373  ZRegion* region2 = privFindRegion (seedX, seedY);
374 
375  // STEP 2:
376  // now merge the newly found region2 with the original region
377  addRegion(region2);
378 
379  delete region2;
380 }
381 
382 
383 /** Find specific slice.
384  * @param x x
385  * @param y y
386  * @param vSeed v seed
387  * @return slice
388  */
389 ZSlice*
390 Zauberstab::findSlice(unsigned int x, unsigned int y,
391  unsigned int vSeed, int uSeed)
392 {
393  // slice with single pixel (x, y)
394  ZSlice *slice = new ZSlice;
395  slice->y = y;
396  slice->leftX = x;
397  slice->rightX = x;
398 
399  unsigned char py __unused;
400  unsigned char pu=0;
401  unsigned char pv=0;
402  int tmpX = x + 1;
403 
404  if ((unsigned int)tmpX < width)
405  {
406  YUV422_PLANAR_YUV(buffer, width, height, tmpX, y, py, pu, pv);
407 
408  // search to the right
409  while (uSeed >= 0 ? isSimilarUV(pu, uSeed, pv, vSeed) : isSimilarV(pv, vSeed)) {
410  (slice->rightX)++;
411  tmpX++;
412  if (tmpX >= (int)this->width) {
413  break;
414  } else {
415  YUV422_PLANAR_YUV(buffer, width, height, tmpX, y, py, pu, pv);
416  }
417  };
418  }
419 
420  // search to the left
421  tmpX = x - 1;
422  if (tmpX >= 0)
423  {
424  YUV422_PLANAR_YUV(buffer, width, height, tmpX, y, py, pu, pv);
425  while (uSeed >= 0 ? isSimilarUV(pu, uSeed, pv, vSeed) : isSimilarV(pv, vSeed)) {
426  (slice->leftX)--;
427  tmpX--;
428  if (tmpX < 0) {
429  break;
430  } else {
431  YUV422_PLANAR_YUV(buffer, width, height, tmpX, y, py, pu, pv);
432  }
433  };
434  }
435  /*
436  cout << "Zauberstab: Slice found." << endl;
437  cout << " (left : " << slice->leftX << endl
438  << " right: " << slice->rightX << endl
439  << " at y = " << slice->y << ")" << endl;
440  */
441  return slice;
442 }
443 
444 
445 /** Add region.
446  * @param region2 region to add
447  */
448 void
449 Zauberstab::addRegion(ZRegion *region2)
450 {
451  ZSlice* nSlice; //A slice to be added
452  ZSlice* oSlice; //A slice currently in the region
453 
454  // add each slice from region 2 to region
455  while (region2->slices->size())
456  {
457  /* check if current slice from region 2 is at
458  at a height different from all slices at region */
459  nSlice = region2->slices->back();
460  region2->slices->pop_back();
461  int heightOfSlice = nSlice->y;
462 
463  unsigned int i = 0;
464 
465  while(i < region->slices->size()) //for all existing slices
466  {
467  oSlice = region->slices->at(i);
468  if (oSlice->y == heightOfSlice) //same height check for overlapping
469  {
470  if (((oSlice->leftX >= nSlice->leftX)
471  && (oSlice->leftX <= nSlice->rightX))
472  || ((nSlice->leftX >= oSlice->leftX)
473  && (nSlice->leftX <= oSlice->rightX)))
474  {
475  //They are overlapping so grow the new slice
476  nSlice->leftX = min(nSlice->leftX, oSlice->leftX);
477  nSlice->rightX = max(nSlice->rightX, oSlice->rightX);
478 
479  //and delete the old one
480  region->slices->erase(region->slices->begin() + i);
481  delete oSlice;
482 
483  //The iterator i now points to the next element in the region->slices vector
484  continue;
485  }
486  }
487 
488  ++i;
489  }
490 
491  //By now all overlapping slices have been removed
492  region->slices->push_back(nSlice);
493  }
494 }
495 
496 
497 /** True if two V values are similar.
498  * @param v1 V value 1
499  * @param v2 V value 2
500  * @return true if V values are similar (depends on threshold)
501  */
502 bool
503 Zauberstab::isSimilarV(unsigned int v1,
504  unsigned int v2) {
505  return ( (unsigned int)abs((int)v1 - (int)v2) > this->threshold ) ? false : true;
506 }
507 
508 
509 /** True if two U values are similar.
510  * @param u1 U value 1
511  * @param u2 U value 2
512  * @return true if U values are similar (depends on threshold)
513  */
514 bool
515 Zauberstab::isSimilarU(unsigned int u1,
516  unsigned int u2) {
517  return ( (unsigned int)abs((int)u1 - (int)u2) > this->threshold ) ? false : true;
518 }
519 
520 
521 /** True if two u and V values are similar.
522  * @param u1 U value 1
523  * @param u2 U value 2
524  * @param v1 V value 1
525  * @param v2 V value 2
526  * @return true if V values are similar (depends on threshold)
527  */
528 bool
529 Zauberstab::isSimilarUV(unsigned int u1, unsigned int u2,
530  unsigned int v1, unsigned int v2)
531 {
532  return isSimilarU(u1, u2) && isSimilarV(v1, v2);
533 }
534 
535 
536 /** Get region.
537  * @return region
538  */
539 ZRegion *
540 Zauberstab::getRegion() const
541 {
542  return region;
543 }
544 
545 
546 /** Get selection.
547  * @return selection as a vector of rectangles.
548  */
549 vector< rectangle_t >
550 Zauberstab::getSelection()
551 {
552 
553  vector< rectangle_t > rv;
554  rv.clear();
555 
556  std::vector< ZSlice *>::iterator it;
557  for (it = region->slices->begin(); it != region->slices->end(); it++) {
558  rectangle_t rect;
559  rect.start.x = (*it)->leftX;
560  rect.start.y = (*it)->y;
561  rect.extent.w = (*it)->rightX - (*it)->leftX;
562  rect.extent.h = 1;
563  rv.push_back( rect );
564  }
565 
566  return rv;
567 }
568 
569 } // end namespace firevision
Fawkes library namespace.
unsigned int y
y coordinate
Definition: types.h:36
STL namespace.
unsigned int x
x coordinate
Definition: types.h:35
unsigned int h
height
Definition: types.h:100
Rectangle (unsigned integers)
Definition: types.h:104
upoint_t start
start point
Definition: types.h:105
int y
Y value.
Definition: zauberstab.h:46
std::vector< ZSlice * > * slices
slices
Definition: zauberstab.h:60
unsigned int w
width
Definition: types.h:99
int topSliceY
top slice Y
Definition: zauberstab.h:61
a region is a stack of slices, together with the y-position of the slice at the top ...
Definition: zauberstab.h:58
extent_2d_t extent
extent
Definition: types.h:106
int leftX
left X
Definition: zauberstab.h:44
int rightX
right X
Definition: zauberstab.h:45
a "slice" is a row of consecutive pixels (horizontal)
Definition: zauberstab.h:43