findleds.h

gehe zur Dokumentation dieser Datei

#ifndef FINDLEDSMODULE_H
#define FINDLEDSMODULE_H

#include "../opencvext.h"
#include "imagesource.h"
#include <iostream>
#include <list>
#include "../microeva.h"
#include "../microadam.h"
#include "../image.h"
#include "correlation.h"
#include "hsvrange.h"

using namespace std;

namespace seemicro
{

const uchar MAXMARKS = 100;

struct FindLEDSModule : public ImageSource
{
  ImageSource& src;

  Micro& micro;
  MicroEva *eva;

  CorrelationModule &corr;

  HSVRange& range;

  myrect ROI;

  int w, h;// Abkürzungen für img->width, img->height

  mypoint markierungen[MAXMARKS];

  int anzahl[MAXMARKS];

  float abstand[MAXMARKS];

  unsigned short int markiert;

  float gamma[MAXMARKS];

  float gamma0[MAXMARKS];

  float beta[MAXMARKS];

  char gemerkt[MAXMARKS], gewaehlt[MAXMARKS], armnummer[MAXMARKS];

  FindLEDSModule(ImageSource& Asrc, Micro& Am, CorrelationModule& Ac,
    HSVRange& Ar) :
    src(Asrc), micro(Am), corr(Ac), range(Ar)
  {
    eva = dynamic_cast<MicroEva*>(&micro);
    if(src.img->nChannels != 3)
    {
      cerr << "FindLEDSModule(): Eingabebild muß RGB-Bild sein!\n";
      throw bad_alloc();
    }
    img = cvCreateImage(cvSize(src.img->width, src.img->height), IPL_DEPTH_8U, 1);
    w = img->width;
    h = img->height;
  }

  ~FindLEDSModule()
  {
    cvReleaseImage(&img);
  }

  inline bool remarkable(const int x, const int y, const HSVRange& range)
  {
    int pix = getPixel8u_3(src.img, x, y);
    unsigned short pH, pS, pV;
    rgb2hsv(pix, &pH, &pS, &pV);

    if(pH>range.hmax) return false;
    if(pH<range.hmin) return false;
    if(pS>range.smax) return false;
    if(pS<range.smin) return false;
    if(pV>range.vmax) return false;
    if(pV<range.vmin) return false;
    return true;
  }

  inline void recursive_mark(const int x, const int y, const uchar m, const HSVRange& range)
  {
    putPixel8u_1(img, x, y, m);
    markierungen[m-1].p.x += x;
    markierungen[m-1].p.y += y;
    anzahl[m-1]++;

    if(x+1<w && getPixel8u_1(img, x+1, y)==0 && remarkable(x+1, y, range)) recursive_mark(x+1, y, m, range);
    if(y+1<h)
    {
      if(          getPixel8u_1(img, x  , y+1)==0 && remarkable(x, y+1, range)) recursive_mark(x, y+1, m, range);
      if(x+1<w  && getPixel8u_1(img, x+1, y+1)==0 && remarkable(x+1, y+1, range)) recursive_mark(x+1, y+1, m, range);
      if(x-1>=0 && getPixel8u_1(img, x-1, y+1)==0 && remarkable(x-1, y+1, range)) recursive_mark(x-1, y+1, m, range);
    }
  }

