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

# Modules/TeamBallLocator/GT2005Particles/GT2005ParticleContainerReceived.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file GT2005ParticleContainerReceived.cpp
    00003  *
    00004  * This file contains a Particle Container used by the GT2005TeamBallLocator.
    00005  *
    00006  * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
    00007  * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
    00008  */
    00009 //------------------------------------------------------------------------------
    00010 #include "GT2005ParticleContainerReceived.h"
    00011 #include "Modules/BallLocator/GT2005Particles/GT2005Particle.h"
    00012 #include "Tools/Debugging/Debugging.h"
    00013 #include "Tools/Actorics/RobotDimensions.h"
    00014 #include "Tools/FieldDimensions.h"
    00015 #include "Platform/SystemCall.h"
    00016 #include "Tools/Debugging/DebugDrawings.h"
    00017 #include "Platform/GTAssert.h"
    00018 #include "GT2005ParticleContainerSend.h"
    00019 //------------------------------------------------------------------------------
    00020 GT2005ParticleContainerReceived::GT2005ParticleContainerReceived()
    00021 {
    00022   n = 4*numOfBLParticlesSend;
    00023   current = new GT2005Particle[n];
    00024   for (int i = 0; i < n; i++)
    00025     current[i].setParameters(0, 0, 0, 0, 0, 0);
    00026 }
    00027 //------------------------------------------------------------------------------
    00028 GT2005ParticleContainerReceived::GT2005ParticleContainerReceived(int n)
    00029 {
    00030   this->n = n;
    00031   current = new GT2005Particle[n];
    00032   for (int i = 0; i < getNumberOfSamples(); i++)
    00033     current[i].setParameters(0, 0, 0, 0, 0, 0);
    00034 }
    00035 //------------------------------------------------------------------------------
    00036 GT2005ParticleContainerReceived::~GT2005ParticleContainerReceived()
    00037 {
    00038   delete [] current;
    00039 }
    00040 //------------------------------------------------------------------------------
    00041 void GT2005ParticleContainerReceived::calculateEstimatedBallPosition(double& ballX,
    00042                                                                      double& ballY,
    00043                                                                      double& ballP)
    00044 {
    00045   /* The idea of calculating an average is taken from the GT2004SelfLocator.
    00046    * First Make a grid on the field and count the number of particles in each
    00047    * cell (in contrast to the SelfLocator we do not only count the number but
    00048    * take the sum of the probabilities), then calculate an average out of the
    00049    * 2x2 grid with the biggest sum.
    00050    */
    00051   int i;
    00052   for (i = 0; i < blMaxGrid; i++)
    00053   {
    00054     for (int j = 0; j < blMaxGrid; j++)
    00055     {
    00056       cells[i][j].clear();
    00057     }
    00058   }
    00059
    00060   double width = 2*yPosLeftSideline,
    00061          height = 2*xPosOpponentGroundline;
    00062
    00063   //Attach the samples to the corresponding grid cells:
    00064   for (i = 0; i < getNumberOfSamples(); ++i)
    00065   {
    00066     GT2005Particle& particle = current[i];
    00067     int y = static_cast<int>((particle.pose.y / height + 0.5) * blMaxGrid),
    00068         x = static_cast<int>((particle.pose.x / width + 0.5) * blMaxGrid);
    00069     x = max(0, x);
    00070     if (x >= blMaxGrid)
    00071       x = blMaxGrid - 1;
    00072     y = max(0, y);
    00073     if (y >= blMaxGrid)
    00074       y = blMaxGrid - 1;
    00075     Cell& c = cells[y][x];
    00076     particle.next = c.first;
    00077     c.first = &particle;
    00078     c.count += particle.probability;
    00079   }
    00080
    00081   //Determine the 2x2 sub-grid with biggest sum:
    00082   yMax = 0;
    00083   xMax = 0;
    00084   countMax = 0;
    00085   int x, y;
    00086   for (y = 0; y < blMaxGrid - 1; ++y)
    00087     for (x = 0; x < blMaxGrid - 1; ++x)
    00088     {
    00089       double count = cells[y][x].count +
    00090                      cells[y][x+1].count +
    00091                      cells[y+1][x].count +
    00092                      cells[y+1][x+1].count;
    00093       if (count > countMax)
    00094       {
    00095         countMax = count;
    00096         yMax = y;
    00097         xMax = x;
    00098       }
    00099     }
    00100
    00101   if (countMax > 0)
    00102   {
    00103     //Determine the average pose of all samples in the winner sub-cube:
    00104     double xSum = 0,
    00105            ySum = 0,
    00106            numOfParticles = 0;
    00107     for (y = 0; y < 2; ++y)
    00108       for (x = 0; x < 2; ++x)
    00109       {
    00110         GT2005Particle* p;
    00111         for (p = cells[(int)(yMax) + y][(int)(xMax) + x].first; p; p = p->next)
    00112         {
    00113           xSum += p->pose.x * p->probability;
    00114           ySum += p->pose.y * p->probability;
    00115           numOfParticles += 1;
    00116         }
    00117       }
    00118     ballX = xSum / countMax;
    00119     ballY = ySum / countMax;
    00120     ballP = countMax / numOfParticles;
    00121   }
    00122 }
    00123 //------------------------------------------------------------------------------
    00124 void GT2005ParticleContainerReceived::calculateEstimatedBallVelocity(double& ballVX,
    00125                                                                      double& ballVY)
    00126 {
    00127   if (countMax > 0)
    00128   {
    00129     //Determine the average pose of all samples in the winner sub-grid:
    00130     double vxSum = 0,
    00131            vySum = 0;
    00132     for (int y = 0; y < 2; ++y)
    00133       for (int x = 0; x < 2; ++x)
    00134       {
    00135         GT2005Particle* p;
    00136         for (p = cells[(int)(yMax) + y][(int)(xMax) + x].first; p; p = p->next)
    00137         {
    00138           vxSum += p->vx * p->probability;
    00139           vySum += p->vy * p->probability;
    00140         }
    00141       }
    00142     ballVX = vxSum / countMax;
    00143     ballVY = vySum / countMax;
    00144   }
    00145 }
    00146 //------------------------------------------------------------------------------
    00147 void GT2005ParticleContainerReceived::calculateEstimatedBallVelocity(double& ballVX,
    00148                                                                      double& ballVY,
    00149                                                                      double& ballVP)
    00150 {
    00151   //Determine the average pose of all samples in the winner sub-grid:
    00152   double vxSum = 0,
    00153     vySum = 0,
    00154     vpSum = 0;
    00155   for (int y = 0; y < 2; ++y)
    00156   {
    00157     for (int x = 0; x < 2; ++x)
    00158     {
    00159       GT2005Particle* p;
    00160       for (p = cells[(int)(yMax) + y][(int)(xMax) + x].first; p; p = p->next)
    00161       {
    00162         vxSum += p->vx * p->probability;
    00163         vySum += p->vy * p->probability;
    00164         vpSum += p->vprob;
    00165       }
    00166     }
    00167   }
    00168   if (vpSum != 0)
    00169   {
    00170     ballVX = vxSum / vpSum;
    00171     ballVY = vySum / vpSum;
    00172   }
    00173   else
    00174   {
    00175     ballVX = 0;
    00176     ballVY = 0;
    00177     ballVP = 0;
    00178   }
    00179 }
    00180 //------------------------------------------------------------------------------