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

GT2004SelfLocator Class Reference

The class implements a lines-based Monte Carlo self-localization. More...

#include <GT2004SelfLocator.h>

Inheritance diagram for GT2004SelfLocator:

Inheritance graph
[legend]
Collaboration diagram for GT2004SelfLocator:

Collaboration graph
[legend]
List of all members.

Public Member Functions

 GT2004SelfLocator (const SelfLocatorInterfaces &interfaces)
 Constructor.

virtual void execute ()
 The function executes the module.

virtual bool handleMessage (InMessage &message)
 Called from a MessageQueue to distribute messages.


Static Public Attributes

double paramUp
double paramDown
double paramDelay
double paramHeight
double paramZ
double paramY
double paramR
double paramTrans
double paramRot

Private Types

enum  { SAMPLES_MAX = 100, GRID_MAX = 10, FLAGS_MAX = 3 }
enum  FlagSides { LEFT_SIDE_OF_FLAG = 1, RIGHT_SIDE_OF_FLAG = -1 }

Private Member Functions

double sigmoid (double d) const
 The function distributes the parameter in a Gaussian way.

void updateByOdometry (const Pose2D &odometry, const Pose2D &camera, bool noise)
 The function updates the samples by the odometry offset.

void updateByPoint (const LinesPercept::LinePoint &point, LinesPercept::LineType type)
 The function updates the samples by a single line point recognized.

void updateByFlag (const Vector2< double > &flag, FlagSides sideOfFlag, double measuredBearing)
 The function updates the samples by a single edge of a flag recognized.

void updateByGoalPost (const Vector2< double > &goalPost, double measuredBearing)
 The function updates the samples by a single goal post recognized.

void resample ()
 The function re-distributes the samples according to their probabilities.

void calcPose (Pose2D &pose, double &validity)
 The function determines the most probable pose from the sample distribution.

void addFlag (const Flag &flag)
 The function adds a flag to the buffer.

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
 The function calculates the current pose of the robot from three bearings.

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
 The function calculates the up to two current poses of the robot from two bearings and a distance.

bool getBearing (const LandmarksPercept &landmarksPercept, int i, Vector2< double > &mark, double &dir, double &dist) const
 The function determines a pair of landmark positions and bearings from a landmarks percept.

void generatePoseTemplates (const LandmarksPercept &landmarksPercept, const Pose2D &odometry)
 The function generates pose templates from a landmark percept.

Sample getTemplate ()
 The function returns the next pose template or a random one if no templates were determined.

void draw (const Pose2D &pose, Drawings::Color color) const
 The function draws an arrow to a debug drawing.

void draw (const Vector2< int > &point, LinesPercept::LineType type) const
 The function draws a point of a line percept.


Private Attributes

DistanceToBorderEstimator distanceToBorderEstimator
 A class that can estimate the distance to the border.

SampleSet< Sample, SAMPLES_MAX > sampleSet
 The sample set.

Pose2D lastOdometry
 The state of the odometry at the previous call of this module.

Pose2D lastOdometry2
 The state of the odometry at the previous call of this module.

Pose2D templates [SAMPLES_MAX]
 Templates for poses replacing bad samples.

Flag flags [FLAGS_MAX]
 A buffer for previously seen flags.

int numOfFlags
 The number of flags in the buffer.

int numOfTemplates
 The number of templates generated.

int nextTemplate
 The next template delivered.

int randomFactor
 A factor that is increased if more templates are required.

LinesPercept::LineType types [LinesPercept::numberOfLineTypes]
int numberOfTypes
bool sensorUpdated
 Did any update of the samples by a sensor reading happen?

double average [Sample::numberOfQualities]
unsigned timeStamp
double speed

Detailed Description

The class implements a lines-based Monte Carlo self-localization.

Definition at line 21 of file GT2004SelfLocator.h.

Member Enumeration Documentation

anonymous enum [private]
 

Enumeration values:
SAMPLES_MAX  The number of samples.
GRID_MAX  The number of cells in one dimension.
FLAGS_MAX  The size of the flag buffer.

Definition at line 79 of file GT2004SelfLocator.h.

enum GT2004SelfLocator::FlagSides [private]
 

