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

# Modules/BallLocator/GT2005Particles/GT2005ParticleContainer.cpp

Go to the documentation of this file.

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