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

# Representations/Perception/BallPercept.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file BallPercept.cpp
    00003 *
    00004 * Implementation of class BallPercept.
    00005 *
    00006 * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00007 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
    00008 * @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
    00009 */
    00010
    00011 #include "BallPercept.h"
    00012 #include <math.h>
    00013
    00014 #include "Tools/Debugging/Debugging.h"
    00015
    00016 #include "Tools/Streams/StreamHandler.h"
    00017 #include "Representations/Perception/Image.h"
    00018
    00019 BallPercept::BallPercept()
    00020 {
    00021   reset(0);
    00022 }
    00023
    00024 void BallPercept::add(const Vector2<double>& offsetOnField,
    00025                       const Vector2<double>& bearingBasedOffsetOnField,
    00026                       const Vector2<double>& sizeBasedOffsetOnField,
    00027                       const Vector2<double>& centerInImage,
    00028                       double radiusInImage,
    00029                       double reliability,
    00030                       bool isCameraMatrixValid)
    00031 {
    00032   ballWasSeen = true;
    00033   this->offsetOnField = offsetOnField;
    00034   this->bearingBasedOffsetOnField = bearingBasedOffsetOnField;
    00035   this->sizeBasedOffsetOnField = sizeBasedOffsetOnField;
    00036   this->centerInImage = centerInImage;
    00037   this->radiusInImage = radiusInImage;
    00038   this->reliability = reliability;
    00039   this->isCameraMatrixValid = isCameraMatrixValid;
    00040 }
    00041
    00042 void BallPercept::addHighRes(const Vector2<int>& centerInPixel,
    00043                              const Vector2<double>& centerAsAngles,
    00044                              double radiusInPixel,
    00045                              double radiusAsAngle,
    00046                              const Vector3<double>& translationOfCamera,
    00047                              bool isCameraMatrixValid,
    00048                              double reliability)
    00049 {
    00050   if (!ballWasSeen || this->reliability < reliability)
    00051   {
    00052     Vector2<double> bbo; // bearing based
    00053     double distanceBearingBased = centerAsAngles.y < 0 ? (translationOfCamera.z - ballRadius) / tan(-centerAsAngles.y) : 10000;
    00054     bbo.x = translationOfCamera.x + distanceBearingBased * cos(centerAsAngles.x);
    00055     bbo.y = translationOfCamera.y + distanceBearingBased * sin(centerAsAngles.x);
    00056     checkOffset(bbo);
    00057
    00058     Vector2<double> sbo; // size based
    00059     double distanceFromCameraToBallCenter =
    00060       Geometry::getBallDistanceByAngleSize(int(2 * ballRadius), 2 * radiusAsAngle);
    00061     double distanceFromCameraToBallCenterOnGround;
    00062     if(distanceFromCameraToBallCenter > fabs(translationOfCamera.z - ballRadius))
    00063       distanceFromCameraToBallCenterOnGround = sqrt(sqr(distanceFromCameraToBallCenter) - sqr(translationOfCamera.z  - ballRadius));
    00064     else
    00065       distanceFromCameraToBallCenterOnGround = 0;
    00066     sbo.x = translationOfCamera.x + distanceFromCameraToBallCenterOnGround * cos(centerAsAngles.x);
    00067     sbo.y = translationOfCamera.y + distanceFromCameraToBallCenterOnGround * sin(centerAsAngles.x);
    00068     checkOffset(sbo);
    00069
    00070     Vector2<double> o; // offset
    00071     double sbdist = sbo.abs();
    00072     if(!isCameraMatrixValid || centerAsAngles.y >= 0 || sbdist > 300.0)
    00073       o = sbo;
    00074     else if(sbdist > 200.0)
    00075     {
    00076       //interpolate between both offsets
    00077       double factor = (300.0 - sbdist) / 100.0;
    00078       o = bbo * factor + sbo * (1.0 - factor);
    00079     }
    00080     else
    00081       o = bbo;
    00082
    00083     add(o, bbo, sbo, Vector2<double>(centerInPixel.x, centerInPixel.y) / 2.0, radiusInPixel / 2.0, reliability, isCameraMatrixValid);
    00084   }
    00085 }
    00086
    00087 /*void BallPercept::getOffsetIntrinsic(Vector2<double>& offset) const
    00088 {
    00089   double distanceSizeBased = Geometry::getDistanceBySize(
    00090     cameraInfo,
    00091     (int) ballRadius,
    00092     radiusInImage,
    00093     centerInImage.x,
    00094     centerInImage.y
    00095     );
    00096   //~ offset.x = translationOfCamera.x + distanceSizeBased * cos(anglesToCenter.x);
    00097   //~ offset.y = translationOfCamera.y + distanceSizeBased * sin(anglesToCenter.x);
    00098   double distanceFromCameraToBallCenterOnGround;
    00099
    00100   if(distanceSizeBased > fabs(translationOfCamera.z - ballRadius))
    00101   {
    00102     distanceFromCameraToBallCenterOnGround = sqrt(sqr(distanceSizeBased) - sqr(translationOfCamera.z  - ballRadius));
    00103   }
    00104   else
    00105   {
    00106     distanceFromCameraToBallCenterOnGround = 0;
    00107   }
    00108
    00109   offset.x = translationOfCamera.x + distanceFromCameraToBallCenterOnGround * cos(anglesToCenter.x);
    00110   offset.y = translationOfCamera.y + distanceFromCameraToBallCenterOnGround * sin(anglesToCenter.x);
    00111   checkOffset(offset);
    00112 }
    00113 */
    00114
    00115 void BallPercept::checkOffset(Vector2<double>& offset) const
    00116 {
    00117   if(offset.x < ballRadius / 2 && fabs(offset.y) < getRobotConfiguration().getRobotDimensions().lowerBodyWidth / 2 + ballRadius)
    00118     offset.x = ballRadius / 2;
    00119 }