Enumeration values:
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 86 of file GT2004SelfLocator.h.

Constructor & Destructor Documentation

GT2004SelfLocator::GT2004SelfLocator (  const SelfLocatorInterfaces &  interfaces  ) 
 

Constructor.

Parameters:
interfaces The paramters of the SelfLocator module.

Definition at line 87 of file GT2004SelfLocator.cpp.

References average, distanceToBorderEstimator, numOfFlags, Field::randomPose(), SAMPLES_MAX, sampleSet, and sensorUpdated.

Here is the call graph for this function:

Member Function Documentation

double GT2004SelfLocator::sigmoid (  double  d  )  const [inline, private]
 

The function distributes the parameter in a Gaussian way.

Parameters:
d A value that should be distributed.
Returns:
A transformation of d according to a Gaussian curve.

Definition at line 113 of file GT2004SelfLocator.h.

Referenced by updateByFlag(), updateByGoalPost(), and updateByPoint().

void GT2004SelfLocator::updateByOdometry (  const Pose2D &  odometry,
const Pose2D &  camera,
bool  noise
)  [private]
 

The function updates the samples by the odometry offset.

Parameters:
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 214 of file GT2004SelfLocator.cpp.

References Vector2< double >::abs(), GT2004SelfLocator::Sample::camera, fmax(), Pose2D::getAngle(), max, PoseSample::probability, random(), SAMPLES_MAX, sampleSet, Pose2D::translation, Vector2< double >::x, and Vector2< double >::y.

Referenced by execute().

Here is the call graph for this function:

void GT2004SelfLocator::updateByPoint (  const LinesPercept::LinePoint &  point,
LinesPercept::LineType  type
)  [private]
 

The function updates the samples by a single line point recognized.

Parameters:
point The relative offset of the point.
type The type of the point.

Definition at line 258 of file GT2004SelfLocator.cpp.

References Vector2< V >::abs(), Vector2< int >::abs(), Vector2< int >::angle(), COMPLEX_DRAWING, draw(), Pose2D::getAngle(), ObservationTable< 280, 200, 25 >::getClosestPoint(), getPlayer(), Player::getTeamColor(), paramHeight, paramY, paramZ, pi, point, SAMPLES_MAX, sampleSet, GT2004SelfLocator::Sample::setProbability(), sigmoid(), Pose2D::translation, Vector2< V >::x, Vector2< int >::x, Vector2< V >::y, and Vector2< int >::y.

Referenced by execute().

Here is the call graph for this function:

void GT2004SelfLocator::updateByFlag (  const Vector2< double > &  flag,
FlagSides  sideOfFlag,
double  measuredBearing
)  [private]
 

The function updates the samples by a single edge of a flag recognized.

Parameters:
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 292 of file GT2004SelfLocator.cpp.

References Vector2< double >::abs(), GT2004SelfLocator::Sample::camera, pi, SAMPLES_MAX, sampleSet, sensorUpdated, GT2004SelfLocator::Sample::setProbability(), sigmoid(), Pose2D::translation, Vector2< double >::x, Vector2< V >::x, Vector2< double >::y, and Vector2< V >::y.

Referenced by execute().

Here is the call graph for this function:

void GT2004SelfLocator::updateByGoalPost (  const Vector2< double > &  goalPost,
double  measuredBearing
)  [private]
 

The function updates the samples by a single goal post recognized.

Parameters:
goalPost The position of the goal post.
measuredBearing The bearing, under which the goal post was seen.

Definition at line 310 of file GT2004SelfLocator.cpp.

References GT2004SelfLocator::Sample::camera, pi, SAMPLES_MAX, sampleSet, sensorUpdated, GT2004SelfLocator::Sample::setProbability(), sigmoid(), Pose2D::translation, Vector2< double >::x, Vector2< V >::x, Vector2< double >::y, and Vector2< V >::y.

Referenced by execute().

Here is the call graph for this function:

void GT2004SelfLocator::resample (   )  [private]
 

The function re-distributes the samples according to their probabilities.

Definition at line 327 of file GT2004SelfLocator.cpp.

