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

# Modules/ImageProcessor/SlamImageProcessor/SlamFlagLocator.cpp

Go to the documentation of this file.

    00001 #include "SlamFlagLocator.h"
    00002
    00003
    00004
    00005 SlamFlagLocator::SlamFlagLocator()
    00006 {
    00007   for (int c=0;c<SlamFlagLocator::NewFlagContainer::NO_OF_CHARACTERIZATIONS;c++)
    00008   {
    00009     cluster[c] = new long*[xSize];
    00010     for (int i=0;i<xSize;i++)
    00011     {
    00012       cluster[c][i] = new long[ySize];
    00013       for (int k=0;k<ySize;k++)
    00014         cluster[c][i][k] = 0;
    00015     }
    00016   }
    00017   updateFinished = false;
    00018   container = new NewFlagContainer();
    00019   averageParam = 1.3;
    00020   maxParam = 0.6;
    00021 }
    00022
    00023 SlamFlagLocator::~SlamFlagLocator()
    00024 {
    00025   for(int c=0;c<SlamFlagLocator::NewFlagContainer::NO_OF_CHARACTERIZATIONS;c++)
    00026   {
    00027     for(int i=0;i<xSize;i++)
    00028       delete[] cluster[c][i];
    00029     delete[] cluster[c];
    00030   }
    00031
    00032   delete container;
    00033 }
    00034
    00035 void SlamFlagLocator::addFlagDirection(const Pose2D& robotPose, double bearing, int characterization)
    00036 {
    00037   if (updateFinished)
    00038     return;
    00039
    00040   CameraInfo ci = CameraInfo();
    00041   ci.resolutionHeight = ySize;
    00042   ci.resolutionWidth = xSize;
    00043   Vector2<int> scaledPos = Vector2<int>((int)(robotPose.translation.x/100 + xSize/2),(int)(robotPose.translation.y/100 + ySize/2));
    00044   Vector2<int> pixel = scaledPos;
    00045   BresenhamLineScan lineScanner(scaledPos,bearing,ci);
    00046   for (int i=0;i<lineScanner.numberOfPixels;i++)
    00047   {
    00048     lineScanner.getNext(pixel);
    00049     pixel.x = pixel.x < 0 ? 0 : pixel.x;
    00050     pixel.y = pixel.y < 0 ? 0 : pixel.y;
    00051     pixel.x = pixel.x > 66 ? 66 : pixel.x;
    00052     pixel.y = pixel.y > 46 ? 46 : pixel.y;
    00053
    00054     // update border only
    00055     if (pixel.x>63 ||pixel.x < 3 || pixel.y>43 || pixel.y<3)
    00056     {
    00057       cluster[characterization][pixel.x][pixel.y]++;
    00058     }
    00059
    00060   }
    00061 }
    00062
    00063 void SlamFlagLocator::draw()
    00064 {
    00065   NDECLARE_DEBUGDRAWING("newFlags","DrawingOnField","new flags");
    00066   int x,y;
    00067   for(int i=0;i<66;i++)
    00068     for(int k=0;k<46;k++)
    00069     {
    00070       x = i*100;
    00071       y = k*100;
    00072       x -= 3300;
    00073       y -= 2300;
    00074       x *= 1;
    00075       y *= 1;
    00076       LINE(selfLocatorField,6000,0,6000,4000,20,Drawings::ps_solid,Drawings::red );
    00077       if (cluster[0][i][k]>50){
    00078         NCIRCLE("newFlags",x,y,20,7,Drawings::ps_solid,Drawings::black);
    00079         CIRCLE(selfLocatorField,x,y,20,7,Drawings::ps_solid,Drawings::black);
    00080       } else if (cluster[0][i][k]>20){
    00081         NCIRCLE("newFlags",x,y,20,7,Drawings::ps_solid,Drawings::red);
    00082         CIRCLE(selfLocatorField,x,y,20,7,Drawings::ps_solid,Drawings::red);
    00083       }  else if (cluster[0][i][k]>0){
    00084         NCIRCLE("newFlags",x,y,20,7,Drawings::ps_solid,Drawings::yellow);
    00085         CIRCLE(selfLocatorField,x,y,20,7,Drawings::ps_solid,Drawings::yellow);
    00086       }
    00087     }
    00088
    00089     Vector2<int>* flags;
    00090     int stored;
    00091     container->getFlagsForCharacterization(&flags,stored,0);
    00092     for(int i=0;i<stored;i++)
    00093     {
    00094       OUTPUT(idText,text,"slamsl: printing flag no "+i);
    00095       CIRCLE(selfLocatorField,flags[i].x,flags[i].y,40,20,Drawings::ps_solid,Drawings::blue);
    00096     }
    00097 }
    00098
    00099 void SlamFlagLocator::finishUpdate()
    00100 {
    00101   OUTPUT(idText,text,"slamflaglocator: update phase ended");
    00102   updateFinished = true;
    00103   calcFlags();
    00104 }
    00105
    00106 void SlamFlagLocator::calcFlags()
    00107   {
    00108     if (!updateFinished)
    00109       return;
    00110
    00111     double avg = 0.0;
    00112     double bestValues[SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS];
    00113     Vector2<int> best[SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS];
    00114
    00115
    00116     for (int c=0;c<SlamFlagLocator::NewFlagContainer::NO_OF_CHARACTERIZATIONS;c++)
    00117     {
    00118
    00119       // store the n best clusters
    00120       for(int i=0;i<SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;i++)
    00121       {
    00122         bestValues[i] = 0.0;
    00123       }
    00124
    00125       Vector2<int> testPoint;
    00126
    00127       for(int y = 0; y < ySize - 2; y += 2)
    00128         for(int x = 0; x < xSize - 2; x +=2)
    00129         {
    00130           long dummy = cluster[c][x][y] +
    00131                        cluster[c][x+1][y] +
    00132                        cluster[c][x+2][y] +
    00133                        cluster[c][x][y+1] +
    00134                        cluster[c][x+1][y+1] +
    00135                        cluster[c][x+2][y+1] +
    00136                        cluster[c][x][y+2] +
    00137                        cluster[c][x+1][y+2] +
    00138                        cluster[c][x+2][y+2];
    00139
    00140           avg = ((double)dummy)/9.0;
    00141
    00142           for(int k=0;k<SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;k++)
    00143           {
    00144             testPoint.x = x+1;
    00145             testPoint.y = y+1;
    00146
    00147             if((best[k]-testPoint).abs()<4){
    00148               if(avg > bestValues[k]){
    00149                 bestValues[k] = avg;
    00150                 best[k].x = x+1;
    00151                 best[k].y = y+1;
    00152               }
    00153               break;
    00154             }
    00155
    00156             if(avg > bestValues[k])
    00157             {
    00158               for (int i=SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS-1;i>k;i--)
    00159               {
    00160                 best[i] = best[i-1];
    00161                 bestValues[i] = bestValues[i-1];
    00162               }
    00163
    00164               bestValues[k] = avg;
    00165               best[k].x = x+1;
    00166               best[k].y = y+1;
    00167             }
    00168           }
    00169         }
    00170
    00171         double max = 0.0;
    00172         double avg2 = 0.0;
    00173         // calculate the best flags out of this buffer
    00174         for(int i=0;i<SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;i++)
    00175         {
    00176           max = bestValues[i] > max ? bestValues[i] : max;
    00177           avg2 += bestValues[i];
    00178         }
    00179         avg2 /= SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;
    00180
    00181         for(int i=0;i<SlamFlagLocator::NewFlagContainer::NO_OF_FLAGS;i++)
    00182         {
    00183           if ((bestValues[i]>=averageParam*avg2 || bestValues[i]>=maxParam*max) && bestValues[i]>0)
    00184           {
    00185             Vector2<int> bestOnField(best[i]);
    00186             bestOnField.x *= 100;
    00187             bestOnField.y *= 100;
    00188             bestOnField.x -= 3500;
    00189             bestOnField.y -= 2500;
    00190             container->addFlag(bestOnField,c);
    00191           }
    00192
    00193         }
    00194     }
    00195   }
    00196 /*
    00197   void getClosestFlagBearing(double& targetBearing, const Pose2D& robotPose, double measuredBearing);
    00198   bool updateFinished;
    00199   */