Modules/SpecialVision/ChallengeSpecialVision.cpp

Go to the documentation of this file.

/**
* @file ChallengeSpecialVision.cpp
* 
* Implementation of class BarCodeReader
*
* @author <A href=mailto:goehring@informatik.hu-berlin.de>Daniel Göhring</A>
*/

#include "ChallengeSpecialVision.h"
#include "Tools/Debugging/DebugImages.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Representations/Perception/SensorData.h"




ChallengeSpecialVision::ChallengeSpecialVision(const SpecialVisionInterfaces& interfaces)
: SpecialVision(interfaces)
{
  executeReset();
}

void ChallengeSpecialVision::executeReset()
{
  flagCount = 0;
  foundAngleCount = 0;
  Vector2<double> tmp;
  tmp.x = 2100;
  tmp.y = 1450;
  flagAngle[flagCount++] = toDegrees(tmp.angle());
  foundAngle[foundAngleCount++] = flagAngle[flagCount];
 
  tmp.x *= -1;
  flagAngle[flagCount++] = toDegrees(tmp.angle());
  foundAngle[foundAngleCount++] = flagAngle[flagCount];

  tmp.y *= -1;
  flagAngle[flagCount++] = toDegrees(tmp.angle());
  foundAngle[foundAngleCount++] = flagAngle[flagCount];

  tmp.x *= -1;
  flagAngle[flagCount++] = toDegrees(tmp.angle());
  foundAngle[foundAngleCount++] = flagAngle[flagCount];

}
void ChallengeSpecialVision::execute()
{
  specialPercept.orientation = 0;
  switch(specialVisionRequest.specialVisionRequest)
  {
    case SpecialVisionRequest::challengeGetOrientation:
      executeGetOrientation();
      break;
    case SpecialVisionRequest::challengeLearnOrientation:
      executeLearnOrientation();
      break;
    case SpecialVisionRequest::challengeFindOrientation:
      executeFindOrientation();
      break;
    default:
      break;
  }
  specialPercept.countAngleOfPinkFlags = 0;
/*
  if (specialVisionRequest.specialVisionRequest == SpecialVisionRequest::challengeGetOrientation)
    {
     executeGetOrientation();
    }
  else
  {
       executeLearnOrientation();
  }
  */
}