References average, COMPLEX_DRAWING, draw(), GT2004SelfLocator::Sample::getQuality(), getTemplate(), numOfTemplates, GT2004SelfLocator::Sample::quality, random(), SAMPLES_MAX, sampleSet, and SampleSet< Sample, SAMPLES_MAX >::swap().

Referenced by execute().

Here is the call graph for this function:

void GT2004SelfLocator::calcPose (  Pose2D &  pose,
double &  validity
)  [private]
 

The function determines the most probable pose from the sample distribution.

Parameters:
pose The pose is returned to this variable.
validity The validity of the pose is returned to this variable.

Definition at line 393 of file GT2004SelfLocator.cpp.

References Field::clip(), GT2004SelfLocator::Cell::count, GT2004SelfLocator::Cell::first, Pose2D::getAngle(), Pose2D::getCos(), GT2004SelfLocator::Sample::getQuality(), Pose2D::getSin(), Range< double >::getSize(), GRID_MAX, GT2004SelfLocator::Sample::isValid(), GT2004SelfLocator::Sample::next, pi2, SAMPLES_MAX, sampleSet, Pose2D::translation, Vector2< double >::x, Boundary< double >::x, Vector2< double >::y, and Boundary< double >::y.

Referenced by execute().

Here is the call graph for this function:

void GT2004SelfLocator::addFlag (  const Flag &  flag  )  [private]
 

The function adds a flag to the buffer.

Parameters:
flag The new flag.

Definition at line 611 of file GT2004SelfLocator.cpp.

References FLAGS_MAX, ConditionalBoundary::isOnBorder(), Range< double >::max, Range< double >::min, numOfFlags, Flag::type, and Boundary< double >::x.

Referenced by generatePoseTemplates().

Here is the call graph for this function:

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.

Parameters:
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.
Returns:
Was the function successful?

Definition at line 498 of file GT2004SelfLocator.cpp.

References Vector2< V >::abs(), pi, pi2, Vector2< V >::x, and Vector2< V >::y.

Referenced by generatePoseTemplates().

Here is the call graph for this function:

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.

Parameters:
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.
Returns:
The number of poses calculated?

Definition at line 536 of file GT2004SelfLocator.cpp.

References Vector2< V >::abs(), pi, pi_2, Vector2< V >::x, and Vector2< V >::y.

Referenced by generatePoseTemplates().

Here is the call graph for this function:

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.

Parameters:
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.
Returns:
returns true if getting the bearing was successful.

Definition at line 576 of file GT2004SelfLocator.cpp.

References Flag::angle, Goal::distance, Flag::distance, LandmarksPercept::goals, ConditionalBoundary::isOnBorder(), Goal::leftPost, Range< double >::max, Range< double >::min, numOfFlags, Flag::position, Goal::rightPost, and Boundary< double >::x.

Referenced by generatePoseTemplates().

Here is the call graph for this function:

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.

Parameters:
landmarksPercept The landmarks percept.
odometry The odometry offset since the last call of this function.

Definition at line 630 of file GT2004SelfLocator.cpp.

References Vector2< int >::abs(), Vector2< V >::abs(), addFlag(), Flag::angle, LandmarksPercept::cameraOffset, Flag::distance, LandmarksPercept::flags, getBearing(), getPlayer(), Player::getTeamColor(), nextTemplate, LandmarksPercept::numberOfFlags, LandmarksPercept::numberOfGoals, LinesPercept::numberOfPoints, numOfFlags, numOfTemplates, point, LinesPercept::points, poseFromBearings(), poseFromBearingsAndDistance(), randomFactor, TemplateTable< 50000 >::sample(), SAMPLES_MAX, templates, Vector2< int >::x, Vector3< double >::x, Vector2< V >::x, Vector2< int >::y, Vector3< double >::y, and Vector2< V >::y.

Referenced by execute().

Here is the call graph for this function:

GT2004SelfLocator::Sample GT2004SelfLocator::getTemplate (   )  [private]
 

The function returns the next pose template or a random one if no templates were determined.

Returns:
A new sample.

Definition at line 693 of file GT2004SelfLocator.cpp.

References average, COMPLEX_DRAWING, draw(), Pose2D::getAngle(), Boundary< double >::isInside(), nextTemplate, numOfTemplates, Pose2D::random(), randomFactor, Field::randomPose(), templates, Pose2D::translation, Vector2< double >::x, and Vector2< double >::y.

