Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/SpecialVision/ChallengeSpecialVision.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file ChallengeSpecialVision.cpp
    00003 *
    00004 * Implementation of class BarCodeReader
    00005 *
    00006 * @author <A href=mailto:goehring@informatik.hu-berlin.de>Daniel Göhring</A>
    00007 */
    00008
    00009 #include "ChallengeSpecialVision.h"
    00010 #include "Tools/Debugging/DebugImages.h"
    00011 #include "Tools/Debugging/DebugDrawings.h"
    00012 #include "Representations/Perception/SensorData.h"
    00013
    00014
    00015
    00016
    00017 ChallengeSpecialVision::ChallengeSpecialVision(const SpecialVisionInterfaces& interfaces)
    00018 : SpecialVision(interfaces)
    00019 {
    00020   executeReset();
    00021 }
    00022
    00023 void ChallengeSpecialVision::executeReset()
    00024 {
    00025   flagCount = 0;
    00026   foundAngleCount = 0;
    00027   Vector2<double> tmp;
    00028   tmp.x = 2100;
    00029   tmp.y = 1450;
    00030   flagAngle[flagCount++] = toDegrees(tmp.angle());
    00031   foundAngle[foundAngleCount++] = flagAngle[flagCount];
    00032
    00033   tmp.x *= -1;
    00034   flagAngle[flagCount++] = toDegrees(tmp.angle());
    00035   foundAngle[foundAngleCount++] = flagAngle[flagCount];
    00036
    00037   tmp.y *= -1;
    00038   flagAngle[flagCount++] = toDegrees(tmp.angle());
    00039   foundAngle[foundAngleCount++] = flagAngle[flagCount];
    00040
    00041   tmp.x *= -1;
    00042   flagAngle[flagCount++] = toDegrees(tmp.angle());
    00043   foundAngle[foundAngleCount++] = flagAngle[flagCount];
    00044
    00045 }
    00046 void ChallengeSpecialVision::execute()
    00047 {
    00048   specialPercept.orientation = 0;
    00049   switch(specialVisionRequest.specialVisionRequest)
    00050   {
    00051     case SpecialVisionRequest::challengeGetOrientation:
    00052       executeGetOrientation();
    00053       break;
    00054     case SpecialVisionRequest::challengeLearnOrientation:
    00055       executeLearnOrientation();
    00056       break;
    00057     case SpecialVisionRequest::challengeFindOrientation:
    00058       executeFindOrientation();
    00059       break;
    00060     default:
    00061       break;
    00062   }
    00063   specialPercept.countAngleOfPinkFlags = 0;
    00064 /*
    00065   if (specialVisionRequest.specialVisionRequest == SpecialVisionRequest::challengeGetOrientation)
    00066     {
    00067      executeGetOrientation();
    00068     }
    00069   else
    00070   {
    00071        executeLearnOrientation();
    00072   }
    00073   */
    00074 }
    00075
    00076 ChallengeSectorData ChallengeSpecialVision::countPixels(int x1,int x2,int y1,int y2)
    00077 {
    00078     int x;
    00079   int y;
    00080   ChallengeSectorData csd;
    00081
    00082   for (x=x1; x<x2; x++)
    00083   {
    00084        for (y=y1; y<y2; y++)
    00085      {
    00086        colorClass color = colorTable.getColorClass(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
    00087            csd.numPixels[(int)color] += 1;
    00088      }
    00089   }
    00090     return csd;
    00091 }
    00092
    00093
    00094 void ChallengeSpecialVision::executeLearnOrientation()
    00095 {
    00096 //    DECLARE_DEBUG_IMAGE(imageProcessorPlayers);
    00097 //      INIT_DEBUG_IMAGE(imageProcessorPlayers, image);
    00098
    00099 /*
    00100     imageProcessorPlayersImage.image[100][0][100]=128;
    00101     imageProcessorPlayersImage.image[100][1][100]=255;
    00102     imageProcessorPlayersImage.image[100][2][100]=0;
    00103 */
    00104 //      SEND_DEBUG_IMAGE(imageProcessorPlayers);
    00105
    00106   for (int ji = 0; ji < specialPercept.countAngleOfPinkFlags; ++ji)
    00107   {
    00108     double flagPerseptAngle;
    00109     flagPerseptAngle = toDegrees(specialPercept.angleOfPinkFlags[ji]+robotPose.getAngle());
    00110
    00111     if (flagPerseptAngle >180)
    00112       flagPerseptAngle -=360;
    00113     if (flagPerseptAngle <-180)
    00114       flagPerseptAngle +=360;
    00115
    00116     //suche kleinste abweichung zu vorhandener Flag
    00117     double mindif=360+360;
    00118     int j;
    00119     for (j=0; j<flagCount;++j)
    00120     {
    00121       double dis = alngleDifDegree(flagAngle[j],flagPerseptAngle);
    00122       dis = dis*dis;
    00123       if (mindif>dis)
    00124       {
    00125          mindif = dis;
    00126       }
    00127     }
    00128
    00129     // einfügen
    00130     if ((mindif > 10)&&(flagCount<MAXLEARNFLAGS))
    00131       flagAngle[flagCount++] = flagPerseptAngle;
    00132
    00133     sort(flagAngle,flagCount);
    00134     specialPercept.orientation = 0;
    00135   }
    00136 }
    00137
    00138 void ChallengeSpecialVision::executeFindOrientation()
    00139 {
    00140   for (int ji = 0; ji < specialPercept.countAngleOfPinkFlags; ++ji)
    00141   {
    00142     // get
    00143     double flagPerseptAngle;
    00144     flagPerseptAngle = toDegrees(specialPercept.angleOfPinkFlags[ji]+robotPose.getAngle());
    00145
    00146     if (flagPerseptAngle >180)
    00147       flagPerseptAngle -=360;
    00148     if (flagPerseptAngle <-180)
    00149       flagPerseptAngle +=360;
    00150
    00151     //suche kleinste abweichung zu vorhandener Flag
    00152     double mindif=360+360;
    00153     int j;
    00154     for (j=0; j<foundAngleCount;++j)
    00155     {
    00156       double dis = alngleDifDegree(foundAngle[j],flagPerseptAngle);
    00157       dis = dis*dis;
    00158       if (mindif>dis)
    00159       {
    00160          mindif = dis;
    00161       }
    00162     }
    00163
    00164     // einfügen
    00165     if ((mindif > 10)&&(foundAngleCount<MAXLEARNFLAGS))
    00166       foundAngle[foundAngleCount++] = flagPerseptAngle;
    00167
    00168     sort(foundAngle,foundAngleCount);
    00169     specialPercept.orientation = 0;
    00170   }
    00171 }
    00172
    00173 void ChallengeSpecialVision::executeGetOrientation()
    00174 {
    00175     double min_abweichung_annahme= 99999999999.0;
    00176     double min_abweichung_kontrolle= 99999999999.0;
    00177
    00178     if ((foundAngleCount >0)&&(flagCount >0))
    00179     {
    00180       //berechne min abweichungen
    00181       for(int begin_mask=0;begin_mask<flagCount; begin_mask++)
    00182       {
    00183       //berechne abweichungen
    00184         double abweichung_annahme=0;//angenomme richtung
    00185         double abweichung_kontrolle=0;// gegenrichtung
    00186         for (int i=0; i< flagCount;++i)
    00187         {
    00188           int j;
    00189           j = (i + begin_mask)%flagCount;
    00190           double tmp3 = flagAngle[j];
    00191           j = (i )%foundAngleCount;
    00192           //div by zero
    00193           double tmp = foundAngle[j];
    00194           if (tmp >180)
    00195             tmp -=360;
    00196           if (tmp <-180)
    00197             tmp +=360;
    00198
    00199           double tmp4;//abweichung positiv
    00200           tmp4 = alngleDifDegree(tmp,tmp3);
    00201           tmp4 = tmp4*tmp4;
    00202           double tmp5;//abweichung negative
    00203
    00204           tmp += 180;
    00205           if (tmp >180)
    00206             tmp -=360;
    00207           if (tmp <-180)
    00208             tmp +=360;
    00209           tmp5 = alngleDifDegree(tmp,tmp3);
    00210           tmp5 = tmp5 * tmp5;
    00211           abweichung_annahme += tmp4;
    00212           abweichung_kontrolle += tmp5;
    00213         }
    00214         if (min_abweichung_annahme>abweichung_annahme)
    00215           min_abweichung_annahme = abweichung_annahme;
    00216         if (min_abweichung_kontrolle>abweichung_kontrolle)
    00217           min_abweichung_kontrolle = abweichung_kontrolle;
    00218       }
    00219   }
    00220
    00221     if (min_abweichung_annahme > min_abweichung_kontrolle)
    00222       specialPercept.orientation = -1;
    00223     else
    00224       specialPercept.orientation = 1;
    00225
    00226 }
    00227
    00228 /* tool */
    00229 void ChallengeSpecialVision::sort( double* a, const int number)
    00230 {
    00231   double tmp;
    00232   for(int i=0; i<number-1;++i)
    00233   {
    00234     if (a[i]>a[i+1])
    00235     {
    00236       tmp = a[i];
    00237       a[i] = a[i+1];
    00238       a[i+1] = tmp;
    00239       i = 0;
    00240     }
    00241   }
    00242 }
    00243 double ChallengeSpecialVision::alngleDifDegree(double a, double b)
    00244 {
    00245   double dif = fabs(a-b);
    00246   if (dif > 180)
    00247     dif = 360 - dif;
    00248
    00249   return dif;
    00250 }