Modules/SelfLocator/GT2004SelfLocator.h

Go to the documentation of this file.

/**
* @file Modules/SelfLocator/GT2004SelfLocator.h
* 
* This file contains a class for self-localization based on the Monte Carlo approach 
* using goals, landmarks, and field lines.
*
* @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
*/

#ifndef __GT2004SelfLocator_h_
#define __GT2004SelfLocator_h_

#include "SelfLocator.h"
#include "LinesTables2004.h"

/**
* The class implements a lines-based Monte Carlo self-localization.
*/
class GT2004SelfLocator : public SelfLocator, public LinesTables2004
{
  private:
    /**
    * The class represents a sample.
    */
    class Sample : public PoseSample
    {
      public:
        enum 
        {
          numberOfQualities = LinesPercept::numberOfLineTypes + 1
        };
        Pose2D camera; /**< Temporary representing the pose of the camera. */
        double quality[numberOfQualities]; /**< The quality of the sample, i.e. a lowpass filtered probability. */
        Sample* next; /**< The next sample in the cell cube. */
    
        /**
        * Constructor.
        */
        Sample();

        /**
        * Constructor.
        * @param pose The pose of the sample.
        * @param quality The quality of the sample.
        */
        Sample(const Pose2D& pose,const double* quality);

        double getQuality() const {return probability;}

        void updateQuality(const double* average);

        void setProbability(LinesPercept::LineType type,double value);

        bool isValid() const {return probability != 2;}
    };
  
    /**
    * The class represents a cell in a cube that is used to determine the largest sample cluster.
    */
    class Cell
    {
    public:
      int count; /**< The number of samples in this cube. */
      Sample* first; /**< The first sample in this cube. */
    
      /**

      * Constructor.
      */
      Cell() {count = 0; first = 0;}
    };


    enum
    {
      SAMPLES_MAX = 100, /**< The number of samples. */
      GRID_MAX = 10, /**< The number of cells in one dimension. */
      FLAGS_MAX = 3 /**< The size of the flag buffer. */
    };
  
    enum FlagSides
    {
      LEFT_SIDE_OF_FLAG = 1, /**< A marker for left edges of flags. */
      RIGHT_SIDE_OF_FLAG = -1 /**< A marker for right edges of flags. */
    };
  
    SampleSet<Sample, SAMPLES_MAX> sampleSet; /**< The sample set. */
    Pose2D lastOdometry, /**< The state of the odometry at the previous call of this module. */
           lastOdometry2,
           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. */
        numOfTemplates, /**< The number of templates generated. */
        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;

    /**
    * The function distributes the parameter in a Gaussian way.
    * @param d A value that should be distributed.
    * @return A transformation of d according to a Gaussian curve.
    */
    double sigmoid(double d) const {return exp(- d * d);}
  
    /** 
    * The function updates the samples by the odometry offset.
    * @param odometry The motion since the last call to this function.
    * @param camera The camera offset.
    * @param noise Dermines whether some additional noise is added to the sample poses.
    */
    void updateByOdometry(const Pose2D& odometry,const Pose2D& camera,bool noise);
  
    /** 
    * The function updates the samples by a single line point recognized.
    * @param point The relative offset of the point.
    * @param type The type of the point.
    */
    void updateByPoint(const LinesPercept::LinePoint& point,LinesPercept::LineType type);
  
    /** 
    * The function updates the samples by a single edge of a flag recognized.
    * @param flag The position of the flag.
    * @param sideOfFlag The side of the flag that was seen.
    * @param measuredBearing The bearing, under which the edge was seen.
    */
    void updateByFlag(const Vector2<double>& flag,
                      FlagSides sideOfFlag,
                      double measuredBearing);
  
    /** 
    * The function updates the samples by a single goal post recognized.
    * @param goalPost The position of the goal post.
    * @param measuredBearing The bearing, under which the goal post was seen.
    */
    void updateByGoalPost(const Vector2<double>& goalPost,
                          double measuredBearing);
  