Referenced by resample().

Here is the call graph for this function:

void GT2004SelfLocator::draw (  const Pose2D &  pose,
Drawings::Color  color
)  const [private]
 

The function draws an arrow to a debug drawing.

Parameters:
pose The position and direction of the arrow.
color The color of the arrow.

Definition at line 721 of file GT2004SelfLocator.cpp.

References LINE, Pose2D::translation, Vector2< double >::x, and Vector2< double >::y.

Referenced by getTemplate(), resample(), and updateByPoint().

void GT2004SelfLocator::draw (  const Vector2< int > &  point,
LinesPercept::LineType  type
)  const [private]
 

The function draws a point of a line percept.

Parameters:
point The relative position in field coordinates.
type The line type of the point.

Definition at line 752 of file GT2004SelfLocator.cpp.

References Geometry::calculatePointInImage(), CIRCLE, getRobotConfiguration(), point, Vector2< V >::x, and Vector2< V >::y.

Here is the call graph for this function:

void GT2004SelfLocator::execute (   )  [virtual]
 

The function executes the module.

Implements Module.

Definition at line 103 of file GT2004SelfLocator.cpp.

References calcPose(), LandmarksPercept::cameraOffset, DEBUG_DRAWING_FINISHED, distanceToBorderEstimator, e, DistanceToBorderEstimator::execute(), LandmarksPercept::flags, LandmarksPercept::frameNumber, generatePoseTemplates(), SystemCall::getCurrentSystemTime(), SystemCall::getTimeSince(), LandmarksPercept::goals, ConditionalBoundary::isOnBorder(), CameraMatrix::isValid, lastOdometry2, LEFT_SIDE_OF_FLAG, Goal::leftPost, SampleSet< Sample, SAMPLES_MAX >::link(), Range< double >::max, Range< double >::min, LandmarksPercept::numberOfFlags, LandmarksPercept::numberOfGoals, LinesPercept::numberOfPoints, paramTrans, paramZ, LinesPercept::points, Flag::position, random(), resample(), RIGHT_SIDE_OF_FLAG, Goal::rightPost, sampleSet, sensorUpdated, RobotPose::setFrameNumber(), RobotPose::setPose(), RobotPose::setValidity(), LandmarksState::update(), updateByFlag(), updateByGoalPost(), updateByOdometry(), updateByPoint(), Boundary< double >::x, Vector3< double >::x, and Vector3< double >::y.

Here is the call graph for this function:

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.

Parameters:
message The message that can be read.
Returns:
true if the message was read (handled).

Reimplemented from Module.

Definition at line 772 of file GT2004SelfLocator.cpp.

References InMessage::bin, GenericDebugData::data, InMessage::getMessageID(), idLinesSelfLocatorParameters, paramDelay, paramDown, paramHeight, paramRot, paramTrans, paramUp, paramY, and paramZ.

Here is the call graph for this function:

Member Data Documentation

DistanceToBorderEstimator GT2004SelfLocator::distanceToBorderEstimator [private]
 

A class that can estimate the distance to the border.

Definition at line 25 of file GT2004SelfLocator.h.

Referenced by execute(), and GT2004SelfLocator().

SampleSet<Sample, SAMPLES_MAX> GT2004SelfLocator::sampleSet [private]
 

The sample set.

Definition at line 92 of file GT2004SelfLocator.h.

Referenced by calcPose(), execute(), GT2004SelfLocator(), resample(), updateByFlag(), updateByGoalPost(), updateByOdometry(), and updateByPoint().

Pose2D GT2004SelfLocator::lastOdometry [private]
 

The state of the odometry at the previous call of this module.

Definition at line 93 of file GT2004SelfLocator.h.

Pose2D GT2004SelfLocator::lastOdometry2 [private]
 

The state of the odometry at the previous call of this module.

Definition at line 93 of file GT2004SelfLocator.h.

Referenced by execute().

Pose2D GT2004SelfLocator::templates[SAMPLES_MAX] [private]
 

Templates for poses replacing bad samples.

