Tools/Math/GaussBell.cpp

Go to the documentation of this file.

/**
 * @file Tools/Math/GaussBell.cpp
 * This class describes a gaussian normal distributen to use with the 
 * gaussBellTeamBallLocator with the ability to merge 2 different gaussbells
 *
 * @author <a href=mailto:Kai_Engel@gmx.de>Kai Engel</a>
 */

#include "GaussBell.h"
#include "Tools/FieldDimensions.h"
#include "Platform/SystemCall.h"
#include <math.h>
#include "Platform/File.h"


const double GaussBell::factor = 1.0/1.78;

const int GaussBell::phi[] = {
    5000, 5040, 5080, 5120, 5160, 5199, 5239, 5279, 5319, 5359,
    5398, 5438, 5478, 5517, 5557, 5596, 5636, 5675, 5714, 5753,
    5793, 5832, 5871, 5910, 5948, 5987, 6026, 6064, 6103, 6141, 
    6179, 6217, 6255, 6293, 6331, 6368, 6406, 6443, 6480, 6517, 
    6554, 6591, 6628, 6664, 6700, 6736, 6772, 6808, 6844, 6879, 
   
    6915, 6950, 6985, 7019, 7054, 7088, 7123, 7157, 7190, 7224,
    7257, 7291, 7324, 7357, 7389, 7422, 7454, 7486, 7517, 7549, 
    7580, 7611, 7642, 7673, 7704, 7734, 7764, 7794, 7823, 7852, 
    7881, 7910, 7939, 7967, 7995, 8023, 8051, 8078, 8106, 8133, 
    8159, 8186, 8212, 8238, 8264, 8289, 8315, 8340, 8365, 8389, 
    
    8413, 8438, 8461, 8485, 8508, 8531, 8554, 8577, 8599, 8621, 
    8643, 8665, 8686, 8708, 8729, 8749, 8770, 8790, 8810, 8830, 
    8849, 8869, 8888, 8907, 8925, 8944, 8962, 8980, 8997, 9015,
    9032, 9049, 9066, 9082, 9099, 9115, 9131, 9147, 9162, 9177, 
    9192, 9207, 9222, 9236, 9251, 9265, 9279, 9292, 9306, 9319, 
    
    9332, 9345, 9357, 9370, 9382, 9394, 9406, 9418, 9429, 9441, 
    9452, 9463, 9474, 9484, 9495, 9505, 9515, 9525, 9535, 9545, 
    9554, 9564, 9573, 9582, 9591, 9599, 9608, 9616, 9625, 9633, 
    9641, 9649, 9656, 9664, 9671, 9678, 9686, 9693, 9699, 9706, 
    9713, 9719, 9726, 9732, 9738, 9744, 9750, 9756, 9761, 9767,
    
    9772, 9778, 9783, 9788, 9793, 9798, 9803, 9808, 9812, 9817, 
    9821, 9826, 9830, 9834, 9838, 9842, 9846, 9850, 9854, 9857,
    9861, 9864, 9868, 9871, 9875, 9878, 9881, 9884, 9887, 9890, 
    9893, 9896, 9898, 9901, 9904, 9906, 9909, 9911, 9913, 9916,
    9918, 9920, 9922, 9925, 9927, 9929, 9931, 9932, 9934, 9936, 
    
    9938, 9940, 9941, 9943, 9945, 9946, 9948, 9949, 9951, 9952, 
    9953, 9955, 9956, 9957, 9959, 9960, 9961, 9962, 9963, 9964, 
    9965, 9966, 9967, 9968, 9969, 9970, 9971, 9972, 9973, 9974, 
    9974, 9975, 9976, 9977, 9977, 9978, 9979, 9979, 9980, 9981, 
    9981, 9982, 9982, 9983, 9984, 9984, 9985, 9985, 9986, 9986,
    
    9986, 9987, 9987, 9988, 9988, 9989, 9989, 9989, 9990, 9990,
    9990, 9991, 9991, 9991, 9992, 9992, 9992, 9992, 9993, 9993,
    9993, 9993, 9994, 9994, 9994, 9994, 9994, 9995, 9995, 9995};
     
    const int GaussBell::invProb[] = {
     
       1,   1,   1,   1,   1,   1,   1,   1,   1,   2,
       2,   2,   2,   2,   2,   2,   2,   3,   3,   3,
       3,   3,   3,   3,   3,   4,   4,   4,   4,   4,
       4,   4,   4,   5,   5,   5,   5,   5,   5,   5,
       6,   6,   6,   6,   6,   6,   6,   6,   7,   7,

       7,   7,   7,   7,   7,   7,   8,   8,   8,   8,
       8,   8,   8,   8,   9,   9,   9,   9,   9,   9,
       9,   9,  10,  10,  10,  10,  10,  10,  10,  10,
      11,  11,  11,  11,  11,  11,  11,  11,  12,  12,
      12,  12,  12,  12,  12,  12,  13,  13,  13,  13,

      13,  13,  13,  13,  14,  14,  14,  14,  14,  14,
      14,  14,  15,  15,  15,  15,  15,  15,  15,  15,
      16,  16,  16,  16,  16,  16,  16,  16,  17,  17,
      17,  17,  17,  17,  17,  18,  18,  18,  18,  18, 
      18,  18,  18,  19,  19,  19,  19,  19,  19,  19, 

      19,  20,  20,  20,  20,  20,  20,  20,  20,  21, 
      21,  21,  21,  21,  21,  21,  21,  22,  22,  22, 
      22,  22,  22,  22,  22,  23,  23,  23,  23,  23, 
      23,  23,  24,  24,  24,  24,  24,  24,  24,  24, 
      25,  25,  25,  25,  25,  25,  25,  25,  26,  26, 

      26,  26,  26,  26,  26,  26,  27,  27,  27,  27, 
      27,  27,  27,  28,  28,  28,  28,  28,  28,  28, 
      28,  29,  29,  29,  29,  29,  29,  29,  29,  30, 
      30,  30,  30,  30,  30,  30,  31,  31,  31,  31, 
      31,  31,  31,  31,  32,  32,  32,  32,  32,  32, 
    
      32,  33,  33,  33,  33,  33,  33,  33,  33,  34, 
      34,  34,  34,  34,  34,  34,  34,  35,  35,  35, 
      35,  35,  35,  35,  36,  36,  36,  36,  36,  36, 
      36,  36,  37,  37,  37,  37,  37,  37,  37,  38, 
      38,  38,  38,  38,  38,  38,  39,  39,  39,  39, 
    
      39,  39,  39,  39,  40,  40,  40,  40,  40,  40, 
      40,  41,  41,  41,  41,  41,  41,  41,  41,  42, 
      42,  42,  42,  42,  42,  42,  43,  43,  43,  43, 
      43,  43,  43,  44,  44,  44,  44,  44,  44,  44, 
      45,  45,  45,  45,  45,  45,  45,  45,  46,  46, 
    
      46,  46,  46,  46,  46,  47,  47,  47,  47,  47, 
      47,  47,  48,  48,  48,  48,  48,  48,  48,  49, 
      49,  49,  49,  49,  49,  49,  50,  50,  50,  50, 
      50,  50,  50,  51,  51,  51,  51,  51,  51,  51, 
      52,  52,  52,  52,  52,  52,  52,  53,  53,  53, 
    
      53,  53,  53,  53,  54,  54,  54,  54,  54,  54, 
      54,  55,  55,  55,  55,  55,  55,  55,  56,  56, 
      56,  56,  56,  56,  56,  57,  57,  57,  57,  57, 
      57,  57,  58,  58,  58,  58,  58,  58,  59,  59, 
      59,  59,  59,  59,  59,  60,  60,  60,  60,  60, 
    
      60,  60,  61,  61,  61,  61,  61,  61,  61,  62, 
      62,  62,  62,  62,  62,  63,  63,  63,  63,  63, 
      63,  63,  64,  64,  64,  64,  64,  64,  65,  65, 
      65,  65,  65,  65,  65,  66,  66,  66,  66,  66, 
      66,  67,  67,  67,  67,  67,  67,  67,  68,  68, 
    
      68,  68,  68,  68,  69,  69,  69,  69,  69,  69, 
      70,  70,  70,  70,  70,  70,  71,  71,  71,  71, 
      71,  71,  71,  72,  72,  72,  72,  72,  72,  73, 
      73,  73,  73,  73,  73,  74,  74,  74,  74,  74, 
      74,  75,  75,  75,  75,  75,  75,  76,  76,  76, 
    
      76,  76,  76,  77,  77,  77,  77,  77,  77,  78, 
      78,  78,  78,  78,  78,  79,  79,  79,  79,  79, 
      79,  80,  80,  80,  80,  80,  80,  81,  81,  81, 
      81,  81,  82,  82,  82,  82,  82,  82,  83,  83, 
      83,  83,  83,  83,  84,  84,  84,  84,  84,  85, 
    
      85,  85,  85,  85,  85,  86,  86,  86,  86,  86, 
      86,  87,  87,  87,  87,  87,  88,  88,  88,  88, 
      88,  88,  89,  89,  89,  89,  89,  90,  90,  90, 
      90,  90,  91,  91,  91,  91,  91,  91,  92,  92, 
      92,  92,  92,  93,  93,  93,  93,  93,  94,  94, 
    
      94,  94,  94,  95,  95,  95,  95,  95,  96,  96, 
      96,  96,  96,  97,  97,  97,  97,  97,  98,  98, 
      98,  98,  98,  99,  99,  99,  99,  99, 100, 100,
      100, 100, 100, 101, 101, 101, 101, 101, 102, 102,
      102, 102, 102, 103, 103, 103, 103, 104, 104, 104,
    
      104, 104, 105, 105, 105, 105, 105, 106, 106, 106,
      106, 107, 107, 107, 107, 107, 108, 108, 108, 108,
      109, 109, 109, 109, 109, 110, 110, 110, 110, 111,
      111, 111, 111, 112, 112, 112, 112, 112, 113, 113,
      113, 113, 114, 114, 114, 114, 115, 115, 115, 115,
    
      116, 116, 116, 116, 117, 117, 117, 117, 118, 118,
      118, 118, 119, 119, 119, 119, 120, 120, 120, 120,
      121, 121, 121, 121, 122, 122, 122, 122, 123, 123,
      123, 123, 124, 124, 124, 125, 125, 125, 125, 126,
      126, 126, 126, 127, 127, 127, 128, 128, 128, 128,
    
      129, 129, 129, 130, 130, 130, 130, 131, 131, 131,
      132, 132, 132, 132, 133, 133, 133, 134, 134, 134,
      135, 135, 135, 136, 136, 136, 136, 137, 137, 137,
      138, 138, 138, 139, 139, 139, 140, 140, 140, 141,
      141, 141, 142, 142, 142, 143, 143, 143, 144, 144,
    
      144, 145, 145, 146, 146, 146, 147, 147, 147, 148,
      148, 148, 149, 149, 150, 150, 150, 151, 151, 152,
      152, 152, 153, 153, 154, 154, 154, 155, 155, 156,
      156, 156, 157, 157, 158, 158, 159, 159, 159, 160,
      160, 161, 161, 162, 162, 163, 163, 164, 164, 165,
    
      165, 166, 166, 167, 167, 168, 168, 169, 169, 170,
      170, 171, 171, 172, 172, 173, 173, 174, 174, 175,
      176, 176, 177, 177, 178, 179, 179, 180, 180, 181,
      182, 182, 183, 184, 184, 185, 186, 186, 187, 188,
      189, 189, 190, 191, 192, 192, 193, 194, 195, 196,
    
      197, 197, 198, 199, 200, 201, 202, 203, 204, 205,
      206, 207, 208, 209, 210, 211, 213, 214, 215, 216,
      218, 219, 220, 222, 223, 225, 226, 228, 230, 231,
      233, 235, 237, 239, 242, 244, 246, 249, 252, 255,
      258, 262, 266, 271, 276, 282, 289, 298, 311, 329
    
    };

