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>

BallPercept& BallPercept::operator=(const BallPercept& other)
{
  cameraInfo = other.cameraInfo;
  ballCenter = other.ballCenter;
  ballRadiusInPixel = other.ballRadiusInPixel;
  anglesToCenter = other.anglesToCenter;
  ballRadiusAsAngle = other.ballRadiusAsAngle;
  ballWasSeen = other.ballWasSeen;
  frameNumber = other.frameNumber;
  isCameraMatrixValid = other.isCameraMatrixValid;
  translationOfCamera = other.translationOfCamera;
  return *this;
}

void BallPercept::add(
  Vector2<double> anglesToCenter,
  double ballRadiusAsAngle,
  Vector3<double> translationOfCamera,
  bool isCameraMatrixValid
  )
{  
  this->anglesToCenter = anglesToCenter;
  this->ballRadiusAsAngle = ballRadiusAsAngle;
  this->translationOfCamera = translationOfCamera;
  this->isCameraMatrixValid = isCameraMatrixValid;
  ballWasSeen = true;
}

void BallPercept::add(
  CameraInfo cameraInfo,
  Vector2<int> ballCenter,
  double ballRadiusInPixel,
  Vector2<double> anglesToCenter,
  double ballRadiusAsAngle,
  Vector3<double> translationOfCamera,
  bool isCameraMatrixValid
  )
{
  this->cameraInfo = cameraInfo;
  this->ballCenter = ballCenter;
  this->ballRadiusInPixel = ballRadiusInPixel;
  this->anglesToCenter = anglesToCenter;
  this->ballRadiusAsAngle = ballRadiusAsAngle;
  this->translationOfCamera = translationOfCamera;
  this->isCameraMatrixValid = isCameraMatrixValid;
  ballWasSeen = true;
}