Definition at line 93 of file GT2004SelfLocator.h.

Referenced by generatePoseTemplates(), and getTemplate().

Flag GT2004SelfLocator::flags[FLAGS_MAX] [private]
 

A buffer for previously seen flags.

Definition at line 96 of file GT2004SelfLocator.h.

int GT2004SelfLocator::numOfFlags [private]
 

The number of flags in the buffer.

Definition at line 97 of file GT2004SelfLocator.h.

Referenced by addFlag(), generatePoseTemplates(), getBearing(), and GT2004SelfLocator().

int GT2004SelfLocator::numOfTemplates [private]
 

The number of templates generated.

Definition at line 97 of file GT2004SelfLocator.h.

Referenced by generatePoseTemplates(), getTemplate(), and resample().

int GT2004SelfLocator::nextTemplate [private]
 

The next template delivered.

Definition at line 97 of file GT2004SelfLocator.h.

Referenced by generatePoseTemplates(), and getTemplate().

int GT2004SelfLocator::randomFactor [private]
 

A factor that is increased if more templates are required.

Definition at line 100 of file GT2004SelfLocator.h.

Referenced by generatePoseTemplates(), and getTemplate().

LinesPercept::LineType GT2004SelfLocator::types[LinesPercept::numberOfLineTypes] [private]
 

Definition at line 101 of file GT2004SelfLocator.h.

int GT2004SelfLocator::numberOfTypes [private]
 

Definition at line 102 of file GT2004SelfLocator.h.

bool GT2004SelfLocator::sensorUpdated [private]
 

Did any update of the samples by a sensor reading happen?

Definition at line 103 of file GT2004SelfLocator.h.

Referenced by execute(), GT2004SelfLocator(), updateByFlag(), and updateByGoalPost().

double GT2004SelfLocator::average[Sample::numberOfQualities] [private]
 

Definition at line 104 of file GT2004SelfLocator.h.

Referenced by getTemplate(), GT2004SelfLocator(), and resample().

unsigned GT2004SelfLocator::timeStamp [private]
 

Definition at line 105 of file GT2004SelfLocator.h.

double GT2004SelfLocator::speed [private]
 

Definition at line 106 of file GT2004SelfLocator.h.

double GT2004SelfLocator::paramUp [static]
 

Initial value:

 0.01 
       GT2004SelfLocator::paramDown = 0.005 
       GT2004SelfLocator::paramDelay = 3 
       GT2004SelfLocator::paramHeight = 150 
       GT2004SelfLocator::paramZ = 10 
       GT2004SelfLocator::paramY = 2 
       GT2004SelfLocator::paramR = 2 
       GT2004SelfLocator::paramTrans = 100 
       GT2004SelfLocator::paramRot = 0.5

Definition at line 20 of file GT2004SelfLocator.cpp.

Referenced by handleMessage().

double GT2004SelfLocator::paramDown [static]
 

Definition at line 266 of file GT2004SelfLocator.h.

Referenced by handleMessage().

double GT2004SelfLocator::paramDelay [static]
 

Definition at line 266 of file GT2004SelfLocator.h.

Referenced by handleMessage().

double GT2004SelfLocator::paramHeight [static]
 

Definition at line 266 of file GT2004SelfLocator.h.

Referenced by handleMessage(), and updateByPoint().

double GT2004SelfLocator::paramZ [static]
 

Definition at line 266 of file GT2004SelfLocator.h.

Referenced by execute(), handleMessage(), and updateByPoint().

double GT2004SelfLocator::paramY [static]
 

Definition at line 266 of file GT2004SelfLocator.h.

Referenced by handleMessage(), and updateByPoint().

double GT2004SelfLocator::paramR [static]
 

Definition at line 266 of file GT2004SelfLocator.h.

double GT2004SelfLocator::paramTrans [static]
 

Definition at line 266 of file GT2004SelfLocator.h.

Referenced by execute(), and handleMessage().

double GT2004SelfLocator::paramRot [static]
 

Definition at line 266 of file GT2004SelfLocator.h.

Referenced by handleMessage().

The documentation for this class was generated from the following files:
Generated on Thu Sep 23 20:08:00 2004 for GT2004 by doxygen 1.3.6