const double GaussBell::radiusMaj = 300;
const double GaussBell::radiusMin = 150;


// Constructor:
GaussBell::GaussBell()
{
  /** standard initialization */
  this->setTimeStamp(SystemCall::getCurrentSystemTime());
  position.x = 0.0;
  position.y = 0.0;
  rotationAngle = 0.0;
  sigmaMaj = 0.0;
  sigmaMin = 0.0;
  validity = 0.0;
}


// Destructor:
GaussBell::~GaussBell()
{
}


/**
  * Returns the Cavariance-Matrix.
  */  
Matrix2x2<double> GaussBell::getCovarianceMatrix()
{
  return covarianceMatrix;
}


/**
  * Returns the position with the maximum validity.
  */ 
Vector2<double> GaussBell::getPositionOfMaximum()
{
  return position;
}


/**
  * Returns the validity.
  */ 
double GaussBell::getValidity(Vector2<double> _position)
{
  return validity;
}


/**
  * Returns the validity.
  */ 
double GaussBell::getValidityAtPositionOfMaximum()
{
  // Validity :  I define the validity here as the probabilty that
  //             the  Zufallsvariable X (eg. the ball) is in the a given
  //             radius around the  Erwartungswert of the normal distribution
    return validity;
}


/**
  * Returns the time stamp
  */ 
