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:

[legend]
Collaboration diagram for GT2004SelfLocator:

[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
## 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 Sample &sample) 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.
void | drawEllipse ()
| The function draws an ellipse representing the localization covariance.
## Private Attributes
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 19 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 74 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 81 of file GT2004SelfLocator.h.

## Constructor & Destructor Documentation

GT2004SelfLocator::GT2004SelfLocator | ( | const SelfLocatorInterfaces & | interfaces | ) |

  | Constructor. Parameters:
  interfaces | The paramters of the SelfLocator module.
Definition at line 84 of file GT2004SelfLocator.cpp. References average, 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 108 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 209 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 239 of file GT2004SelfLocator.cpp. References Vector2< V >::abs(), Vector2< int >::abs(), Vector2< int >::angle(), draw(), Pose2D::getAngle(), getPlayer(), Player::getTeamColor(), NCOMPLEX_DRAWING, 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 273 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 296 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 313 of file GT2004SelfLocator.cpp. References average, draw(), GT2004SelfLocator::Sample::getQuality(), getTemplate(), NCOMPLEX_DRAWING, 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 379 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 598 of file GT2004SelfLocator.cpp. References FLAGS_MAX, ConditionalBoundary::isOnBorder(), Range< double >::max, Range< double >::min, 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 485 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 523 of file GT2004SelfLocator.cpp. References Vector2< V >::abs(), alpha, 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 563 of file GT2004SelfLocator.cpp. References Flag::angle, Goal::distance, Flag::distance, LandmarksPercept::goals, ConditionalBoundary::isOnBorder(), Goal::leftPost, Range< double >::max, Range< double >::min, 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 617 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, numOfTemplates, point, LinesPercept::points, poseFromBearings(), poseFromBearingsAndDistance(), randomFactor, SAMPLES_MAX, templates, types, 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 680 of file GT2004SelfLocator.cpp. References average, 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 Sample & | sample | ) | 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 707 of file GT2004SelfLocator.cpp. References NLINE, PoseSample::probability, Pose2D::translation, Vector2< double >::x, and Vector2< double >::y. Referenced by 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 753 of file GT2004SelfLocator.cpp. References Geometry::calculatePointInImage(), getRobotConfiguration(), NCIRCLE, point, Vector2< V >::x, and Vector2< V >::y.

Here is the call graph for this function:

void GT2004SelfLocator::drawEllipse | ( |  | ) | `[private]`

| The function draws an ellipse representing the localization covariance. Definition at line 773 of file GT2004SelfLocator.cpp. References alpha, SampleSetProxyBase::getNumberOfSamples(), NDOT, and sqr.

Here is the call graph for this function:

void GT2004SelfLocator::execute | ( |  | ) | `[virtual]`

| The function executes the module. Reimplemented from Module. Definition at line 99 of file GT2004SelfLocator.cpp. References calcPose(), LandmarksPercept::cameraOffset, DEBUG_DRAWING_FINISHED, e, 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, NDECLARE_DEBUGDRAWING, LandmarksPercept::numberOfFlags, LandmarksPercept::numberOfGoals, paramZ, LinesPercept::points, Flag::position, random(), resample(), RIGHT_SIDE_OF_FLAG, Goal::rightPost, sampleSet, sensorUpdated, RobotPose::setFrameNumber(), RobotPose::setPose(), RobotPose::setTimestamp(), RobotPose::setValidity(), types, 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 856 of file GT2004SelfLocator.cpp. References InMessage::bin, GenericDebugData::data, InMessage::getMessageID(), idLinesSelfLocatorParameters, paramDelay, paramDown, paramHeight, paramUp, paramY, and paramZ.

Here is the call graph for this function:

## Member Data Documentation

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

| The sample set. Definition at line 87 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 88 of file GT2004SelfLocator.h.

Pose2D GT2004SelfLocator::lastOdometry2`[private]`

| The state of the odometry at the previous call of this module. Definition at line 88 of file GT2004SelfLocator.h. Referenced by execute().

Pose2D GT2004SelfLocator::templates[SAMPLES_MAX]`[private]`

| Templates for poses replacing bad samples. Definition at line 88 of file GT2004SelfLocator.h. Referenced by generatePoseTemplates(), and getTemplate().

Flag GT2004SelfLocator::flags[FLAGS_MAX]`[private]`

| A buffer for previously seen flags. Definition at line 91 of file GT2004SelfLocator.h.

int GT2004SelfLocator::numOfFlags`[private]`

| The number of flags in the buffer. Definition at line 92 of file GT2004SelfLocator.h.

int GT2004SelfLocator::numOfTemplates`[private]`

| The number of templates generated. Definition at line 92 of file GT2004SelfLocator.h. Referenced by generatePoseTemplates(), getTemplate(), and resample().

int GT2004SelfLocator::nextTemplate`[private]`

| The next template delivered. Definition at line 92 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 95 of file GT2004SelfLocator.h. Referenced by generatePoseTemplates(), and getTemplate().

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

| Definition at line 96 of file GT2004SelfLocator.h. Referenced by execute(), and generatePoseTemplates().

int GT2004SelfLocator::numberOfTypes`[private]`

| Definition at line 97 of file GT2004SelfLocator.h.

bool GT2004SelfLocator::sensorUpdated`[private]`

| Did any update of the samples by a sensor reading happen? Definition at line 98 of file GT2004SelfLocator.h. Referenced by execute(), GT2004SelfLocator(), updateByFlag(), and updateByGoalPost().

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

| Definition at line 99 of file GT2004SelfLocator.h. Referenced by getTemplate(), GT2004SelfLocator(), and resample().

unsigned GT2004SelfLocator::timeStamp`[private]`

| Definition at line 100 of file GT2004SelfLocator.h.

double GT2004SelfLocator::speed`[private]`

| Definition at line 101 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
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().

The documentation for this class was generated from the following files:

- Modules/SelfLocator/GT2004SelfLocator.h
- Modules/SelfLocator/GT2004SelfLocator.cpp