#include <GaussBell.h>
Public Member Functions | |
GaussBell () | |
~GaussBell () | |
Matrix2x2< double > | getCovarianceMatrix () |
Vector2< double > | getPositionOfMaximum () |
double | getValidity (Vector2< double > _position) |
double | getValidityAtPositionOfMaximum () |
unsigned int | getTimeStamp () |
void | getSigmas (double &sigmaMaj, double &sigmaMin) |
double | getAngle () |
void | setCovarianceMatrix (double _validity, double _angle) |
void | setCovarianceMatrix (double _validity, double _deltaX, double _deltaY) |
void | setCovarianceMatrix (Matrix2x2< double > _covarianceMatrix) |
void | setCovarianceMatrix (double _sigmaMaj, double _sigmaMin, double _deltaX, double _deltaY) |
void | setPositionOfMaximum (Vector2< double > _positionOfMaximum) |
void | setRotationAngle (double _rotationAngle) |
void | setValidity (double _validity) |
void | setSigmas (double _sigmaMaj, double _sigmaMin) |
void | setTimeStamp (unsigned long _timeStampGaussBell) |
void | setTimeStamp () |
void | updateValidity (double _validity) |
void | mergeBells (GaussBell _gaussBell1, GaussBell _gaussBell2) |
Private Member Functions | |
void | transformSigmaToValidity () |
void | transformValidityToSigma () |
void | updateSigmasAndAngle () |
void | updateCovarianceMatrix () |
Private Attributes | |
Matrix2x2< double > | covarianceMatrix |
Vector2< double > | position |
double | sigmaMaj |
double | sigmaMin |
double | rotationAngle |
double | validity |
unsigned long | timeStampGaussBell |
Static Private Attributes | |
static const int | phi [330] |
static const int | invProb [1000] |
static const double | radiusMaj = 300 |
static const double | radiusMin = 150 |
static const double | factor = 1.0/1.78 |
Definition at line 23 of file GaussBell.h.
GaussBell::GaussBell | ( | ) |
Constructor.
standard initialization
Definition at line 188 of file GaussBell.cpp.
References SystemCall::getCurrentSystemTime(), position, rotationAngle, setTimeStamp(), sigmaMaj, sigmaMin, validity, Vector2< V >::x, and Vector2< V >::y.
GaussBell::~GaussBell | ( | ) |
Definition at line 202 of file GaussBell.cpp.
double GaussBell::getAngle | ( | ) |
Matrix2x2< double > GaussBell::getCovarianceMatrix | ( | ) |
Returns the covariance matrix of the Gaussbell.
Definition at line 210 of file GaussBell.cpp.
References covarianceMatrix.
Vector2< double > GaussBell::getPositionOfMaximum | ( | ) |
Returns the position of the GaussBell-Maximum
Definition at line 219 of file GaussBell.cpp.
References position.
void GaussBell::getSigmas | ( | double & | sigmaMaj, | |
double & | sigmaMin | |||
) |
unsigned int GaussBell::getTimeStamp | ( | ) |
Returns the time stamp
Definition at line 249 of file GaussBell.cpp.
References timeStampGaussBell.
Referenced by mergeBells().
double GaussBell::getValidity | ( | Vector2< double > | _position | ) |
Returns the validity-value at the position given in the argument
Definition at line 228 of file GaussBell.cpp.
References validity.
double GaussBell::getValidityAtPositionOfMaximum | ( | ) |
merges the bells _gaussBell1 and _gaussBell2
Definition at line 258 of file GaussBell.cpp.
References covarianceMatrix, SystemCall::getCurrentSystemTime(), getTimeStamp(), invert(), position, timeStampGaussBell, transformSigmaToValidity(), and updateSigmasAndAngle().
void GaussBell::setCovarianceMatrix | ( | double | _sigmaMaj, | |
double | _sigmaMin, | |||
double | _deltaX, | |||
double | _deltaY | |||
) |
Sets the covariance-matrix (sigmas and validity will be correctly set, too)
Definition at line 339 of file GaussBell.cpp.
References rotationAngle, sigmaMaj, sigmaMin, sqr, transformSigmaToValidity(), and updateCovarianceMatrix().
void GaussBell::setCovarianceMatrix | ( | Matrix2x2< double > | _covarianceMatrix | ) |
Sets the covarianceMatrix by another Matrix
Definition at line 293 of file GaussBell.cpp.
References covarianceMatrix, transformSigmaToValidity(), and updateSigmasAndAngle().
void GaussBell::setCovarianceMatrix | ( | double | _validity, | |
double | _deltaX, | |||
double | _deltaY | |||
) |
like the previous method aber faster!
Definition at line 316 of file GaussBell.cpp.
References rotationAngle, sqr, transformValidityToSigma(), updateCovarianceMatrix(), and validity.
void GaussBell::setCovarianceMatrix | ( | double | _validity, | |
double | _angle | |||
) |
Sets the covarianceMatrix by the percepts of a player
Definition at line 304 of file GaussBell.cpp.
References rotationAngle, transformValidityToSigma(), updateCovarianceMatrix(), and validity.
void GaussBell::setPositionOfMaximum | ( | Vector2< double > | _positionOfMaximum | ) |
when the sigmas are given but no validity This method will be used for the SensorFusionPlayersLocator Sets the position of one object one robot has percepted
Definition at line 361 of file GaussBell.cpp.
References position.
void GaussBell::setRotationAngle | ( | double | _rotationAngle | ) |
Sets the rotation-angle and updates the covariance-matrix
Definition at line 370 of file GaussBell.cpp.
References rotationAngle, and updateCovarianceMatrix().
void GaussBell::setSigmas | ( | double | _sigmaMaj, | |
double | _sigmaMin | |||
) |
Sets the sigma-values and updates the covariance-matrix and the validity
Definition at line 398 of file GaussBell.cpp.
References sigmaMaj, sigmaMin, transformSigmaToValidity(), and updateCovarianceMatrix().
void GaussBell::setTimeStamp | ( | ) |
set the time stamp to the actual system time
Definition at line 389 of file GaussBell.cpp.
References SystemCall::getCurrentSystemTime(), and timeStampGaussBell.
Referenced by GaussBell().
void GaussBell::setTimeStamp | ( | unsigned long | _timeStampGaussBell | ) |
timeStamp = _timeStampGaussBell
Definition at line 380 of file GaussBell.cpp.
References timeStampGaussBell.
void GaussBell::setValidity | ( | double | _validity | ) |
validity = _validity Error-correction included
Definition at line 281 of file GaussBell.cpp.
References transformValidityToSigma(), updateCovarianceMatrix(), and validity.
void GaussBell::transformSigmaToValidity | ( | ) | [private] |
Calculates the validity from the sigma-values
Definition at line 440 of file GaussBell.cpp.
References int(), phi, radiusMaj, radiusMin, sigmaMaj, sigmaMin, and validity.
Referenced by mergeBells(), setCovarianceMatrix(), and setSigmas().
void GaussBell::transformValidityToSigma | ( | ) | [private] |
calculates sigmaMaj and sigmaMin when only the validity is given
Definition at line 425 of file GaussBell.cpp.
References factor, int(), invProb, radiusMaj, radiusMin, sigmaMaj, sigmaMin, and validity.
Referenced by setCovarianceMatrix(), setValidity(), and updateValidity().
void GaussBell::updateCovarianceMatrix | ( | ) | [private] |
calculates the CovarianceMatrix from sigmaMaj, sigmaMin and the rotationAngle
Definition at line 489 of file GaussBell.cpp.
References Matrix2x2< V >::c, covarianceMatrix, rotationAngle, sigmaMaj, sigmaMin, Matrix2x2< V >::transpose(), Vector2< V >::x, and Vector2< V >::y.
Referenced by setCovarianceMatrix(), setRotationAngle(), setSigmas(), setValidity(), and updateValidity().
void GaussBell::updateSigmasAndAngle | ( | ) | [private] |
calculates sigmaMaj, sigamMin and rotationAngle form the covariance matrix
Definition at line 512 of file GaussBell.cpp.
References Matrix2x2< V >::c, covarianceMatrix, pi_4, rotationAngle, sigmaMaj, sigmaMin, Matrix2x2< V >::transpose(), Vector2< V >::x, and Vector2< V >::y.
Referenced by mergeBells(), and setCovarianceMatrix().
void GaussBell::updateValidity | ( | double | _validity | ) |
if a older percept should be merged the validity has to be decreased. This will be used in SensorFusionBallLocator.
Definition at line 411 of file GaussBell.cpp.
References transformValidityToSigma(), updateCovarianceMatrix(), and validity.
Matrix2x2<double> GaussBell::covarianceMatrix [private] |
Definition at line 70 of file GaussBell.h.
Referenced by getCovarianceMatrix(), mergeBells(), setCovarianceMatrix(), updateCovarianceMatrix(), and updateSigmasAndAngle().
const double GaussBell::factor = 1.0/1.78 [static, private] |
const int GaussBell::invProb [static, private] |
inverted probability table of the normalized gauss distribution (index: factor 1000, array: factor 100)
Definition at line 79 of file GaussBell.h.
Referenced by transformValidityToSigma().
const int GaussBell::phi [static, private] |
phi-table of the normalized gauss distribution (index: factor 100, array: factor 10000)
Definition at line 78 of file GaussBell.h.
Referenced by transformSigmaToValidity().
Vector2<double> GaussBell::position [private] |
Definition at line 71 of file GaussBell.h.
Referenced by GaussBell(), getPositionOfMaximum(), mergeBells(), and setPositionOfMaximum().
const double GaussBell::radiusMaj = 300 [static, private] |
radius in maj-direction of the validity-ellipse. the validity is the probability that the real position lies within this ellipe
Definition at line 80 of file GaussBell.h.
Referenced by transformSigmaToValidity(), and transformValidityToSigma().
const double GaussBell::radiusMin = 150 [static, private] |
radius in min-direction of the validity-ellipse
Definition at line 81 of file GaussBell.h.
Referenced by transformSigmaToValidity(), and transformValidityToSigma().
double GaussBell::rotationAngle [private] |
Definition at line 75 of file GaussBell.h.
Referenced by GaussBell(), getAngle(), setCovarianceMatrix(), setRotationAngle(), updateCovarianceMatrix(), and updateSigmasAndAngle().
double GaussBell::sigmaMaj [private] |
variance in maj. direction
Definition at line 73 of file GaussBell.h.
Referenced by GaussBell(), getSigmas(), setCovarianceMatrix(), setSigmas(), transformSigmaToValidity(), transformValidityToSigma(), updateCovarianceMatrix(), and updateSigmasAndAngle().
double GaussBell::sigmaMin [private] |
variance in min. direction
Definition at line 73 of file GaussBell.h.
Referenced by GaussBell(), getSigmas(), setCovarianceMatrix(), setSigmas(), transformSigmaToValidity(), transformValidityToSigma(), updateCovarianceMatrix(), and updateSigmasAndAngle().
unsigned long GaussBell::timeStampGaussBell [private] |
The time when this GaussBell was active. This attribut will be needed for merging history-Data into the fusion.
Definition at line 89 of file GaussBell.h.
Referenced by getTimeStamp(), mergeBells(), and setTimeStamp().
double GaussBell::validity [private] |
the validity of the percept
Definition at line 76 of file GaussBell.h.
Referenced by GaussBell(), getValidity(), getValidityAtPositionOfMaximum(), setCovarianceMatrix(), setValidity(), transformSigmaToValidity(), transformValidityToSigma(), and updateValidity().