unsigned int GaussBell::getTimeStamp()
{
  return this->timeStampGaussBell;
}


/**
  * Two gauss-bells will be operated. Result is a gauss-bell, 
  */
void GaussBell::mergeBells(GaussBell _gaussBell1, GaussBell _gaussBell2)
{
  // Merging of the Covariance-Matrices:
  this->covarianceMatrix = _gaussBell1.covarianceMatrix - _gaussBell1.covarianceMatrix
                 * ((_gaussBell1.covarianceMatrix + _gaussBell2.covarianceMatrix).invert())
                 * _gaussBell1.covarianceMatrix;
  this->position = _gaussBell1.position + _gaussBell1.covarianceMatrix 
                 * ((_gaussBell1.covarianceMatrix + _gaussBell2.covarianceMatrix).invert())
                 * (_gaussBell2.position - _gaussBell1.position);


  this->timeStampGaussBell = SystemCall::getCurrentSystemTime();
  this->timeStampGaussBell = this->getTimeStamp();

  this->updateSigmasAndAngle();
  this->transformSigmaToValidity();
}


/**
  * validity = _validity
  * Error-correction included
  */
void GaussBell::setValidity(double _validity)
{
  this->validity = _validity;
  if (validity >= 1.0 ) validity = 0.9;
  this->transformValidityToSigma();// calculate new SigmaMin / SigmaMai
  this->updateCovarianceMatrix();  // calculate new covariance-Matrix
}


