Modules/BallLocator/GT2005Particles/GT2005ParticleContainer.cpp

Go to the documentation of this file.

/**
 * @file GT2005ParticleContainer.cpp
 * 
 * This file contains a Particle Container used by the GT2005BallLocator.
 *
 * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
 * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
 */
//------------------------------------------------------------------------------
#include "GT2005ParticleContainer.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "Tools/FieldDimensions.h"
#include "Platform/SystemCall.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Platform/GTAssert.h"
//------------------------------------------------------------------------------
GT2005ParticleContainer::GT2005ParticleContainer()
{
  for (int i = 0; i < getNumberOfSamples(); i++)
    (*this)[i].setParameters(0, 0, 0, 0, 0, 0);
}
//------------------------------------------------------------------------------
int GT2005ParticleContainer::getIndexOfParticleWithLowestProb()
{
  //Find the first particle with probability != 0:
  int lowest = 0;
  while ((*this)[lowest].probability == 0)
  {
    lowest += 1;
    if (lowest == getNumberOfSamples())
      return 0;
  }

  //Find the particle with the lowest probability != 0:
  for (int i = lowest + 1; i < getNumberOfSamples(); i++)
  {
    if (((*this)[i].probability != 0)
      && ((*this)[i].probability < (*this)[lowest].probability))
    {
      lowest = i;
    }
  }
  return lowest;
}
//------------------------------------------------------------------------------
void GT2005ParticleContainer::calculateEstimatedBall(double& ballX,
                                                     double& ballY,
                                                     double& ballP,
                                                     double& ballVX,
                                                     double& ballVY,
                                                     double& ballVP)
{
  /* The idea of calculating an average is taken from the GT2004SelfLocator.
   * First Make a grid on the field and count the number of particles in each
   * cell (in contrast to the SelfLocator we do not only count the number but
   * take the sum of the probabilities), then calculate an average out of the
   * 2x2 grid with the biggest sum.
   */
  int i;
  for (i = 0; i < blMaxGrid; i++)
  {
    for (int j = 0; j < blMaxGrid; j++)
    {
      cells[i][j].clear();
    }
  }

  double width = 2*yPosLeftSideline,
         height = 2*xPosOpponentGroundline;

  //Attach the samples to the corresponding grid cells:
  for (i = 0; i < getNumberOfSamples(); ++i)
  {
    GT2005Particle& particle = (*this)[i];
    int y = static_cast<int>((particle.pose.y / height + 0.5) * blMaxGrid),
        x = static_cast<int>((particle.pose.x / width + 0.5) * blMaxGrid);
    x = max(0, x);
    if (x >= blMaxGrid)
      x = blMaxGrid - 1;
    y = max(0, y);
    if (y >= blMaxGrid)
      y = blMaxGrid - 1;
    Cell& c = cells[y][x];
    particle.next = c.first;
    c.first = &particle;
    c.count += particle.probability;
  }

  //Determine the 2x2 sub-grid with biggest sum:
  yMax = 0;
  xMax = 0;
  countMax = 0;
  int x, y;
  for (y = 0; y < blMaxGrid - 1; ++y)
    for (x = 0; x < blMaxGrid - 1; ++x)
    {
      double count = cells[y][x].count +
                     cells[y][x+1].count +
                     cells[y+1][x].count +
                     cells[y+1][x+1].count;
      if (count > countMax)
      {
        countMax = count;
        yMax = y;
        xMax = x;
      }
    }

  if (countMax > 0)
  {
    //Determine the average pose of all samples in the winner sub-cube:
    double xSum = 0,
           ySum = 0,
           numOfParticles = 0,
           vxSum = 0,
           vySum = 0,
           vpSum = 0;
    for (y = 0; y < 2; ++y)
      for (x = 0; x < 2; ++x)
      {
        GT2005Particle* p;
        for (p = cells[(int)(yMax) + y][(int)(xMax) + x].first; p; p = p->next)
        {
          xSum += p->pose.x * p->probability;
          ySum += p->pose.y * p->probability;
          numOfParticles += 1;
          vxSum += p->vx * p->probability;
          vySum += p->vy * p->probability;
          vpSum += p->vprob * p->probability;
        }
      }
    ballX = xSum / countMax;
    ballY = ySum / countMax;
    ballVX = vxSum / countMax;
    ballVY = vySum / countMax;
    ballVP = vpSum / countMax;

    ballP = 0;
    double distance = 0;
    double distSum = 0;
    for (i = 0; i < getNumberOfSamples(); i++)
    {
      GT2005Particle& particle = (*this)[i];
      distance = Geometry::distance(Vector2<double>(ballX, ballY),
                                    Vector2<double>(particle.pose.x,
                                                    particle.pose.y));
      distSum += (1 / distance);
      ballP += particle.probability * (1 / distance);
    }

    ballP /= distSum;
  }
}
//------------------------------------------------------------------------------
int GT2005ParticleContainer::getNumberOfSamples()
{
  return numOfBLParticles;
}
//------------------------------------------------------------------------------

---