void BallPercept::getOffsetBearingBased(Vector2<double>& offset) const
{
  double distanceBearingBased;
  if(anglesToCenter.y < 0)
  {
    distanceBearingBased = (translationOfCamera.z - ballRadius) / tan(-anglesToCenter.y);
  }
  else
  {
    distanceBearingBased = 10000;
  }

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

void BallPercept::getOffsetIntrinsic(Vector2<double>& offset) const
{
  double distanceSizeBased = Geometry::getDistanceBySize(
                                          cameraInfo,
                                          (int)ballRadius,
                                          ballRadiusInPixel,
                                          ballCenter.x,
                                          ballCenter.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::getOffsetSizeBased(Vector2<double>& offset) const
{
  double distanceFromCameraToBallCenter = 
    Geometry::getBallDistanceByAngleSize(int(2 * ballRadius), 2 * ballRadiusAsAngle);

  double distanceFromCameraToBallCenterOnGround;

  if(distanceFromCameraToBallCenter > fabs(translationOfCamera.z - ballRadius))
  {
    distanceFromCameraToBallCenterOnGround = sqrt(sqr(distanceFromCameraToBallCenter) - 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::getOffset(Vector2<double>& offset) const
{
  if(!isCameraMatrixValid || anglesToCenter.y >= 0 || (getDistanceSizeBased() > 300.0)) 
    getOffsetSizeBased(offset);
  else if (getDistanceSizeBased() > 200.0)
  {
    //interpolate between both offsets
    double factor = (300.0 - getDistanceSizeBased()) / 100.0;
    Vector2<double> offsetSize, offsetBearing;
    getOffsetSizeBased(offsetSize);
    getOffsetBearingBased(offsetBearing);
    offset = offsetBearing * factor + offsetSize * (1.0 - factor);
  }
  else 
    getOffsetBearingBased(offset);
}

double BallPercept::getDistance() const
{
  Vector2<double> offset;
  getOffset(offset);
  return offset.abs();
}

double BallPercept::getAngle() const
{
  Vector2<double> offset;
  getOffset(offset);
  return offset.angle();
}

double BallPercept::getDistanceBearingBased() const
{
  Vector2<double> offset;
  getOffsetBearingBased(offset);
  return offset.abs();
}

double BallPercept::getDistanceSizeBased() const
{
  Vector2<double> offset;
  getOffsetSizeBased(offset);
  return offset.abs();
}

double BallPercept::getAngleBearingBased() const
{
  Vector2<double> offset;
  getOffsetBearingBased(offset);
  return offset.angle();
}

double BallPercept::getAngleSizeBased() const
{
  Vector2<double> offset;
  getOffsetSizeBased(offset);
  return offset.angle();
}

double BallPercept::getDistanceIntrinsicBased() const
{
  Vector2<double> offset;
  getOffsetIntrinsic(offset);
  return offset.abs();
}

double BallPercept::getAngleIntrinsicBased() const
{
  Vector2<double> offset;
  getOffsetIntrinsic(offset);
  return offset.angle();
}

In& operator>>(In& stream,BallPercept& ballPercept)
{
  int temp;
  stream >> ballPercept.ballCenter;
  stream >> ballPercept.ballRadiusInPixel;
  stream >> ballPercept.cameraInfo;
  stream >> ballPercept.anglesToCenter.x;
  stream >> ballPercept.anglesToCenter.y;
  stream >> ballPercept.ballRadiusAsAngle;
  stream >> ballPercept.translationOfCamera.x;
  stream >> ballPercept.translationOfCamera.y;
  stream >> ballPercept.translationOfCamera.z;
  stream >> temp;
  ballPercept.ballWasSeen = (temp != 0);
  stream >> ballPercept.frameNumber;
  stream >> temp;
  ballPercept.isCameraMatrixValid = (temp != 0);
  return stream;
}
 
Out& operator<<(Out& stream, const BallPercept& ballPercept)
{
  stream << ballPercept.ballCenter;
  stream << ballPercept.ballRadiusInPixel;
  stream << ballPercept.cameraInfo;
  stream << ballPercept.anglesToCenter.x;
  stream << ballPercept.anglesToCenter.y;
  stream << ballPercept.ballRadiusAsAngle;
  stream << ballPercept.translationOfCamera.x;
  stream << ballPercept.translationOfCamera.y;
  stream << ballPercept.translationOfCamera.z;
  stream << ballPercept.ballWasSeen;
  stream << ballPercept.frameNumber;
  stream << ballPercept.isCameraMatrixValid;
  return stream;
}


/*
 * Change log :
 * 
 * $Log: BallPercept.cpp,v $
 * Revision 1.7  2004/06/14 23:20:07  spranger
 * -changed some functions in Geometry from int to double including ballradius(fieldimensions)
 * -maybe all Geometric-functions in Geometry should be as precise as possible, avoiding int to double conversion errors
 *
 * Revision 1.6  2004/06/14 22:38:17  risler
 * getOffset interpolates betweeen size and bearing based percepts
 *
 * Revision 1.5  2004/06/11 14:30:39  mkunz
 * use bearing based calculations only if ball is very near
 *
 * Revision 1.4  2004/06/05 18:36:04  nistico
 * Cleanup
 *
 * Revision 1.3  2004/06/04 21:35:10  juengel
 * Added getBallDistanceByAngleSize.
 *
 * Revision 1.2  2004/05/22 22:52:02  juengel
 * Renamed ballP_osition to ballModel.
 *
 * Revision 1.1.1.1  2004/05/22 17:25:31  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.11  2004/04/08 15:33:08  wachter
 * GT04 checkin of Microsoft-Hellounds
 *
 * Revision 1.11  2004/03/25 15:12:59  nistico
 * Ball detection improved in MSH2004ImageProcessor
 * Ball distance measurement stabilized in MSH2004BallLocator (NOTE: to work with RIP, a small change has to be done in RIP code, just ask me...)
 * Fixed bugs with visualization of percepts (using intrinsic camera parameter based measurements)
 *
 * Revision 1.10  2004/02/10 10:48:04  nistico
 * Introduced Intrinsic camera parameters to perform geometric calculations (distance, angle, size...) without opening angle
 * Implemented radial distortion correction function
 * Implemented ball distance calculation based on size and intrinsic params (potentially more stable)
 * To Be Done: calculate intrinsic params for ERS7, as soon as we get our puppies back
 *
 * Revision 1.9  2003/12/29 20:57:56  roefer
 * Removed crash
 *
 * Revision 1.8  2003/12/18 14:55:24  schumann
 * removed crash that occured about every 10th boot:
 *   if(distanceFromCameraToBallCenter > translationOfCamera.z - ballRadius)
 *   {
 *     distanceFromCameraToBallCenterOnGround = sqrt(sqr(distanceFromCameraToBallCenter) - sqr(translationOfCamera.z  - ballRadius));
 * [...]
 * Case:
 * distanceFromCameraToBallCenter=10
 * translationOfCamera.z=0
 * ballRadius=40
 * condition is assumed as true
 * Problem:
 * distanceFromCameraToBallCenterOnGround = sqrt(sqr(10) - sqr(-42));
 * instruction tries to get a square root of a negative number ==> CRASH
 *
 * Revision 1.7  2003/12/15 11:46:13  juengel
 * Introduced CameraInfo
 *
 * Revision 1.6  2003/11/16 14:43:57  goehring
 * getBallDistance and Angle methods for size- and bearing  added
 *
 * Revision 1.5  2003/11/15 17:58:45  juengel
 * implemented getOffset
 *
 * Revision 1.4  2003/11/15 17:09:02  juengel
 * changed BallPercept
 *
 * Revision 1.3  2003/11/12 16:19:35  goehring
 * frameNumber added to percepts
 *
 * Revision 1.2  2003/11/05 16:34:28  goehring
 * FrameNumber added
 *
 * Revision 1.1  2003/10/07 10:09:35  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.1.1.1  2003/07/02 09:40:22  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.3  2003/04/16 06:58:06  roefer
 * Balls are sorted by size
 *
 * Revision 1.3  2003/04/12 06:29:34  roefer
 * Less balls and position improved
 *
 * Revision 1.2  2002/09/17 23:55:20  loetzsch
 * - unraveled several datatypes
 * - changed the WATCH macro
 * - completed the process restructuring
 *
 * Revision 1.1  2002/09/10 15:26:40  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed Challenge Code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.1.1.1  2002/05/10 12:40:13  cvsadm
 * Moved GT2002 Project from ute to tamara.
 *
 * Revision 1.9  2002/04/25 20:29:57  roefer
 * New BallPercept and BallP_osition, GTMath errors in SimGT2002 fixed
 *
 * Revision 1.8  2002/02/11 11:19:56  roefer
 * no message
 *
 * Revision 1.7  2002/02/11 11:13:06  roefer
 * BallPerceptor and BallLocator inserted
 *
 * Revision 1.6  2002/02/05 03:30:52  loetzsch
 * replaced direct member access by
 * inline const VALUE& get...() const   and
 * inline void set...(const Value&) methods.
 *
 * Revision 1.5  2002/01/22 00:06:55  loetzsch
 * In den Get...() Funktionen wurden die Parameter nicht als Referenz übergeben,
 * geändert
 *
 * Revision 1.4  2002/01/11 23:50:55  xiang
 * BallPercept von Hong & Lang
 *
 * Revision 1.3  2001/12/10 17:47:05  risler
 * change log added
 *
 */

---