/**
  * Sets the covariance-matrix (sigmas and validity will be correctly set, too)
  */
void GaussBell::setCovarianceMatrix(Matrix2x2<double> _covarianceMatrix)
{
  this->covarianceMatrix = _covarianceMatrix;
  this->updateSigmasAndAngle();
  this->transformSigmaToValidity();
}


/**
  * Sets the covariance-matrix (sigmas and validity will be correctly set, too)
  */
void GaussBell::setCovarianceMatrix(double _validity, double _angle)
{ 
  this->validity = _validity;
  this->rotationAngle = _angle;
  this->transformValidityToSigma(); // creates sigmaMaj and sigmaMin
  this->updateCovarianceMatrix();
}


/**
  * Sets the covariance-matrix (sigmas and validity will be correctly set, too)
  */
void GaussBell::setCovarianceMatrix(double _validity, double _deltaX, double _deltaY)
{ 
  double diagonal = sqrt(sqr(_deltaX) + sqr(_deltaY)); // Pythagoras
  
  if (diagonal > 0.00001) 
  {
     this->rotationAngle = asin(_deltaX / diagonal);
  }
  else
  {
      this->rotationAngle = 0;
  }

  validity = _validity;
  transformValidityToSigma(); // creates sigmaMaj and sigmaMin
  updateCovarianceMatrix();

}


