zauberstab.cpp

00001 
00002 /***************************************************************************
00003  *  zauberstab.cpp - Implementation of class "Zauberstab"
00004  *                   which offers methods for finding 
00005  *                   maximal, color-contiguous region
00006  *                   around a seed pixel
00007  *
00008  *  Created: Mon Jul 02 2005
00009  *  Copyright  2005       Martin Heracles  <Martin.Heracles@rwth-aachen.de>
00010  *             2005-2008  Tim Niemueller   [www.niemueller.de]
00011  *
00012  ****************************************************************************/
00013 
00014 /*  This program is free software; you can redistribute it and/or modify
00015  *  it under the terms of the GNU General Public License as published by
00016  *  the Free Software Foundation; either version 2 of the License, or
00017  *  (at your option) any later version. A runtime exception applies to
00018  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00019  *
00020  *  This program is distributed in the hope that it will be useful,
00021  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00022  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023  *  GNU Library General Public License for more details.
00024  *
00025  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00026  */
00027 
00028 #include <fvutils/color/zauberstab.h>
00029 #include <fvutils/color/yuv.h>
00030 #include <core/macros.h>
00031 
00032 #include <cstdlib>
00033 #include <iostream>
00034 
00035 using namespace std;
00036 using namespace fawkes;
00037 
00038 namespace firevision {
00039 #if 0 /* just to make Emacs auto-indent happy */
00040 }
00041 #endif
00042 
00043 /** @class Zauberstab <fvutils/color/zauberstab.h>
00044  * Zaubertab selection utility.
00045  */
00046 
00047 /** Constructor. */
00048 ZRegion::ZRegion()
00049 {
00050   topSliceY = 0;
00051   slices = new vector<ZSlice*>();
00052   slices->clear();
00053 }
00054 
00055 /** Constructor. */
00056 ZRegion::~ZRegion()
00057 {
00058         for (std::vector<ZSlice*>::iterator it = slices->begin(); it != slices->end(); ++it)
00059         {
00060                 delete (*it);
00061         }
00062         
00063         delete slices;
00064 }
00065 
00066 /** Clears all slices.
00067  */
00068 void
00069 ZRegion::clear()
00070 {
00071         for (std::vector<ZSlice*>::iterator it = slices->begin(); it != slices->end(); ++it)
00072         {
00073                 delete (*it);
00074         }
00075 
00076         slices->clear();
00077 }
00078 
00079 /** @class Zauberstab <fvutils/color/zauberstab.h>
00080  * Zaubertab selection utility.
00081  */
00082 
00083 /** Constructor. */
00084 Zauberstab::Zauberstab() {
00085   // create empty region
00086   region = new ZRegion();
00087 
00088   buffer = NULL;
00089   width = 0;
00090   height = 0;
00091 
00092   /* by default, "Zauberstab" does not allow
00093      any deviation from seed color */
00094   this->threshold = 0 ;
00095 }
00096 
00097 
00098 /** Destructor. */
00099 Zauberstab::~Zauberstab() {
00100   delete(region);
00101 }
00102 
00103 
00104 /** Set threshold.
00105  * @param t new threshold
00106  */
00107 void
00108 Zauberstab::setThreshold(unsigned int t) {
00109   this->threshold = t;
00110 }
00111 
00112 
00113 /** Get threshold.
00114  * @return threshold
00115  */
00116 unsigned int
00117 Zauberstab::getThreshold() {
00118   return this->threshold;
00119 }
00120 
00121 
00122 /** Set buffer to work on.
00123  * @param b buffer
00124  * @param w width of image
00125  * @param h height of buffer
00126  */
00127 void 
00128 Zauberstab::setBuffer(unsigned char *b,
00129                       unsigned int w,
00130                       unsigned int h) {
00131   this->buffer = b;
00132   this->width = w;
00133   this->height = h;
00134 }
00135 
00136 
00137 /** Check if region is empty.
00138  * @return true if empty
00139  */
00140 bool
00141 Zauberstab::isEmptyRegion() {
00142   return (region->slices->size() == 0);
00143 }
00144 
00145 
00146 /** Delete all regions. */
00147 void
00148 Zauberstab::deleteRegion() {
00149   region->clear();
00150 }
00151 
00152 /** Delete region.
00153  * @param seedX seed x
00154  * @param seedY seed y
00155  */
00156 void
00157 Zauberstab::deleteRegion(unsigned int seedX, unsigned int seedY)
00158 {
00159   // STEP 1:
00160   // find the region
00161   ZRegion* region2 = privFindRegion (seedX, seedY);
00162 
00163   // STEP 2:
00164   // now delete the newly found region2 from the original region
00165   deleteRegion(region2);
00166 
00167   delete region2;
00168 }
00169 
00170 
00171 /** Delete region.
00172  * @param region2 region to delete
00173  */
00174 void
00175 Zauberstab::deleteRegion(ZRegion *region2)
00176 {
00177   ZSlice* nSlice; //A slice to be deleted
00178   ZSlice* oSlice; //A slice currently in the region
00179 
00180         // delete each slice of region 2 from region
00181         while (region2->slices->size())
00182         {
00183                 /* check if current slice from region 2 is at
00184                          at a height different from all slices at region */
00185     nSlice = region2->slices->back();
00186     region2->slices->pop_back();
00187                 int heightOfSlice = nSlice->y;
00188 
00189                 unsigned int i = 0;
00190                 unsigned int size = region->slices->size();
00191                 
00192                 while(i < size) //for all existing slices (but not the newly added)
00193                 {
00194                         oSlice = region->slices->at(i);
00195                         if (oSlice->y == heightOfSlice) //same height check for overlapping
00196                         {
00197                                 if ((oSlice->leftX >= nSlice->leftX) 
00198                                                 && (oSlice->leftX < nSlice->rightX))
00199                                 {
00200                                         //The slice to delete starts before the slice to be deleted
00201                                         if (oSlice->rightX > nSlice->rightX) //A part of the region remains
00202                                         {
00203                                                 oSlice->leftX = nSlice->rightX;
00204                                         }
00205                                         else //The whole slice dissapears
00206                                         {
00207                                                 region->slices->erase(region->slices->begin() + i);
00208                                                 size--;
00209                                                 delete oSlice;
00210 
00211                                                 //The index now points to the next element in the region->slices vector
00212                                                 continue;
00213                                         }
00214                                 }
00215 
00216                                 if ((nSlice->leftX >= oSlice->leftX) 
00217                                                 && (nSlice->leftX < oSlice->rightX))
00218                                 {
00219                                         //The slice to be deleted starts before the part that should be deleted
00220                                         if (oSlice->rightX <= nSlice->rightX)
00221                                         {//just truncate the old slices
00222                                                 oSlice->rightX = nSlice->leftX;
00223                                         }
00224                                         else //split the old spice
00225                                         {
00226                                                 ZSlice* newPart = new ZSlice;
00227                                                 newPart->rightX = oSlice->rightX;
00228                                                 newPart->leftX = nSlice->rightX;
00229                                                 newPart->y = heightOfSlice;
00230 
00231                                                 oSlice->rightX = nSlice->leftX;
00232                                                 region->slices->push_back(newPart);
00233                                         }
00234                                 }
00235                         }
00236 
00237                         i++;
00238                 }
00239         }
00240 }
00241 
00242 /** A private region finder
00243  * @param seedX seed x
00244  * @param seedY seed y
00245  * @return a ZRegion pointer (has to be deleted by the caller)
00246  */
00247 ZRegion*
00248 Zauberstab::privFindRegion (unsigned int seedX, unsigned int seedY)
00249 {
00250   unsigned char py __unused;
00251   unsigned char pu = 0;
00252   unsigned char pv = 0;
00253 
00254   // STEP 1:
00255   // first of all find the region around (seedX, seedY)
00256   // (this is analogously to method "findRegion")
00257   // (could be done more elegantly without the following redundant code)
00258 
00259   // create empty region
00260   ZRegion *region2 = new ZRegion();
00261 
00262   /* find seed pixel's v-value
00263      (consider seed pixel's neighborhood
00264       and take average v-value) */
00265   unsigned int uSeed = 0;
00266   unsigned int vSeed = 0;
00267   unsigned int cnt = 0;
00268 
00269   for (int x = seedX - 2; x <= (int)seedX + 2; ++x) {
00270     if (x < 0) continue;
00271     if ((unsigned int )x >= width) continue;
00272     for (int y = seedY - 2; y <= (int)seedY + 2; ++y) {
00273       if (y < 0) continue;
00274       if ((unsigned int)y >= height) continue;
00275       YUV422_PLANAR_YUV(buffer, width, height, x, y, py, pu, pv);
00276       uSeed += pu;
00277       vSeed += pv;
00278       ++cnt;
00279     }
00280   }
00281 
00282         if (cnt) 
00283         {
00284                 uSeed = uSeed / cnt;
00285                 vSeed = vSeed / cnt;
00286         }
00287   /* initial slice 
00288      thru seed pixel (seedX, seedY) */
00289   ZSlice *tmp = NULL;
00290   tmp = findSlice(seedX, seedY, vSeed, uSeed);
00291   region2->slices->push_back(tmp);
00292 
00293   /* NOTE: The following search works fine for
00294      objects that are convex (such as ball, goal, ...).
00295      For non-convex objects it may miss parts
00296      (e.g. for a U-shaped object it can only find right or left half). */
00297 
00298   // search downwards for more slices
00299   tmp = region2->slices->front();
00300   int tmpY = ((int)seedY >= (int)(height - 1)) ? height -1 : seedY + 1;
00301   // new "seed" pixel has x-coordinate in the middle of initial slice
00302   int tmpX = int(float(tmp->leftX + tmp->rightX) / 2.0);
00303 
00304   YUV422_PLANAR_YUV(buffer, width, height, tmpX, tmpY, py, pu, pv);
00305   while (isSimilarUV(pu, uSeed, pv, vSeed)) {
00306     tmp = findSlice(tmpX, tmpY, vSeed, uSeed);
00307     region2->slices->push_back(tmp);
00308     // new "seed" pixel has x-coordinate in the middle of previous slice
00309     tmpX = int(float(tmp->leftX + tmp->rightX) / 2.0);
00310     tmpY++;
00311     if (tmpY >= (int)this->height) {
00312       break;
00313     } else {
00314       YUV422_PLANAR_YUV(buffer, width, height, tmpX, tmpY, py, pu, pv);
00315     }
00316   }
00317 
00318   /* search upwards for more slices
00319      (start search from initial slice again) */
00320   tmp = region2->slices->front();
00321   tmpY = (seedY == 0) ? 0 : seedY - 1;
00322   // new "seed" pixel has x-coordinate in the middle of initial slice
00323   tmpX = int(float(tmp->leftX + tmp->rightX) / 2.0);
00324 
00325   YUV422_PLANAR_YUV(buffer, width, height, tmpX, tmpY, py, pu, pv);
00326   while (isSimilarUV(pu, uSeed, pv, vSeed)) {
00327     tmp = findSlice(tmpX, tmpY, vSeed, uSeed);
00328     region2->slices->push_back(tmp);
00329     // new "seed" pixel has x-coordinate in the middle of previous slice
00330     tmpX = int(float(tmp->leftX + tmp->rightX) / 2.0);
00331     tmpY--;
00332     if (tmpY < 0) {
00333       break;
00334     } else {
00335       YUV422_PLANAR_YUV(buffer, width, height, tmpX, tmpY, py, pu, pv);
00336     }
00337   }
00338   
00339   region2->topSliceY = tmpY + 1;
00340 
00341         for (std::vector<ZSlice*>::iterator it = region2->slices->begin(); it != region2->slices->end(); ++it)
00342         {
00343                 cout << "start x: " << ((*it)->leftX) << " end x: " << ((*it)->rightX) << " y: " << ((*it)->y) << endl;
00344         }
00345         cout << endl;
00346   return region2;
00347 }
00348 
00349 /** Find region.
00350  * @param seedX seed x
00351  * @param seedY seed y
00352  */
00353 void
00354 Zauberstab::findRegion(unsigned int seedX, unsigned int seedY) {
00355   if (buffer == NULL) return;
00356   if (width == 0) return;
00357   if (height == 0) return;
00358 
00359   delete region;
00360   region = privFindRegion(seedX, seedY);
00361 }
00362 
00363 
00364 /** Add region.
00365  * @param seedX seed x
00366  * @param seedY seed y
00367  */
00368 void
00369 Zauberstab::addRegion(unsigned int seedX, unsigned int seedY)
00370 {
00371   // STEP 1:
00372   // find the region
00373   ZRegion* region2 = privFindRegion (seedX, seedY);
00374 
00375   // STEP 2:
00376   // now merge the newly found region2 with the original region
00377   addRegion(region2);
00378 
00379   delete region2;
00380 }
00381 
00382 
00383 /** Find specific slice.
00384  * @param x x
00385  * @param y y
00386  * @param vSeed v seed
00387  * @return slice
00388  */
00389 ZSlice*
00390 Zauberstab::findSlice(unsigned int x, unsigned int y,
00391                       unsigned int vSeed, int uSeed)
00392 {
00393   // slice with single pixel (x, y)
00394   ZSlice *slice = new ZSlice;
00395   slice->y = y;
00396   slice->leftX = x;
00397   slice->rightX = x;
00398 
00399   unsigned char py __unused;
00400   unsigned char pu=0;
00401   unsigned char pv=0;
00402   int tmpX = x + 1;
00403 
00404   if ((unsigned int)tmpX < width)
00405   {
00406     YUV422_PLANAR_YUV(buffer, width, height, tmpX, y, py, pu, pv);
00407 
00408     // search to the right
00409                 while (uSeed >= 0 ? isSimilarUV(pu, uSeed, pv, vSeed) : isSimilarV(pv, vSeed)) {
00410       (slice->rightX)++;
00411       tmpX++;
00412       if (tmpX >= (int)this->width) {
00413         break;
00414       } else {
00415         YUV422_PLANAR_YUV(buffer, width, height, tmpX, y, py, pu, pv);
00416       }
00417     };
00418   }
00419 
00420   // search to the left
00421   tmpX = x - 1;
00422   if (tmpX >= 0)
00423   {
00424     YUV422_PLANAR_YUV(buffer, width, height, tmpX, y, py, pu, pv);
00425     while (uSeed >= 0 ? isSimilarUV(pu, uSeed, pv, vSeed) : isSimilarV(pv, vSeed)) {
00426       (slice->leftX)--;
00427       tmpX--;
00428       if (tmpX < 0) {
00429         break;
00430       } else {
00431         YUV422_PLANAR_YUV(buffer, width, height, tmpX, y, py, pu, pv);
00432       }
00433     };
00434   }
00435   /*
00436   cout << "Zauberstab: Slice found." << endl;
00437   cout << "            (left : " << slice->leftX << endl
00438        << "             right: " << slice->rightX << endl
00439        << "             at y = " << slice->y << ")" << endl;
00440   */
00441   return slice;
00442 }
00443 
00444 
00445 /** Add region.
00446  * @param region2 region to add
00447  */
00448 void
00449 Zauberstab::addRegion(ZRegion *region2)
00450 {
00451   ZSlice* nSlice; //A slice to be added
00452   ZSlice* oSlice; //A slice currently in the region
00453 
00454   // add each slice from region 2 to region
00455   while (region2->slices->size())
00456   {
00457     /* check if current slice from region 2 is at
00458        at a height different from all slices at region */
00459     nSlice = region2->slices->back();
00460     region2->slices->pop_back();
00461     int heightOfSlice = nSlice->y;
00462 
00463                 unsigned int i = 0;
00464 
00465                 while(i < region->slices->size()) //for all existing slices
00466                 {
00467                         oSlice = region->slices->at(i);
00468                         if (oSlice->y == heightOfSlice) //same height check for overlapping
00469                         {
00470                                 if (((oSlice->leftX >= nSlice->leftX) 
00471                                      && (oSlice->leftX <= nSlice->rightX))
00472                                     || ((nSlice->leftX >= oSlice->leftX) 
00473                                         && (nSlice->leftX <= oSlice->rightX)))
00474                                 {
00475                                         //They are overlapping so grow the new slice
00476                                         nSlice->leftX  = min(nSlice->leftX,  oSlice->leftX);
00477                                         nSlice->rightX = max(nSlice->rightX, oSlice->rightX);
00478                                         
00479                                         //and delete the old one
00480                                         region->slices->erase(region->slices->begin() + i);
00481                                         delete oSlice;
00482                                         
00483                                         //The iterator i now points to the next element in the region->slices vector
00484                                         continue;
00485                                 }
00486                         }
00487 
00488                         ++i;
00489                 }
00490 
00491                 //By now all overlapping slices have been removed
00492                 region->slices->push_back(nSlice);
00493   }
00494 }
00495 
00496 
00497 /** True if two V values are similar.
00498  * @param v1 V value 1
00499  * @param v2 V value 2
00500  * @return true if V values are similar (depends on threshold)
00501  */
00502 bool 
00503 Zauberstab::isSimilarV(unsigned int v1,
00504                        unsigned int v2) {
00505   return ( (unsigned int)abs((int)v1 - (int)v2) > this->threshold ) ? false : true;
00506 }
00507 
00508 
00509 /** True if two U values are similar.
00510  * @param u1 U value 1
00511  * @param u2 U value 2
00512  * @return true if U values are similar (depends on threshold)
00513  */
00514 bool 
00515 Zauberstab::isSimilarU(unsigned int u1,
00516                        unsigned int u2) {
00517   return ( (unsigned int)abs((int)u1 - (int)u2) > this->threshold ) ? false : true;
00518 }
00519 
00520 
00521 /** True if two u and V values are similar.
00522  * @param u1 U value 1
00523  * @param u2 U value 2
00524  * @param v1 V value 1
00525  * @param v2 V value 2
00526  * @return true if V values are similar (depends on threshold)
00527  */
00528 bool 
00529 Zauberstab::isSimilarUV(unsigned int u1, unsigned int u2,
00530                        unsigned int v1, unsigned int v2)
00531 {
00532   return isSimilarU(u1, u2) && isSimilarV(v1, v2);
00533 }
00534 
00535 
00536 /** Get region.
00537  * @return region
00538  */
00539 ZRegion *
00540 Zauberstab::getRegion() const
00541 {
00542   return region;
00543 }
00544 
00545 
00546 /** Get selection.
00547  * @return selection as a vector of rectangles.
00548  */
00549 vector< rectangle_t >
00550 Zauberstab::getSelection()
00551 {
00552 
00553   vector< rectangle_t > rv;
00554   rv.clear();
00555 
00556   std::vector< ZSlice *>::iterator it;
00557   for (it = region->slices->begin(); it != region->slices->end(); it++) {
00558     rectangle_t rect;
00559     rect.start.x = (*it)->leftX;
00560     rect.start.y = (*it)->y;
00561     rect.extent.w = (*it)->rightX - (*it)->leftX;
00562     rect.extent.h = 1;
00563     rv.push_back( rect );
00564   }
00565 
00566   return rv;
00567 }
00568 
00569 } // end namespace firevision