    /**
    * The function re-distributes the samples according to their probabilities.
    */
    void resample();
  
    /**
    * The function determines the most probable pose from the sample distribution.
    * @param pose The pose is returned to this variable.
    * @param validity The validity of the pose is returned to this variable.
    */
    void calcPose(Pose2D& pose,double& validity);
  
    /**
    * The function adds a flag to the buffer.
    * @param flag The new flag.
    */
    void addFlag(const Flag& flag);
  
    /**
    * The function calculates the current pose of the robot from three bearings.
    * @param dir0 The bearing on the first landmark.
    * @param dir1 The bearing on the second landmark.
    * @param dir2 The bearing on the third landmark.
    * @param mark0 The position of the first landmark.
    * @param mark1 The position of the second landmark.
    * @param mark2 The position of the third landmark.
    * @param cameraOffset The offset of the camera relative to the body center.
    * @param resultingPose The calculated pose is returned to this variable.
    * @return Was the function successful?
    */
    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 up to two current poses of the robot from two bearings and a distance.
    * @param dir0 The bearing on the first landmark.
    * @param dir1 The bearing on the second landmark.
    * @param dist The distance of the first landmark.
    * @param mark0 The position of the first landmark.
    * @param mark1 The position of the second landmark.
    * @param cameraOffset The offset of the camera relative to the body center.
    * @param resultingPose1 One calculated pose is returned to this variable.
    * @param resultingPose2 A second calculated pose is returned to this variable.
    * @return The number of poses calculated?
    */
    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 determines a pair of landmark positions and bearings from a landmarks percept.
    * @param landmarksPercept The landmarks percept.
    * @param i The index of the entry in the percept.
    * @param mark The position of the mark is returned here.
    * @param dir The bearing on the mark is returned to this variable.
    * @param dist The distance of the mark is returned to this variable.
    * @return returns true if getting the bearing was successful.
    */
    bool getBearing(const LandmarksPercept& landmarksPercept,int i,
                    Vector2<double>& mark,
                    double& dir,double& dist) const;
  
    /**
    * The function generates pose templates from a landmark percept.
    * The pose templates can be used to initialize new samples.
    * @param landmarksPercept The landmarks percept.
    * @param odometry The odometry offset since the last call of this function.
    */
    void generatePoseTemplates(const LandmarksPercept& landmarksPercept,
                               const Pose2D& odometry);
 
    /**
    * The function returns the next pose template or a random one if no templates were determined.
    * @return A new sample.
    */
    Sample getTemplate();

    /**
    * The function creates a random sample inside the field boundaries.
    * @return The random sample.
    */
    //Sample getTemplate() const;
  
    /**
    * The function draws an arrow to a debug drawing.
    * @param pose The position and direction of the arrow.
    * @param color The color of the arrow.
    */
    void draw(const Sample& sample) const;
  
    /**
    * The function draws a point of a line percept.
    * @param point The relative position in field coordinates.
    * @param type The line type of the point.
    */
    void draw(const Vector2<int>& point,LinesPercept::LineType type) const;

  /**
    * The function draws an ellipse representing the localization covariance
  */
  void drawEllipse();

  public:
    /** 
    * Constructor.
    * @param interfaces The paramters of the SelfLocator module.
    */
    GT2004SelfLocator(const SelfLocatorInterfaces& interfaces);
    
    /**
     * The function executes the module.
     */
    virtual void execute();
    virtual bool handleMessage(InMessage& message);

    static double paramUp, // step size for increasing qualities
                  paramDown, // step size for decreasing qualities
                  paramDelay, // factor
                  paramHeight, // hypothetic height of head
                  paramZ, // sharpness of Gauss for rotational errors 
                  paramY; // sharpness of Gauss for translational errors
};

#endif// __GT2004SelfLocator_h_

---