/**
  * Sets the covariance-matrix (sigmas and validity will be correctly set, too)
  */
void GaussBell::setCovarianceMatrix(double _sigmaMaj, double _sigmaMin, double _deltaX, double _deltaY)
{ 
  double diagonal = sqrt(sqr(_deltaX) + sqr(_deltaY)); // Pythagoras
  if (diagonal > 0.00001) 
  {
      rotationAngle = asin(_deltaX / diagonal);
  }
  else
  {
      rotationAngle = 0;
  }
  
  sigmaMaj = _sigmaMaj;
  sigmaMin = _sigmaMin;
  transformSigmaToValidity();
  updateCovarianceMatrix(); 
}


/** 
  * position = _positionOfMaximum
  */
void GaussBell::setPositionOfMaximum(Vector2<double> _positionOfMaximum)
{
  position = _positionOfMaximum;
}


/** 
  * Sets the rotation-angle and updates the covariance-matrix
  */
void GaussBell::setRotationAngle(double _rotationAngle)
{
  rotationAngle = _rotationAngle;
  updateCovarianceMatrix();
}


/** 
  * timeStamp = _timeStampGaussBell
  */
void GaussBell::setTimeStamp(unsigned long _timeStampGaussBell)
{
  timeStampGaussBell = _timeStampGaussBell;
}


/** 
  * Sets a dafault time-stamp (current system time)
  */
void GaussBell::setTimeStamp()
{
  timeStampGaussBell =  SystemCall::getCurrentSystemTime();
}


/** 
  * Sets the sigma-values and updates the covariance-matrix and the validity
  */
void GaussBell::setSigmas(double _sigmaMaj, double _sigmaMin)
{
  sigmaMaj = _sigmaMaj;
  sigmaMin = _sigmaMin;

  updateCovarianceMatrix();
  transformSigmaToValidity();
}


/** 
  * similar to setValidity
  */
void GaussBell::updateValidity(double _validity)
{
  this->validity = _validity;
  if (validity >= 1.0) 
    validity = 0.99;

  transformValidityToSigma();
  updateCovarianceMatrix();
}


/** 
  * Calculates the sigma-values from the validity
  */
void GaussBell::transformValidityToSigma()
{
  if (validity == 1) 
  {
    validity = 0.999; // validity == 1 is not possibel, because the bell would be a peac.
  }
  double factor = invProb[(int)(sqrt(validity)*1000)] / 100.0;
  sigmaMaj = radiusMaj / factor;
  sigmaMin = radiusMin / factor;
}


/** 
  * Calculates the validity from the sigma-values
  */
void GaussBell::transformSigmaToValidity()
{
  double index1 = 329;
  double index2 = 329;
  if (sigmaMaj > 0.5) {
    index1 = (radiusMaj / sigmaMaj) * 100.0;
  }
  if (sigmaMin > 0.5) {
    index2 = (radiusMin / sigmaMin) * 100.0;
  }

  if (index1>329) {
    index1=329;
  } else {
    if (index1 < 0) {
      index1 = 0;
    }
  }
  if (index2>329) {
    index2=329;
  } else {
    if (index2 < 0) {
      index2 = 0;
    }
  }
  validity = (2 * phi[ int(index1) ] / 10000.0 - 1) 
               *
             (2 * phi[ int(index2) ] / 10000.0 - 1);
}