  inline void FloodFill(IplImage *img, myrect& ROI, unsigned short int& markiert, const HSVRange& range)
  {
    cvZero(img);// alte Markierungen löschen
    markiert = 0;

    for(int x=ROI.r.x; x<ROI.r.x+ROI.r.width; x++)
      for(int y=ROI.r.y; y<ROI.r.y+ROI.r.height; y++)
      {
        if(!remarkable(x, y, range)) continue;
        unsigned short int m = getPixel8u_1(img, x, y);
        if(m!=0) continue;

        // Womit ist rechts oder drunter markiert?
        if(x+1<w) m = getPixel8u_1(img, x+1, y);
        if(m==0 && y+1<h)
        {
                             m = getPixel8u_1(img, x  , y+1);
          if(m==0 && x+1<w)  m = getPixel8u_1(img, x+1, y+1);
          if(m==0 && x-1>=0) m = getPixel8u_1(img, x-1, y+1);
        }

        if(m==0)
        {
          // Neue Markierung vergeben
          if(markiert<MAXMARKS-1) markiert++;
          m = markiert;
          markierungen[m-1] = mypoint(0, 0);
          anzahl[m-1] = 0;
        }
        recursive_mark(x, y, m, range);
      } // for()

    if(DEBUGLEVEL>=3) cerr << "Vergebene Markierungen: " << markiert << endl;

    // Schwerpunktbildung: sum(x|y)/anzahl der pixel
    for(int i=0; i<markiert; i++)
    {
      markierungen[i].p.x /= anzahl[i];
      markierungen[i].p.y /= anzahl[i];
      if(DEBUGLEVEL>=3)
        cerr << "#" << i << " (" << markierungen[i].p.x << " " <<
          markierungen[i].p.y << ") " << anzahl[i] << " pix\n";
    }
  }

  void FloodFillMarks2Colors(IplImage *out)
  {
    for(int x=0; x<img->width; x++)
      for(int y=0; y<img->height; y++)
      {
        uchar m = getPixel8u_1(img, x, y);
        int n = 0;
        switch(m)
        {
          case 0: break;
          case 1: n = CV_RGB(255, 0, 0);break;// rot
          case 2: n = CV_RGB(0, 255, 0);break;// grün
          case 3: n = CV_RGB(0, 0, 255);break;// blau
          case 4: n = CV_RGB(255, 255, 0);break;// gelb
          case 5: n = CV_RGB(255, 0, 255);break;// magenta
          case 6: n = CV_RGB(0, 255, 255);break;// cyan
          case 7: n = CV_RGB(255, 0, 128);break;// pink
          case 8: n = CV_RGB(255, 128, 0);break;// orange
          case 9: n = CV_RGB(128, 0, 255);break;// violett
          case 10: n = CV_RGB(0, 128, 255);break;// hellblau
          default:
                  {
                    static int been_here=0;
                    if(!been_here)
                    {
                      been_here++;
                      cerr << "No more colors defined for marks>=" << int(m) << endl;
                    }
                  }
        } // switch
        putPixel8u_3(out, x, y, n);
      } // for y
  }

