Modules/TeamBallLocator/GT2005Particles/GT2005ParticleContainerReceived.cpp

Go to the documentation of this file.

/**
 * @file GT2005ParticleContainerReceived.cpp
 * 
 * This file contains a Particle Container used by the GT2005TeamBallLocator.
 *
 * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
 * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
 */
//------------------------------------------------------------------------------
#include "GT2005ParticleContainerReceived.h"
#include "Modules/BallLocator/GT2005Particles/GT2005Particle.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"
#include "GT2005ParticleContainerSend.h"
//------------------------------------------------------------------------------
GT2005ParticleContainerReceived::GT2005ParticleContainerReceived()
{
  n = 4*numOfBLParticlesSend;
  current = new GT2005Particle[n];
  for (int i = 0; i < n; i++)
    current[i].setParameters(0, 0, 0, 0, 0, 0);
}
//------------------------------------------------------------------------------
GT2005ParticleContainerReceived::GT2005ParticleContainerReceived(int n)
{
  this->n = n;
  current = new GT2005Particle[n];
  for (int i = 0; i < getNumberOfSamples(); i++)
    current[i].setParameters(0, 0, 0, 0, 0, 0);
}
//------------------------------------------------------------------------------
GT2005ParticleContainerReceived::~GT2005ParticleContainerReceived()
{
  delete [] current;
}
//------------------------------------------------------------------------------
void GT2005ParticleContainerReceived::calculateEstimatedBallPosition(double& ballX,
                                                                     double& ballY,
                                                                     double& ballP)
{
  /* 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 = current[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;
    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;
        }
      }
    ballX = xSum / countMax;
    ballY = ySum / countMax;
    ballP = countMax / numOfParticles;
  }
}
//------------------------------------------------------------------------------
void GT2005ParticleContainerReceived::calculateEstimatedBallVelocity(double& ballVX,
                                                                     double& ballVY)
{
  if (countMax > 0)
  {
    //Determine the average pose of all samples in the winner sub-grid:
    double vxSum = 0,
           vySum = 0;
    for (int y = 0; y < 2; ++y)
      for (int x = 0; x < 2; ++x)
      {
        GT2005Particle* p;
        for (p = cells[(int)(yMax) + y][(int)(xMax) + x].first; p; p = p->next)
        {
          vxSum += p->vx * p->probability;
          vySum += p->vy * p->probability;
        }
      }
    ballVX = vxSum / countMax;
    ballVY = vySum / countMax;
  }
}
//------------------------------------------------------------------------------
void GT2005ParticleContainerReceived::calculateEstimatedBallVelocity(double& ballVX,
                                                                     double& ballVY,
                                                                     double& ballVP)
{
  //Determine the average pose of all samples in the winner sub-grid:
  double vxSum = 0,
    vySum = 0,
    vpSum = 0;
  for (int y = 0; y < 2; ++y)
  {
    for (int x = 0; x < 2; ++x)
    {
      GT2005Particle* p;
      for (p = cells[(int)(yMax) + y][(int)(xMax) + x].first; p; p = p->next)
      {
        vxSum += p->vx * p->probability;
        vySum += p->vy * p->probability;
        vpSum += p->vprob;
      }
    }
  }
  if (vpSum != 0)
  {
    ballVX = vxSum / vpSum;
    ballVY = vySum / vpSum;
  }
  else
  {
    ballVX = 0;
    ballVY = 0;
    ballVP = 0;
  }
}
//------------------------------------------------------------------------------

---