/**
  * Returns the angle of the gauss bell. (angle between x-axis and major-axis of bell).
  */ 
double GaussBell::getAngle() 
{
     return rotationAngle;
}


void GaussBell::getSigmas(double& SigmaMaj, double& SigmaMin) {
  SigmaMaj = this->sigmaMaj;
  SigmaMin = this->sigmaMin;  
}


/**
  * Update the Covariance-Matrix if the rotationAngel or the sigmas changed.
  */ 
void GaussBell::updateCovarianceMatrix()
{
  // Update the Covariance-Matrix if the rotationAngel or the sigmas changed

  Matrix2x2<double> rotationMatrix;

  rotationMatrix.c[0].x =  rotationMatrix.c[1].y = cos(-rotationAngle);
  rotationMatrix.c[0].y =  sin(-rotationAngle);
  rotationMatrix.c[1].x =  -rotationMatrix.c[0].y;  
 
  this->covarianceMatrix.c[0].x = (this->sigmaMaj)*(this->sigmaMaj);
  this->covarianceMatrix.c[1].y = (this->sigmaMin)*(this->sigmaMin);
  this->covarianceMatrix.c[0].y = 0.0;
  this->covarianceMatrix.c[1].x = 0.0;
 
  // C_new = R(-TETA)trans*C_old*R(-TETA):
  this->covarianceMatrix = (rotationMatrix.transpose()) * this->covarianceMatrix * rotationMatrix; 
}


/** 
  * Updates the sigmas and the angle
  */
void GaussBell::updateSigmasAndAngle()
{
  
  if ((covarianceMatrix.c[0].x-covarianceMatrix.c[1].y) > 0.0000001)
    rotationAngle = (atan(2*covarianceMatrix.c[1].x/(covarianceMatrix.c[0].x-covarianceMatrix.c[1].y)) / 2.0);
  else 
  {
    if (covarianceMatrix.c[0].y < 0.0)
      rotationAngle = -pi_4;   // -45 degree
    else rotationAngle = pi_4; //  45 degree
  }
  
  Matrix2x2<double> rotationMatrix;

  rotationMatrix.c[0].x = cos(rotationAngle);
  rotationMatrix.c[0].y = sin(rotationAngle);
  rotationMatrix.c[1].x = - rotationMatrix.c[0].y;
  rotationMatrix.c[1].y = rotationMatrix.c[0].x;

  Matrix2x2<double> tmp = 
         (rotationMatrix.transpose()) * covarianceMatrix * rotationMatrix;
  sigmaMaj = sqrt(tmp.c[0].x);
  sigmaMin = sqrt(tmp.c[1].y);
}



