#include <GT2004SelfLocator.h>
Inheritance diagram for GT2004SelfLocator:
Public Member Functions | |
GT2004SelfLocator (const SelfLocatorInterfaces &interfaces) | |
virtual void | execute () |
virtual bool | handleMessage (InMessage &message) |
Static Public Attributes | |
static double | paramUp = 0.01 |
static double | paramDown = 0.005 |
static double | paramDelay = 3 |
static double | paramHeight = 150 |
static double | paramZ = 10 |
static double | paramY = 2 |
static double | paramR = 2 |
static double | paramTrans = 100 |
static double | paramRot = 0.5 |
static double | paramOdometryOnly = 0.99 |
static double | paramPerceptDecay = 0.99 |
static double | paramPerceptChance = 0.15 |
Private Types | |
SAMPLES_MAX = 100 | |
GRID_MAX = 10 | |
FLAGS_MAX = 3 | |
GOALS_MAX = 1 | |
LEFT_SIDE_OF_FLAG = 1 | |
RIGHT_SIDE_OF_FLAG = -1 | |
enum | { SAMPLES_MAX = 100, GRID_MAX = 10, FLAGS_MAX = 3, GOALS_MAX = 1 } |
enum | FlagSides { LEFT_SIDE_OF_FLAG = 1, RIGHT_SIDE_OF_FLAG = -1 } |
Private Member Functions | |
double | sigmoid (double d) const |
void | updateByOdometry (const Pose2D &odometry, const Pose2D &camera, bool noise) |
void | updateFlagAndGoalBufferByOdometry (const LandmarksPercept &landmarksPercept, const Pose2D &odometry) |
void | updateByPoint (const LinesPercept::LinePoint &point, LinesPercept::LineType type) |
void | updateByFlag (const Vector2< double > &flag, FlagSides sideOfFlag, double measuredBearing, double measuredDistance) |
void | updateByGoalPost (const Vector2< double > &goalPost, double measuredBearing, double measuredDistance) |
void | resample () |
void | calcPose (Pose2D &pose, double &validity) |
void | addFlag (const Flag &flag, long frame) |
void | addGoal (const Goal &goal, long frame) |
bool | poseFromBearings (double dir0, double dir1, double dir2, const Vector2< double > &mark0, const Vector2< double > &mark1, const Vector2< double > &mark2, const Vector2< double > &cameraOffset, Pose2D &resultingPose) const |
int | poseFromBearingsAndDistance (double dir0, double dir1, double dist, const Vector2< double > &mark0, const Vector2< double > &mark1, const Vector2< double > &cameraOffset, Pose2D &resultingPose1, Pose2D &resultingPose2) const |
bool | getBearing (const LandmarksPercept &landmarksPercept, int i, Vector2< double > &mark, double &dir, double &dist) const |
void | generatePoseTemplates (const LandmarksPercept &landmarksPercept, const Pose2D &odometry) |
Sample | getTemplate () |
void | draw (const Pose2D &pose, Drawings::Color color) const |
void | draw (const Vector2< int > &point, LinesPercept::LineType type) const |
Private Attributes | |
DistanceToBorderEstimator | distanceToBorderEstimator |
SampleSet< Sample, SAMPLES_MAX > | sampleSet |
Pose2D | lastOdometry |
Pose2D | lastOdometry2 |
Pose2D | templates [SAMPLES_MAX] |
Flag | flags [FLAGS_MAX] |
long | flagsFrame [FLAGS_MAX] |
Goal | goals [GOALS_MAX] |
long | goalsFrame [GOALS_MAX] |
int | numOfFlags |
int | numOfGoals |
int | numOfTemplates |
int | nextTemplate |
int | randomFactor |
LinesPercept::LineType | types [LinesPercept::numberOfLineTypes] |
int | numberOfTypes |
bool | sensorUpdated |
double | average [Sample::numberOfQualities] |
unsigned | timeStamp |
double | speed |
Classes | |
class | Cell |
class | Sample |
Definition at line 28 of file GT2004SelfLocator.h.
anonymous enum [private] |
SAMPLES_MAX | The number of samples. |
GRID_MAX | The number of cells in one dimension. |
FLAGS_MAX | The size of the flag buffer. |
GOALS_MAX | The size of the goal buffer. |
Definition at line 86 of file GT2004SelfLocator.h.
enum GT2004SelfLocator::FlagSides [private] |
LEFT_SIDE_OF_FLAG | A marker for left edges of flags. |
RIGHT_SIDE_OF_FLAG | A marker for right edges of flags. |
Definition at line 94 of file GT2004SelfLocator.h.
GT2004SelfLocator::GT2004SelfLocator | ( | const SelfLocatorInterfaces & | interfaces | ) |
Constructor.
interfaces | The paramters of the SelfLocator module. |
Definition at line 107 of file GT2004SelfLocator.cpp.
References average, LinesTables2004::field, GT2004SelfLocator::Sample::numberOfQualities, numberOfTypes, numOfFlags, numOfGoals, Field::randomPose(), SAMPLES_MAX, sampleSet, sensorUpdated, speed, and timeStamp.
void GT2004SelfLocator::addFlag | ( | const Flag & | flag, | |
long | frame | |||
) | [private] |
The function adds a flag to the buffer.
flag | The new flag. |
Definition at line 815 of file GT2004SelfLocator.cpp.
References flags, FLAGS_MAX, flagsFrame, ConditionalBoundary::isOnBorder(), Range< T >::max, Range< T >::min, numOfFlags, Flag::type, and Boundary< T >::x.
Referenced by updateFlagAndGoalBufferByOdometry().
void GT2004SelfLocator::addGoal | ( | const Goal & | goal, | |
long | frame | |||
) | [private] |
Definition at line 837 of file GT2004SelfLocator.cpp.
References Goal::color, goals, GOALS_MAX, goalsFrame, ConditionalBoundary::isOnBorder(), Range< T >::max, Range< T >::min, numOfGoals, and Boundary< T >::x.
Referenced by updateFlagAndGoalBufferByOdometry().
void GT2004SelfLocator::calcPose | ( | Pose2D & | pose, | |
double & | validity | |||
) | [private] |
The function determines the most probable pose from the sample distribution.
pose | The pose is returned to this variable. | |
validity | The validity of the pose is returned to this variable. |
Definition at line 583 of file GT2004SelfLocator.cpp.
References Field::clip(), GT2004SelfLocator::Cell::count, LinesTables2004::field, GT2004SelfLocator::Cell::first, Range< T >::getSize(), GRID_MAX, log(), pi2, sampleSet, Pose2D::translation, Boundary< T >::x, and Boundary< T >::y.
void GT2004SelfLocator::draw | ( | const Vector2< int > & | point, | |
LinesPercept::LineType | type | |||
) | const [private] |
The function draws a point of a line percept.
point | The relative position in field coordinates. | |
type | The line type of the point. |
Definition at line 971 of file GT2004SelfLocator.cpp.
References Geometry::calculatePointInImage(), SelfLocatorInterfaces::cameraMatrix, CIRCLE, getRobotConfiguration(), Drawings::green, Drawings::ps_solid, Drawings::red, Drawings::skyblue, Vector2< V >::x, Vector2< V >::y, and Drawings::yellow.
void GT2004SelfLocator::draw | ( | const Pose2D & | pose, | |
Drawings::Color | color | |||
) | const [private] |
The function draws an arrow to a debug drawing.
pose | The position and direction of the arrow. | |
color | The color of the arrow. |
Definition at line 940 of file GT2004SelfLocator.cpp.
References LINE, Drawings::ps_solid, Pose2D::translation, Vector2< V >::x, and Vector2< V >::y.
Referenced by getTemplate(), and updateByPoint().
void GT2004SelfLocator::execute | ( | ) | [virtual] |
The function executes the module.
Implements Module.
Definition at line 126 of file GT2004SelfLocator.cpp.
References SelfLocatorInterfaces::cameraMatrix, LandmarksPercept::cameraOffset, distanceToBorderEstimator, e, DistanceToBorderEstimator::execute(), flags, flagsFrame, RobotPose::frameNumber, LandmarksPercept::frameNumber, generatePoseTemplates(), SystemCall::getCurrentSystemTime(), RobotPose::getPose(), SystemCall::getTimeSince(), RobotPose::getValidity(), goals, goalsFrame, CameraMatrix::isValid, SelfLocatorInterfaces::landmarksPercept, lastOdometry, lastOdometry2, LEFT_SIDE_OF_FLAG, SelfLocatorInterfaces::linesPercept, LinesPercept::numberOfPoints, numberOfTypes, numOfFlags, numOfGoals, SelfLocatorInterfaces::odometryData, paramOdometryOnly, paramPerceptChance, paramPerceptDecay, paramTrans, paramZ, LinesPercept::points, random(), resample(), RIGHT_SIDE_OF_FLAG, SelfLocatorInterfaces::robotPose, sensorUpdated, RobotPose::setFrameNumber(), RobotPose::setPose(), RobotPose::setValidity(), LinesPercept::skyblueGoal, speed, timeStamp, types, updateByFlag(), updateByGoalPost(), updateByOdometry(), updateByPoint(), updateFlagAndGoalBufferByOdometry(), USE_FLASHBACK, Vector3< V >::x, Vector3< V >::y, and LinesPercept::yellowGoal.
void GT2004SelfLocator::generatePoseTemplates | ( | const LandmarksPercept & | landmarksPercept, | |
const Pose2D & | odometry | |||
) | [private] |
The function generates pose templates from a landmark percept. The pose templates can be used to initialize new samples.
landmarksPercept | The landmarks percept. | |
odometry | The odometry offset since the last call of this function. |
Definition at line 859 of file GT2004SelfLocator.cpp.
References Player::blue, LandmarksPercept::cameraOffset, getBearing(), getPlayer(), SelfLocatorInterfaces::landmarksPercept, SelfLocatorInterfaces::linesPercept, nextTemplate, LandmarksPercept::numberOfGoals, LinesPercept::numberOfPoints, numberOfTypes, numOfFlags, numOfTemplates, LinesPercept::points, poseFromBearings(), poseFromBearingsAndDistance(), randomFactor, TemplateTable< TEMPLATES_MAX >::sample(), LinesPercept::skyblueGoal, templates, LinesTables2004::templateTable, types, Vector3< V >::x, Vector3< V >::y, and LinesPercept::yellowGoal.
Referenced by execute().
bool GT2004SelfLocator::getBearing | ( | const LandmarksPercept & | landmarksPercept, | |
int | i, | |||
Vector2< double > & | mark, | |||
double & | dir, | |||
double & | dist | |||
) | const [private] |
The function determines a pair of landmark positions and bearings from a landmarks percept.
landmarksPercept | The landmarks percept. | |
i | The index of the entry in the percept. | |
mark | The position of the mark is returned here. | |
dir | The bearing on the mark is returned to this variable. | |
dist | The distance of the mark is returned to this variable. |
Definition at line 780 of file GT2004SelfLocator.cpp.
References Flag::angle, Goal::distance, Flag::distance, flags, LandmarksPercept::goals, ConditionalBoundary::isOnBorder(), SelfLocatorInterfaces::landmarksPercept, Goal::leftPost, Range< T >::max, Range< T >::min, numOfFlags, Flag::position, Goal::rightPost, and Boundary< T >::x.
Referenced by generatePoseTemplates().
GT2004SelfLocator::Sample GT2004SelfLocator::getTemplate | ( | ) | [private] |
The function returns the next pose template or a random one if no templates were determined.
Definition at line 912 of file GT2004SelfLocator.cpp.
References COMPLEX_DRAWING, draw(), LinesTables2004::field, Pose2D::getAngle(), Boundary< T >::isInside(), nextTemplate, numOfTemplates, Pose2D::random(), randomFactor, Field::randomPose(), Drawings::red, templates, Pose2D::translation, Vector2< V >::x, and Vector2< V >::y.
bool GT2004SelfLocator::handleMessage | ( | InMessage & | message | ) | [virtual] |
Called from a MessageQueue to distribute messages. Use message.getMessageID to decide if the message is relavant for the MesssageHandler derivate. Use message.bin, message.text or message.config as In streams to get the data from.
message | The message that can be read. |
Reimplemented from Module.
Definition at line 991 of file GT2004SelfLocator.cpp.
References InMessage::bin, d, InMessage::getMessageID(), idLinesSelfLocatorParameters, paramDelay, paramDown, paramHeight, paramRot, paramTrans, paramUp, paramY, and paramZ.
bool GT2004SelfLocator::poseFromBearings | ( | double | dir0, | |
double | dir1, | |||
double | dir2, | |||
const Vector2< double > & | mark0, | |||
const Vector2< double > & | mark1, | |||
const Vector2< double > & | mark2, | |||
const Vector2< double > & | cameraOffset, | |||
Pose2D & | resultingPose | |||
) | const [private] |
The function calculates the current pose of the robot from three bearings.
dir0 | The bearing on the first landmark. | |
dir1 | The bearing on the second landmark. | |
dir2 | The bearing on the third landmark. | |
mark0 | The position of the first landmark. | |
mark1 | The position of the second landmark. | |
mark2 | The position of the third landmark. | |
cameraOffset | The offset of the camera relative to the body center. | |
resultingPose | The calculated pose is returned to this variable. |
Definition at line 702 of file GT2004SelfLocator.cpp.
References Vector2< V >::abs(), b, pi, pi2, Vector2< V >::x, and Vector2< V >::y.
Referenced by generatePoseTemplates().
int GT2004SelfLocator::poseFromBearingsAndDistance | ( | double | dir0, | |
double | dir1, | |||
double | dist, | |||
const Vector2< double > & | mark0, | |||
const Vector2< double > & | mark1, | |||
const Vector2< double > & | cameraOffset, | |||
Pose2D & | resultingPose1, | |||
Pose2D & | resultingPose2 | |||
) | const [private] |
The function calculates the up to two current poses of the robot from two bearings and a distance.
dir0 | The bearing on the first landmark. | |
dir1 | The bearing on the second landmark. | |
dist | The distance of the first landmark. | |
mark0 | The position of the first landmark. | |
mark1 | The position of the second landmark. | |
cameraOffset | The offset of the camera relative to the body center. | |
resultingPose1 | One calculated pose is returned to this variable. | |
resultingPose2 | A second calculated pose is returned to this variable. |
Definition at line 740 of file GT2004SelfLocator.cpp.
References Vector2< V >::abs(), dist(), pi, pi_2, Vector2< V >::x, and Vector2< V >::y.
Referenced by generatePoseTemplates().
void GT2004SelfLocator::resample | ( | ) | [private] |
The function re-distributes the samples according to their probabilities.
Definition at line 517 of file GT2004SelfLocator.cpp.
References average, GT2004SelfLocator::Sample::numberOfQualities, SAMPLES_MAX, and sampleSet.
Referenced by execute().
double GT2004SelfLocator::sigmoid | ( | double | d | ) | const [inline, private] |
The function distributes the parameter in a Gaussian way.
d | A value that should be distributed. |
Definition at line 124 of file GT2004SelfLocator.h.
Referenced by updateByFlag(), updateByGoalPost(), and updateByPoint().
void GT2004SelfLocator::updateByFlag | ( | const Vector2< double > & | flag, | |
FlagSides | sideOfFlag, | |||
double | measuredBearing, | |||
double | measuredDistance | |||
) | [private] |
The function updates the samples by a single edge of a flag recognized.
flag | The position of the flag. | |
sideOfFlag | The side of the flag that was seen. | |
measuredBearing | The bearing, under which the edge was seen. |
Definition at line 465 of file GT2004SelfLocator.cpp.
References Vector2< V >::abs(), LinesPercept::numberOfLineTypes, pi, SAMPLES_MAX, sampleSet, sensorUpdated, sigmoid(), Pose2D::translation, USE_BEARING_TO_LANDMARK, USE_DISTANCE_TO_LANDMARK, Vector2< V >::x, and Vector2< V >::y.
Referenced by execute().
void GT2004SelfLocator::updateByGoalPost | ( | const Vector2< double > & | goalPost, | |
double | measuredBearing, | |||
double | measuredDistance | |||
) | [private] |
The function updates the samples by a single goal post recognized.
goalPost | The position of the goal post. | |
measuredBearing | The bearing, under which the goal post was seen. |
Definition at line 491 of file GT2004SelfLocator.cpp.
References Vector2< V >::abs(), LinesPercept::numberOfLineTypes, pi, SAMPLES_MAX, sampleSet, sensorUpdated, sigmoid(), Pose2D::translation, USE_BEARING_TO_LANDMARK, USE_DISTANCE_TO_LANDMARK, Vector2< V >::x, and Vector2< V >::y.
Referenced by execute().
void GT2004SelfLocator::updateByOdometry | ( | const Pose2D & | odometry, | |
const Pose2D & | camera, | |||
bool | noise | |||
) | [private] |
The function updates the samples by the odometry offset.
odometry | The motion since the last call to this function. | |
camera | The camera offset. | |
noise | Dermines whether some additional noise is added to the sample poses. |
Definition at line 387 of file GT2004SelfLocator.cpp.
References Vector2< V >::abs(), dist(), fmax(), Pose2D::getAngle(), max, random(), SAMPLES_MAX, sampleSet, Pose2D::translation, Vector2< V >::x, and Vector2< V >::y.
Referenced by execute().
void GT2004SelfLocator::updateByPoint | ( | const LinesPercept::LinePoint & | point, | |
LinesPercept::LineType | type | |||
) | [private] |
The function updates the samples by a single line point recognized.
point | The relative offset of the point. | |
type | The type of the point. |
Definition at line 431 of file GT2004SelfLocator.cpp.
References Vector2< V >::abs(), Vector2< V >::angle(), Player::blue, COMPLEX_DRAWING, d, draw(), LinesPercept::field, Pose2D::getAngle(), ObservationTable< xSize, ySize, cellSize >::getClosestPoint(), getPlayer(), LinesPercept::numberOfLineTypes, LinesTables2004::observationTable, paramHeight, paramY, paramZ, pi, SAMPLES_MAX, sampleSet, sigmoid(), LinesPercept::skyblueGoal, Pose2D::translation, Vector2< V >::x, Vector2< V >::y, and LinesPercept::yellowGoal.
Referenced by execute().
void GT2004SelfLocator::updateFlagAndGoalBufferByOdometry | ( | const LandmarksPercept & | landmarksPercept, | |
const Pose2D & | odometry | |||
) | [private] |
The function updates the flag buffer by the odometry offset and adds new flags
Definition at line 316 of file GT2004SelfLocator.cpp.
References Vector2< V >::abs(), addFlag(), addGoal(), Goal::angle, Flag::angle, DIV1200_IGNORE_IF_SMALLER, DIV20_IGNORE_IF_SMALLER, DIV400_IGNORE_IF_SMALLER, LandmarksPercept::flags, flags, LandmarksPercept::frameNumber, LandmarksPercept::goals, goals, SelfLocatorInterfaces::landmarksPercept, LandmarksPercept::numberOfFlags, LandmarksPercept::numberOfGoals, numOfFlags, numOfGoals, Vector2< V >::x, and Vector2< V >::y.
Referenced by execute().
double GT2004SelfLocator::average[Sample::numberOfQualities] [private] |
Definition at line 115 of file GT2004SelfLocator.h.
Referenced by GT2004SelfLocator(), and resample().
A class that can estimate the distance to the border
Definition at line 32 of file GT2004SelfLocator.h.
Referenced by execute().
Flag GT2004SelfLocator::flags[FLAGS_MAX] [private] |
A buffer for previously seen flags.
Definition at line 104 of file GT2004SelfLocator.h.
Referenced by addFlag(), execute(), getBearing(), and updateFlagAndGoalBufferByOdometry().
long GT2004SelfLocator::flagsFrame[FLAGS_MAX] [private] |
A buffer for previously seen flags, containing the framenumber
Definition at line 105 of file GT2004SelfLocator.h.
Goal GT2004SelfLocator::goals[GOALS_MAX] [private] |
A buffer for previously seen goals.
Definition at line 106 of file GT2004SelfLocator.h.
Referenced by addGoal(), execute(), and updateFlagAndGoalBufferByOdometry().
long GT2004SelfLocator::goalsFrame[GOALS_MAX] [private] |
A buffer for previously seen goals, containing the framenumber
Definition at line 107 of file GT2004SelfLocator.h.
Pose2D GT2004SelfLocator::lastOdometry [private] |
The state of the odometry at the previous call of this module.
Definition at line 101 of file GT2004SelfLocator.h.
Referenced by execute().
Pose2D GT2004SelfLocator::lastOdometry2 [private] |
int GT2004SelfLocator::nextTemplate [private] |
The next template delivered.
Definition at line 108 of file GT2004SelfLocator.h.
Referenced by generatePoseTemplates(), and getTemplate().
int GT2004SelfLocator::numberOfTypes [private] |
Definition at line 113 of file GT2004SelfLocator.h.
Referenced by execute(), generatePoseTemplates(), and GT2004SelfLocator().
int GT2004SelfLocator::numOfFlags [private] |
Definition at line 108 of file GT2004SelfLocator.h.
Referenced by addFlag(), execute(), generatePoseTemplates(), getBearing(), GT2004SelfLocator(), and updateFlagAndGoalBufferByOdometry().
int GT2004SelfLocator::numOfGoals [private] |
The number of flags in the buffer.
Definition at line 108 of file GT2004SelfLocator.h.
Referenced by addGoal(), execute(), GT2004SelfLocator(), and updateFlagAndGoalBufferByOdometry().
int GT2004SelfLocator::numOfTemplates [private] |
The number of templates generated.
Definition at line 108 of file GT2004SelfLocator.h.
Referenced by generatePoseTemplates(), and getTemplate().
double GT2004SelfLocator::paramDelay = 3 [static] |
Definition at line 282 of file GT2004SelfLocator.h.
Referenced by handleMessage(), GT2004SelfLocator::Sample::setProbability(), and GT2004SelfLocator::Sample::updateQuality().
double GT2004SelfLocator::paramDown = 0.005 [static] |
Definition at line 282 of file GT2004SelfLocator.h.
Referenced by handleMessage(), and GT2004SelfLocator::Sample::setProbability().
double GT2004SelfLocator::paramHeight = 150 [static] |
Definition at line 282 of file GT2004SelfLocator.h.
Referenced by handleMessage(), and updateByPoint().
double GT2004SelfLocator::paramOdometryOnly = 0.99 [static] |
double GT2004SelfLocator::paramPerceptChance = 0.15 [static] |
double GT2004SelfLocator::paramPerceptDecay = 0.99 [static] |
double GT2004SelfLocator::paramR = 2 [static] |
Definition at line 282 of file GT2004SelfLocator.h.
double GT2004SelfLocator::paramRot = 0.5 [static] |
double GT2004SelfLocator::paramTrans = 100 [static] |
double GT2004SelfLocator::paramUp = 0.01 [static] |
Definition at line 282 of file GT2004SelfLocator.h.
Referenced by handleMessage(), GT2004SelfLocator::Sample::setProbability(), and GT2004SelfLocator::Sample::updateQuality().
double GT2004SelfLocator::paramY = 2 [static] |
Definition at line 282 of file GT2004SelfLocator.h.
Referenced by handleMessage(), and updateByPoint().
double GT2004SelfLocator::paramZ = 10 [static] |
Definition at line 282 of file GT2004SelfLocator.h.
Referenced by execute(), handleMessage(), and updateByPoint().
int GT2004SelfLocator::randomFactor [private] |
A factor that is increased if more templates are required.
Definition at line 111 of file GT2004SelfLocator.h.
Referenced by generatePoseTemplates(), and getTemplate().
SampleSet<Sample, SAMPLES_MAX> GT2004SelfLocator::sampleSet [private] |
The sample set.
Definition at line 100 of file GT2004SelfLocator.h.
Referenced by calcPose(), GT2004SelfLocator(), resample(), updateByFlag(), updateByGoalPost(), updateByOdometry(), and updateByPoint().
bool GT2004SelfLocator::sensorUpdated [private] |
Did any update of the samples by a sensor reading happen?
Definition at line 114 of file GT2004SelfLocator.h.
Referenced by execute(), GT2004SelfLocator(), updateByFlag(), and updateByGoalPost().
double GT2004SelfLocator::speed [private] |
Definition at line 117 of file GT2004SelfLocator.h.
Referenced by execute(), and GT2004SelfLocator().
Pose2D GT2004SelfLocator::templates[SAMPLES_MAX] [private] |
Templates for poses replacing bad samples.
Definition at line 101 of file GT2004SelfLocator.h.
Referenced by generatePoseTemplates(), and getTemplate().
unsigned GT2004SelfLocator::timeStamp [private] |
Definition at line 116 of file GT2004SelfLocator.h.
Referenced by execute(), and GT2004SelfLocator().
LinesPercept::LineType GT2004SelfLocator::types[LinesPercept::numberOfLineTypes] [private] |
Definition at line 112 of file GT2004SelfLocator.h.
Referenced by execute(), and generatePoseTemplates().