  virtual void processFrame(myrect *AROI)
  {
    if(!paramChanged && !src.outputChanged &&
       !corr.outputChanged && !range.outputChanged) return;
    if(AROI) ROI = *AROI;
    else ROI = myrect(0, 0, w, h);

    FloodFill(img, ROI, markiert, range);

    // -------------------------------------------------------------------------------
    // Erkennung auf Pixelebene abgeschlossen
    // -------------------------------------------------------------------------------

    // sehr nahe Schwerpunkte (<10 pixel, mindestabstand) zusammenwerfen
    int i,j;
    for(i=0; i<markiert; i++)
    {
      mypoint& s1 = markierungen[i];
      bool zusammengelegt;
      // Folgende while-Konstruktion existiert aufgrund fehlerhafter
      // Optimierung im gcc 3.2 (SuSE Prerelease) (Endlosschleife)
      // Bei "restart iteration of while" stand einst
      //        // "restart iteration of outer loop
      //        i--;break;
      do {
        zusammengelegt=false;
        for(j=i+1; j<markiert; j++)
        {
          if(anzahl[j] == 0) continue;// als gelöscht gekennzeichneter Schwerpunkt
          mypoint& s2 = markierungen[j];
          if(abs(s1-s2)<10)
          {
            if(DEBUGLEVEL>=3)
              cerr << "Lege #" << i << " und #" << j << " zusammen.\n";
            // neuen Schwerpunkt, gewichtet nach Pixelzahlen
            int alle = anzahl[i] + anzahl[j];
            float gi = anzahl[i]/(float)alle;
            float gj = anzahl[j]/(float)alle;

            markierungen[i].p.x = int(markierungen[i].p.x*gi + markierungen[j].p.x*gj);
            markierungen[i].p.y = int(markierungen[i].p.y*gi + markierungen[j].p.y*gj);
            anzahl[i] = alle;

            anzahl[j] = 0;
            //restart iteration of while
            zusammengelegt=true;
            break;
          } // if
        } // for
      } while(zusammengelegt==true);
    }

    // ungefähre Anzahl der Pixel berechnen, die eine LED im Bild hat
    // Gemessen wurde:
    //
    //  Modell | Radradius in Pixeln | Anzahl der Pixel pro LED
    //  -------+---------------------+-------------------------
    //  Adam   | 104                 | 55
    //         |  71                 | 2,5,6,11,14
    //  Eva    |  68                 | 1-10
    //         |                     | 2-20
    //         |                     | 3-15
    // Ansatz: Radradius ~ LED-Fläche = PI*radius_LED^2
    //         radius_LED = radradius*c (c wurde aus obiger Tabelle abgeleitet: 0.045 < c < 0.068)
    int pixPerLED = int(sqr(micro.radius*0.07));

    int MaxPixPerLED = int(pixPerLED*1.4);
    int MinPixPerLED = int(pixPerLED*0.1);
    if(DEBUGLEVEL>=3) cerr << "Pixel pro LED: ideal=" << pixPerLED
      << " min=" << MinPixPerLED
      << " max=" << MaxPixPerLED << endl;

    // Schwerpunkte mit >MaxPixPerLED (früher war es fest >50) oder <MinPixPerLED Pixeln rauswerfen
    for(int i=0; i<markiert; i++)
    {
      if(anzahl[i] > MaxPixPerLED) anzahl[i] = 0;
      if(anzahl[i] < MinPixPerLED) anzahl[i] = 0;
    }

    // Abstand zum Radmittelpunkt berechnen (r)
    if(!micro.mitteValid && DEBUGLEVEL>=1) cerr << "Radmitte unbekannt :(\n";
    if(!micro.radiusValid && DEBUGLEVEL>=1) cerr << "Radius unbekannt :(\n";

    const float LED_MINABSTAND = 0.1;
    const float LED_MAXABSTAND = 0.8;
    for(int i=0; i<markiert; i++)
    {
      if(anzahl[i] == 0) continue;// als gelöscht gekennzeichneter Schwerpunkt
      mypoint& s1 = markierungen[i];
      s1 = s1 - micro.mitte;
      abstand[i] = abs(s1)/micro.radius;

      // Schwerpunkte mit zu großen/zu kleinem Abstand vom Mittelpunkt
      // rauswerfen, da sie sich nur auf einem Ring darum befinden können
      if(abstand[i] > LED_MAXABSTAND) anzahl[i] = 0;
      if(abstand[i] < LED_MINABSTAND) anzahl[i] = 0;
      //cerr << "Abstand zur Radmitte #" << i << ": " << abstand[i] << endl;
    }

    // Schwerpunkte mit der Anzahl der Pixel gewichten
    // zuviele = schlecht, zuwenige auch
    // auch müssen die Schwerpkt. einen Mindestabstand voneinander
    // aufweisen
    // TODO

    // Gamma, Winkel zwischen positiver x-Achse und LED-Position,
    // relativ zur Radmitte berechnen
    // Inverse Funktion von:
    //  x = cos(gamma)*r
    //  y = sin(gamma)*r,
    // sowie Berechnung von beta und gamma0, welches der LED-Position bei am Rand anliegendem
    // Arm entspricht: gamma = gamma0 + beta
    // beta ~ 1/r
    for(int i=0; i<markiert; i++)
    {
      if(anzahl[i] == 0) continue;// als gelöscht gekennzeichneter Schwerpunkt
      gamma[i] = acos((markierungen[i].p.x) / (abstand[i]*micro.radius));
      // da 0 <= acos() <= M_PI:
      if(markierungen[i].p.y > 0) // < was for eva
      gamma[i] = 2*M_PI - gamma[i];

      // 0 <= beta <= 1
      // wobei 0 bedeutet: Arm liegt am Rad außen an
      // und 1 bedeutet: Arm ist maximal nach innen zur Radmitte bewegt
      beta[i] = 1-(abstand[i]-LED_MINABSTAND)/(LED_MAXABSTAND-LED_MINABSTAND);

      // Als Eva-Armansatzpunkt wird die LED-Position bei am Rad außen
      // anliegendem Arm betrachtet
      // Ist der Arm maximal ausgefahren, also beta==1, so
      // erscheint die LED schätzungsweise um 10 Grad im Gegenurzeigersinn
      // vom Armansatzpunkt bei beta==0
      gamma0[i] = gamma[i] + beta[i]*20/180*M_PI;
      //cerr << "#" << i << " gamma=" << gamma[i]*180/M_PI << "° gamma0="
      //  << gamma0[i]*180/M_PI << "° beta=" << int(beta[i]*100) << "%\n";
    }

    if(eva)
    {
      // -------------------------------------------------------------------------------
      // Erkennung der LED von micro.Eva
      // -------------------------------------------------------------------------------

      // Die Markierungen die auswählen, die am besten ins 72°-Raster passen
      // per Fehlerminimierung
      for(int z=0; z<markiert; z++) gewaehlt[z] = 0;
      float fehler1min = 1e6;

      for(int y=0; y<markiert; y++)
      {
        if(anzahl[y] == 0) continue;

        // jeden Schwerpunkt einmal als Ausgangspunkt merken
        for(int z=0; z<markiert; z++)
        {
          gemerkt[z] = 0;
        }
        gemerkt[y] = 1;// das ist Arm #1

        float fehler1temp = 0;
        for(int i=1; i<5; i++) // je ein gamma0 Arm #2-5 zuordnen
        {
          // wo wäre der nächste Arm zu finden?
          float igamma0 = gamma0[y] + i*72./360.*2.*M_PI;
          if(igamma0 > 2*M_PI) igamma0 -= 2.*M_PI;
          float fehler2min = 50./360.*2.*M_PI;
          //    cerr << "fehler2min=" << fehler2min << endl;
          int minx = -1;
          for(int x=0; x<markiert; x++) // am besten passenden Schwerpunkt finden
          {
            if(anzahl[x] == 0) continue;
            if(gemerkt[x]) continue;
            float adiff =  anglediff(igamma0, gamma0[x]);
            if(adiff<fehler2min)
            {
              //cerr << "igamma0=" << igamma0 << " gamma0[x]=" << gamma0[x]
              //  << " fehler2min=" << adiff << endl;
              fehler2min = adiff;
              minx = x;
            }
          }
          if(minx==-1)
            // kein passender Schwerpunkt gefunden
            fehler1temp += 100;
          else
          {
            fehler1temp += fehler2min;
            gemerkt[minx] = i+1;// das ist Arm #i+1
          }
        }
        if(fehler1temp<fehler1min)
        {
          fehler1min = fehler1temp;
          //cerr << " fehler1min=" << fehler1min << endl;
          memcpy(gewaehlt, gemerkt, sizeof(gewaehlt));
        }
      }
      // in gewaehlt[] stehen nun die Indizes der max. 5 Schwerpunkte,
      // die am besten auf Armpositionen passen

      // alpha berechnen
      // finde das i, für das min(gamma0[i], alpha)
      // auch falls !alphaValid, kann Zuordnung so erfolgen
      // da dann egal ist, welches i gewinnt
      float fehler3=10;
      int alphai = -1;
      for(int i=0; i<markiert; i++)
      {
        if(!gewaehlt[i]) continue;
        float adiff = anglediff(micro.alpha, gamma0[i]);
        if(fehler3 > adiff)
        {
          alphai = i;
          fehler3 = adiff;
        }
      }
      if(fehler3<10)
      {
        micro.alpha = gamma[alphai];
        micro.alphaValid = true;
      }

      if(alphai==-1)
      {
        if(DEBUGLEVEL>=1)
          cerr << "Kein Schwerpunkt gefunden -> kein alpha.\n";
      }
      else
      {
        // Zuordnen der Arm# in gewaehlt[]
        // zu den richtigen beta[]s
        // Es ist eine Rotation der in gewaehlt[] gespeicherten Indizes
        // um ga Positionen nach links (bzw. 5-ga nach rechts), z.B. sei ga=3
        // gewaehlt[]:  1 2 3 4 5
        // armnummer[]: 4 5 1 2 3
        // alphai ist der "neue" Arm#1
        char ga = gewaehlt[alphai];// alte Arnmummer, die nun Arm #1 wird
        eva->beta[0] = beta[alphai];
        armnummer[alphai] = 1;
        //cerr << "alphai=" << alphai << " alt=" << int(ga) << " neu: 1!" << endl;
        for(int i=0; i<markiert; i++)
        {
          char gi = gewaehlt[i];// alte Armnummer
          if(gi<1)
          {
            armnummer[i] = 0;
            continue;
          }
          if(gi==ga) continue;// schon vor der Schleife behandelt
          int an = 1+((gi+(5-ga)) % 5);
          eva->beta[an-1] = beta[i];
          armnummer[i] = an;
          //cerr << " i=" << i << " alt=" << int(gi) << " neu=" << an << endl;
        }
        //cerr << endl;
      }
    }
    else
    // -------------------------------------------------------------------------------
    // Erkennung der LED von micro.Adam
    // -------------------------------------------------------------------------------
    {
      MicroAdam *adam = dynamic_cast<MicroAdam*>(&micro);
      if(adam)
      {
        // alpha += 180°, wenn gamma[LED] nahe alpha+beta liegt

        int ledIndex=-1;
        for(int i=0; i<markiert; i++)
        {
          // gelöschter Schwerpunkt? weiter!
          if(anzahl[i] == 0) continue;
          // Schwerpunkt zu nah am Mittelpunkt? Kann nicht sein!
          if(abstand[i]<0.55)
          {
            anzahl[i]=0; continue;
          }
          float d = anglediff(gamma[i], micro.alpha+adam->beta);
          if(d > 10./180.*M_PI) continue;

          // if more than 1 LED is found, situation
          // is ambiguous and we don't try to correct alpha
          if(ledIndex!=-1)
          {
            ledIndex=-1;
            break;// leave for()
          }
          ledIndex=i;
          //cerr << "Remembering Index: " << ledIndex;
        }
        if(ledIndex!=-1)
        {
          if(anglediff(gamma[ledIndex], micro.alpha+adam->beta)
              < 10./180.*M_PI)
          {
            if(DEBUGLEVEL>=2)
              cerr << "Switching alpha by 180°: gamma[]=" << gamma[ledIndex]
                << " alpha=" << micro.alpha << "\n";
            micro.alpha += M_PI;
          }
        }
      }
    }

    if(DEBUGLEVEL>=3) cerr << "<-FindLEDS\n";
    paramChanged = false;
    range.outputChanged = false;
    outputChanged = true;
  }

  virtual void processKeyFrame(void)
  {
    if(DEBUGLEVEL>=3) cerr << "FindLEDSModule::processKeyFrame()\n";
    processFrame(NULL);
  }

};

}

#endif // FINDLEDSMODULE_H

---