ChallengeSectorData ChallengeSpecialVision::countPixels(int x1,int x2,int y1,int y2)
{
    int x;
  int y;
  ChallengeSectorData csd;

  for (x=x1; x<x2; x++)
  {
       for (y=y1; y<y2; y++)
     {
       colorClass color = colorTable.getColorClass(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
           csd.numPixels[(int)color] += 1;
     }
  }
    return csd;
}


void ChallengeSpecialVision::executeLearnOrientation()
{
//    DECLARE_DEBUG_IMAGE(imageProcessorPlayers);
//      INIT_DEBUG_IMAGE(imageProcessorPlayers, image);

/*
    imageProcessorPlayersImage.image[100][0][100]=128;
    imageProcessorPlayersImage.image[100][1][100]=255;
    imageProcessorPlayersImage.image[100][2][100]=0;
*/
//      SEND_DEBUG_IMAGE(imageProcessorPlayers);

  for (int ji = 0; ji < specialPercept.countAngleOfPinkFlags; ++ji)
  {
    double flagPerseptAngle;
    flagPerseptAngle = toDegrees(specialPercept.angleOfPinkFlags[ji]+robotPose.getAngle());

    if (flagPerseptAngle >180) 
      flagPerseptAngle -=360;
    if (flagPerseptAngle <-180) 
      flagPerseptAngle +=360;

    //suche kleinste abweichung zu vorhandener Flag
    double mindif=360+360;
    int j;
    for (j=0; j<flagCount;++j)
    {
      double dis = alngleDifDegree(flagAngle[j],flagPerseptAngle);
      dis = dis*dis;
      if (mindif>dis)
      {
         mindif = dis;
      }
    }

    // einfügen
    if ((mindif > 10)&&(flagCount<MAXLEARNFLAGS))
      flagAngle[flagCount++] = flagPerseptAngle;
 
    sort(flagAngle,flagCount);
    specialPercept.orientation = 0;
  }
}

void ChallengeSpecialVision::executeFindOrientation()
{
  for (int ji = 0; ji < specialPercept.countAngleOfPinkFlags; ++ji)
  {
    // get
    double flagPerseptAngle;
    flagPerseptAngle = toDegrees(specialPercept.angleOfPinkFlags[ji]+robotPose.getAngle());

    if (flagPerseptAngle >180) 
      flagPerseptAngle -=360;
    if (flagPerseptAngle <-180) 
      flagPerseptAngle +=360;

    //suche kleinste abweichung zu vorhandener Flag
    double mindif=360+360;
    int j;
    for (j=0; j<foundAngleCount;++j)
    {
      double dis = alngleDifDegree(foundAngle[j],flagPerseptAngle);
      dis = dis*dis;
      if (mindif>dis)
      {
         mindif = dis;
      }
    }

    // einfügen
    if ((mindif > 10)&&(foundAngleCount<MAXLEARNFLAGS))
      foundAngle[foundAngleCount++] = flagPerseptAngle;

    sort(foundAngle,foundAngleCount);
    specialPercept.orientation = 0;
  }
}

void ChallengeSpecialVision::executeGetOrientation()
{
    double min_abweichung_annahme= 99999999999.0;
    double min_abweichung_kontrolle= 99999999999.0;

    if ((foundAngleCount >0)&&(flagCount >0))
    {
      //berechne min abweichungen 
      for(int begin_mask=0;begin_mask<flagCount; begin_mask++)
      {
      //berechne abweichungen 
        double abweichung_annahme=0;//angenomme richtung 
        double abweichung_kontrolle=0;// gegenrichtung
        for (int i=0; i< flagCount;++i)
        {
          int j;
          j = (i + begin_mask)%flagCount;
          double tmp3 = flagAngle[j];
          j = (i )%foundAngleCount;
          //div by zero
          double tmp = foundAngle[j];
          if (tmp >180) 
            tmp -=360;
          if (tmp <-180) 
            tmp +=360;

          double tmp4;//abweichung positiv
          tmp4 = alngleDifDegree(tmp,tmp3);
          tmp4 = tmp4*tmp4;
          double tmp5;//abweichung negative 

          tmp += 180;
          if (tmp >180) 
            tmp -=360;
          if (tmp <-180) 
            tmp +=360;
          tmp5 = alngleDifDegree(tmp,tmp3);
          tmp5 = tmp5 * tmp5;
          abweichung_annahme += tmp4;
          abweichung_kontrolle += tmp5;
        }
        if (min_abweichung_annahme>abweichung_annahme)
          min_abweichung_annahme = abweichung_annahme;
        if (min_abweichung_kontrolle>abweichung_kontrolle)
          min_abweichung_kontrolle = abweichung_kontrolle;
      }
  }

    if (min_abweichung_annahme > min_abweichung_kontrolle)
      specialPercept.orientation = -1;
    else
      specialPercept.orientation = 1;

}

/* tool */
void ChallengeSpecialVision::sort( double* a, const int number)
{
  double tmp;
  for(int i=0; i<number-1;++i)
  {
    if (a[i]>a[i+1])
    {
      tmp = a[i];
      a[i] = a[i+1];
      a[i+1] = tmp;
      i = 0;
    }
  }
}
double ChallengeSpecialVision::alngleDifDegree(double a, double b)
{
  double dif = fabs(a-b);
  if (dif > 180)
    dif = 360 - dif;

  return dif;
}





/*
* Change log :
* 
* $Log: ChallengeSpecialVision.cpp,v $
* Revision 1.5  2004/07/01 18:42:08  hamerla
* SLAM challenge find Orientation
*
* Revision 1.4  2004/06/28 20:04:52  altmeyer
* outcomment error lines
*
* Revision 1.3  2004/06/28 18:43:31  wachter
* - bugfixes
*
* Revision 1.2  2004/06/28 16:02:37  wachter
* First lines of code for challenge-special-vision
*
* Revision 1.1  2004/06/28 11:54:55  wachter
* Added Special-Vision classes for Challenge
*
*
*/

---