/*
 * Changelog:
 * 
 * $Log: GaussBell.cpp,v $
 * Revision 1.1.1.1  2004/05/22 17:37:03  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.3  2004/01/28 08:28:02  dueffert
 * beautified
 *
 * Revision 1.2  2003/11/17 15:15:20  dueffert
 * doxygen docu corrected
 *
 * Revision 1.1  2003/10/07 10:13:24  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.2  2003/09/28 09:28:48  juengel
 * Comments corrected.
 *
 * Revision 1.1  2003/09/26 11:40:40  juengel
 * - sorted tools
 * - clean-up in DataTypes
 *
 * Revision 1.1.1.1  2003/07/02 09:40:28  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.2  2003/05/19 15:35:01  wachter
 * Moved genration of debug-drawing
 *
 * Revision 1.1  2003/05/19 09:17:30  wachter
 * Renamed SensorFusionGaussBell to GaussBell as requested.
 *
 * Revision 1.32  2003/05/05 12:02:00  dueffert
 * platform independence restored
 *
 * Revision 1.31  2003/04/25 16:09:41  engel
 * Comments inserted
 *
 * Revision 1.30  2003/04/19 03:09:28  pruente
 * Changes by Sebastian S.: -Bugfix and avoidance of phantoms
 *
 * Revision 1.30  2003/04/10 16:32:44  schmidt
 * Bugfix and avoidance of phantoms
 *
 * Revision 1.29  2003/04/07 17:03:18  thiel
 * Catch wrong values causing a crash
 *
 * Revision 1.28  2003/04/06 15:57:52  engel
 * Little changes (cosmetic and some improvements).
 *
 * Revision 1.27  2003/04/01 16:12:11  schmidt
 * Bugfix
 *
 * Revision 1.26  2003/04/01 15:57:52  wachter
 * Fix of transformValidityToSigma
 *
 * Revision 1.25  2003/04/01 15:16:26  engel
 * if-case befor a division to fix a possibel division by zero
 *
 * Revision 1.24  2003/04/01 10:35:09  schmidt
 * Removed outcommented code line
 *
 * Revision 1.23  2003/04/01 09:18:33  schumann
 * fixed division by zero
 *
 * Revision 1.22  2003/04/01 08:06:22  engel
 * Adaption of InvProb[]-Table.
 * Division by Zero fixed...
 *
 * Revision 1.21  2003/03/31 14:03:06  wachter
 * Cleanup of SensorFusionGaussBell,
 * added "real" Validity to SensorFusionGaussBell
 *
 * Revision 1.20  2003/03/27 16:17:23  engel
 * Invalid BallPercepepts will be destroyed or repaired...
 *
 * Revision 1.19  2003/03/26 15:36:00  engel
 * Body for calculateValidityOfBall(...) inserted.
 * Changes for test-text-files.
 * Calculation of sigmas from Validity.
 *
 * Revision 1.18  2003/03/21 15:11:52  engel
 * fixed crash for dx=dy=0
 * inserted getValitityAtPositionOfMaximum()
 *
 * Revision 1.17  2003/03/20 18:28:16  engel
 * Bug in Hisrtory fixed. The Value (0,0) is NOT an angle.
 *
 * Revision 1.16  2003/03/18 13:40:53  wachter
 * Bugfixes
 *
 * Revision 1.15  2003/03/14 16:04:46  wachter
 * Bugfixes
 *
 * Revision 1.14  2003/03/11 11:34:30  engel
 * Debug-changes
 *
 * Revision 1.13  2003/03/06 16:22:11  wachter
 * Divsion by zero fixed.
 *
 * Revision 1.12  2003/03/06 11:33:05  engel
 * Debug-Changes
 *
 * Revision 1.11  2003/03/05 16:44:37  engel
 * Factor set to 1.0/3.0 (again)
 *
 * Revision 1.10  2003/03/05 16:20:57  engel
 * Initialization changed.
 * If one ball was seen with validity 0 it will not be merged!
 * Still working on...
 *
 * Revision 1.9  2003/03/04 16:44:07  engel
 * still working on (testing)
 *
 * Revision 1.8  2003/03/04 11:55:04  engel
 * still working on...
 *
 * Revision 1.7  2003/02/28 16:29:26  engel
 * still working on ;-))
 *
 * Revision 1.6  2003/02/28 12:08:35  engel
 * Still working on ;-)
 *
 * Revision 1.5  2003/02/26 19:08:37  roefer
 * warnings removed
 *
 * Revision 1.4  2003/02/25 16:49:57  engel
 * some changes to calculate the validity (mean-Value)
 *
 * Revision 1.3  2003/02/21 16:34:16  dueffert
 * common pi in own code, warnings removed, platform compatibility restored
 *
 * Revision 1.2  2003/02/21 12:21:14  engel
 * Now the angles PI/4 and -PI/4 can be calculated.
 * The angles are allways given in [0, 2PI].ö
 *
 * Revision 1.1  2003/02/14 14:34:02  wachter
 * Added SensorFusionBallLocator
 *
 */

---