Modules/ImageProcessor/SlamImageProcessor/SlamFlagLocator.cpp

Go to the documentation of this file.

#include "SlamFlagLocator.h"



SlamFlagLocator::SlamFlagLocator()
{
  for (int c=0;c<SlamFlagLocator::NewFlagContainer::NO_OF_CHARACTERIZATIONS;c++)
  {
    cluster[c] = new long*[xSize];
    for (int i=0;i<xSize;i++)
    {
      cluster[c][i] = new long[ySize];
      for (int k=0;k<ySize;k++)
        cluster[c][i][k] = 0;
    }
  }
  updateFinished = false;
  container = new NewFlagContainer();
  averageParam = 1.3;
  maxParam = 0.6;
}

SlamFlagLocator::~SlamFlagLocator()
{
  for(int c=0;c<SlamFlagLocator::NewFlagContainer::NO_OF_CHARACTERIZATIONS;c++)
  {
    for(int i=0;i<xSize;i++)
      delete[] cluster[c][i];
    delete[] cluster[c];
  }

  delete container;
}

void SlamFlagLocator::addFlagDirection(const Pose2D& robotPose, double bearing, int characterization)
{
  if (updateFinished)
    return;

  CameraInfo ci = CameraInfo();
  ci.resolutionHeight = ySize;
  ci.resolutionWidth = xSize;
  Vector2<int> scaledPos = Vector2<int>((int)(robotPose.translation.x/100 + xSize/2),(int)(robotPose.translation.y/100 + ySize/2));
  Vector2<int> pixel = scaledPos;
  BresenhamLineScan lineScanner(scaledPos,bearing,ci);
  for (int i=0;i<lineScanner.numberOfPixels;i++)
  {
    lineScanner.getNext(pixel);
    pixel.x = pixel.x < 0 ? 0 : pixel.x;
    pixel.y = pixel.y < 0 ? 0 : pixel.y;
    pixel.x = pixel.x > 66 ? 66 : pixel.x;
    pixel.y = pixel.y > 46 ? 46 : pixel.y;

    // update border only
    if (pixel.x>63 ||pixel.x < 3 || pixel.y>43 || pixel.y<3)
    {
      cluster[characterization][pixel.x][pixel.y]++;
    }

  }
}

void SlamFlagLocator::draw()
{
  NDECLARE_DEBUGDRAWING("newFlags","DrawingOnField","new flags");
  int x,y;
  for(int i=0;i<66;i++)
    for(int k=0;k<46;k++)
    {
      x = i*100;
      y = k*100;
      x -= 3300;
      y -= 2300;
      x *= 1;
      y *= 1;
      LINE(selfLocatorField,6000,0,6000,4000,20,Drawings::ps_solid,Drawings::red );
      if (cluster[0][i][k]>50){
        NCIRCLE("newFlags",x,y,20,7,Drawings::ps_solid,Drawings::black);
        CIRCLE(selfLocatorField,x,y,20,7,Drawings::ps_solid,Drawings::black);
      } else if (cluster[0][i][k]>20){
        NCIRCLE("newFlags",x,y,20,7,Drawings::ps_solid,Drawings::red);
        CIRCLE(selfLocatorField,x,y,20,7,Drawings::ps_solid,Drawings::red);
      }  else if (cluster[0][i][k]>0){
        NCIRCLE("newFlags",x,y,20,7,Drawings::ps_solid,Drawings::yellow);
        CIRCLE(selfLocatorField,x,y,20,7,Drawings::ps_solid,Drawings::yellow);
      }
    }

    Vector2<int>* flags;
    int stored;
    container->getFlagsForCharacterization(&flags,stored,0);
    for(int i=0;i<stored;i++)
    {
      OUTPUT(idText,text,"slamsl: printing flag no "+i);
      CIRCLE(selfLocatorField,flags[i].x,flags[i].y,40,20,Drawings::ps_solid,Drawings::blue);
    }
}

void SlamFlagLocator::finishUpdate()
{
  OUTPUT(idText,text,"slamflaglocator: update phase ended");
  updateFinished = true;
  calcFlags();
}

void SlamFlagLocator::calcFlags()
  {
    if (!updateFinished)
      return;

    double avg = 0.0;
    double bestValues[SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS];
    Vector2<int> best[SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS];


    for (int c=0;c<SlamFlagLocator::NewFlagContainer::NO_OF_CHARACTERIZATIONS;c++)
    {

      // store the n best clusters
      for(int i=0;i<SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;i++)
      {
        bestValues[i] = 0.0;
      }

      Vector2<int> testPoint;

      for(int y = 0; y < ySize - 2; y += 2)
        for(int x = 0; x < xSize - 2; x +=2)
        {
          long dummy = cluster[c][x][y] +
                       cluster[c][x+1][y] +
                       cluster[c][x+2][y] + 
                       cluster[c][x][y+1] +
                       cluster[c][x+1][y+1] +
                       cluster[c][x+2][y+1] +
                       cluster[c][x][y+2] +
                       cluster[c][x+1][y+2] +
                       cluster[c][x+2][y+2];

          avg = ((double)dummy)/9.0;
          
          for(int k=0;k<SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;k++)
          {
            testPoint.x = x+1;
            testPoint.y = y+1;

            if((best[k]-testPoint).abs()<4){
              if(avg > bestValues[k]){
                bestValues[k] = avg;
                best[k].x = x+1;
                best[k].y = y+1;
              }
              break;
            }

            if(avg > bestValues[k])
            {
              for (int i=SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS-1;i>k;i--)
              {
                best[i] = best[i-1];
                bestValues[i] = bestValues[i-1];
              }

              bestValues[k] = avg;
              best[k].x = x+1;
              best[k].y = y+1;
            }        
          }
        }

        double max = 0.0;
        double avg2 = 0.0;
        // calculate the best flags out of this buffer
        for(int i=0;i<SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;i++)
        {
          max = bestValues[i] > max ? bestValues[i] : max;
          avg2 += bestValues[i];
        }
        avg2 /= SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;

        for(int i=0;i<SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;i++)
        {
          if ((bestValues[i]>=averageParam*avg2 || bestValues[i]>=maxParam*max) && bestValues[i]>0)
          {
            Vector2<int> bestOnField(best[i]);            
            bestOnField.x *= 100;
            bestOnField.y *= 100;
            bestOnField.x -= 3500;
            bestOnField.y -= 2500;
            container->addFlag(bestOnField,c);
          }

        }
    }
  }
/*
  void getClosestFlagBearing(double& targetBearing, const Pose2D& robotPose, double measuredBearing);
  bool updateFinished;
  */

---