Representations/Perception/BallPercept.cpp

Go to the documentation of this file.

/**
* @file BallPercept.cpp
*
* Implementation of class BallPercept.
*
* @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
* @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
* @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
*/

#include "BallPercept.h"
#include <math.h>

#include "Tools/Debugging/Debugging.h"

#include "Tools/Streams/StreamHandler.h"
#include "Representations/Perception/Image.h"

BallPercept::BallPercept() 
{
  reset(0);
}

void BallPercept::add(const Vector2<double>& offsetOnField,
                      const Vector2<double>& bearingBasedOffsetOnField,
                      const Vector2<double>& sizeBasedOffsetOnField,
                      const Vector2<double>& centerInImage,
                      double radiusInImage,
                      double reliability,
                      bool isCameraMatrixValid)
{
  ballWasSeen = true;
  this->offsetOnField = offsetOnField;
  this->bearingBasedOffsetOnField = bearingBasedOffsetOnField;
  this->sizeBasedOffsetOnField = sizeBasedOffsetOnField;
  this->centerInImage = centerInImage;
  this->radiusInImage = radiusInImage;
  this->reliability = reliability;
  this->isCameraMatrixValid = isCameraMatrixValid;
}

void BallPercept::addHighRes(const Vector2<int>& centerInPixel,
                             const Vector2<double>& centerAsAngles,
                             double radiusInPixel,
                             double radiusAsAngle,
                             const Vector3<double>& translationOfCamera,
                             bool isCameraMatrixValid,
                             double reliability)
{
  if (!ballWasSeen || this->reliability < reliability)
  {
    Vector2<double> bbo; // bearing based
    double distanceBearingBased = centerAsAngles.y < 0 ? (translationOfCamera.z - ballRadius) / tan(-centerAsAngles.y) : 10000;
    bbo.x = translationOfCamera.x + distanceBearingBased * cos(centerAsAngles.x);
    bbo.y = translationOfCamera.y + distanceBearingBased * sin(centerAsAngles.x);
    checkOffset(bbo);

    Vector2<double> sbo; // size based
    double distanceFromCameraToBallCenter = 
      Geometry::getBallDistanceByAngleSize(int(2 * ballRadius), 2 * radiusAsAngle);
    double distanceFromCameraToBallCenterOnGround;
    if(distanceFromCameraToBallCenter > fabs(translationOfCamera.z - ballRadius))
      distanceFromCameraToBallCenterOnGround = sqrt(sqr(distanceFromCameraToBallCenter) - sqr(translationOfCamera.z  - ballRadius));
    else
      distanceFromCameraToBallCenterOnGround = 0;
    sbo.x = translationOfCamera.x + distanceFromCameraToBallCenterOnGround * cos(centerAsAngles.x);
    sbo.y = translationOfCamera.y + distanceFromCameraToBallCenterOnGround * sin(centerAsAngles.x);
    checkOffset(sbo);

    Vector2<double> o; // offset
    double sbdist = sbo.abs();
    if(!isCameraMatrixValid || centerAsAngles.y >= 0 || sbdist > 300.0) 
      o = sbo;
    else if(sbdist > 200.0)
    {
      //interpolate between both offsets
      double factor = (300.0 - sbdist) / 100.0;
      o = bbo * factor + sbo * (1.0 - factor);
    }
    else 
      o = bbo;

    add(o, bbo, sbo, Vector2<double>(centerInPixel.x, centerInPixel.y) / 2.0, radiusInPixel / 2.0, reliability, isCameraMatrixValid);
  }
}

/*void BallPercept::getOffsetIntrinsic(Vector2<double>& offset) const
{
  double distanceSizeBased = Geometry::getDistanceBySize(
    cameraInfo,
    (int) ballRadius,
    radiusInImage,
    centerInImage.x,
    centerInImage.y
    );
  //~ offset.x = translationOfCamera.x + distanceSizeBased * cos(anglesToCenter.x);
  //~ offset.y = translationOfCamera.y + distanceSizeBased * sin(anglesToCenter.x);
  double distanceFromCameraToBallCenterOnGround;

  if(distanceSizeBased > fabs(translationOfCamera.z - ballRadius))
  {
    distanceFromCameraToBallCenterOnGround = sqrt(sqr(distanceSizeBased) - sqr(translationOfCamera.z  - ballRadius));
  }
  else
  {
    distanceFromCameraToBallCenterOnGround = 0;
  }

  offset.x = translationOfCamera.x + distanceFromCameraToBallCenterOnGround * cos(anglesToCenter.x);
  offset.y = translationOfCamera.y + distanceFromCameraToBallCenterOnGround * sin(anglesToCenter.x);
  checkOffset(offset);
}
*/

void BallPercept::checkOffset(Vector2<double>& offset) const
{
  if(offset.x < ballRadius / 2 && fabs(offset.y) < getRobotConfiguration().getRobotDimensions().lowerBodyWidth / 2 + ballRadius)
    offset.x = ballRadius / 2;
}

---