Modules/SelfLocator/GT2005SelfLocator/GT2005SelfLocator.h

Go to the documentation of this file.

/**
* @file Modules/SelfLocator/GT2005SelfLocator.h
* 
* This file contains a class for self-localization based on the Monte Carlo approach 
* using goals, landmarks, line crossings, field lines and the center circle
*
*    @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
*    @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
*    @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
*
*/

#ifndef __GT2005SelfLocator_h_
#define __GT2005SelfLocator_h_

#include "../SelfLocator.h"
#include "../LinesTables2005.h"
#include "GT2005SelfLocatorSample.h"
#include "GT2005SampleTemplateGenerator.h"
#include "GT2005LineCrossingsTable.h"
#include "GT2005SelfLocatorParameters.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/ColorClasses.h"
#include "Tools/Math/Geometry.h"
#include "Tools/TRingBufferWithSum.h"

class GT2005SelfLocator : public SelfLocator, public LinesTables2005
{
  public:
    /*------------------------------ public methods -----------------------------------*/
    
    /** 
    * Constructor.
    * @param interfaces The paramters of the SelfLocator module.
    */
    GT2005SelfLocator(const SelfLocatorInterfaces& interfaces);
      
    /**
    * The function executes the module.
    */
    virtual void execute();
    virtual bool handleMessage(InMessage& message);


  private:

  /* ------------------------- private classes here --------------------- */

    /**
    * 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. */
        GT2005SelfLocatorSample* first;       /**< The first sample in this cube. */
        
      /**
      * Constructor.
      * preinititalize first with 0 to enshure correct end of queue detection
      */
      Cell() {count = 0; first = 0;}
    };


/* ----------------------------------- private constants here ----------------------------------*/


    enum FlagSides{
      LEFT_SIDE_OF_FLAG = 1,                    /**< flag indicating a position on the left side of a flag */
      RIGHT_SIDE_OF_FLAG = -1                   /**< flag indicating a position on the right side of a flag */
    };

    enum{
      SAMPLES_MAX = 100,                          /**< maximum number of samples */
      POSE_SPACE_GRID = 10,                       /**< quantization of the pose space */

   /** in the original 04 observation table LinesPercept::field is a x directed line and
        LinesPercept::numberOfLineTypes is a y directed line */
       OBS_TABLE_X_FIELD_LINE = LinesPercept::field,
       OBS_TABLE_Y_FIELD_LINE = LinesPercept::numberOfLineTypes,

       //HEAD_HEIGHT_SMOOTHING_FRAMENUMBER = 5,

       NUM_OF_CALCULATED_POSES = 3
    };

    static double const QUASI_ZERO;

/* ----------------------------------- private variables here ----------------------------------*/

    SampleSet<GT2005SelfLocatorSample, SAMPLES_MAX> sampleSet;    /**< the sampleSet */
    bool teamColorBlue;
    
    //double headHeight;             // is calculated dynamically
      
    double linePointZAngleMotionDependentVariance,
          /*linePointYAngleMotionDependentVariance,

          crossingZAngleMotionDependentVariance,
          crossingYAngleMotionDependentVariance,

          centerCircleZAngleMotionDependentVariance,
          centerCircleYAngleMotionDependentVariance,
          centerCircleOrientationAngleMotionDependentVariance,

          flagZAngleMotionDependentVariance,
          flagYAngleMotionDependentVariance,
          
          goalZAngleMotionDependentVariance,
          goalYAngleMotionDependentVariance,*/

          linePointZAngleTrust,
          linePointYAngleTrust,

          crossingZAngleTrust,
          crossingYAngleTrust,

          centerCircleZAngleTrust,
          centerCircleYAngleTrust,
          centerCircleOrientationAngleTrust,

          //flagZAngleTrust,
          flagYAngleTrust,

          goalZAngleTrust,
          goalYAngleTrust;

    Pose2D lastOdometry, lastOdometry2;                       /**< the last Odometry, used to calculate the delta_odometry */
    unsigned timeStamp;                                       /**< timestap */
    bool observationUpdateDone;                               /**< a flag indicating if an observation update was done during the current run of execute */
    double speed;                                             /**< current robot speed */

    double averagePerceptTypeProb[GT2005SelfLocatorSample::numberOfPerceptTypes]; /**< the average probability for each PerceptType and all Samples*/

    GT2005SampleTemplateGenerator sampleTemplateGenerator;                        /**< this class encapsules template generation */

    GT2005LineCrossingsTable lineCrossingsTable;

    //TRingBufferWithSum<double,HEAD_HEIGHT_SMOOTHING_FRAMENUMBER> headHeightBuffer;

    /** testing and debugging stuff */
    GT2005SelfLocatorSample* testSample;
    int testSampleIndex;
    Vector2<double> lastModelCrossing, lastSeenCrossing;
    GT2005LineCrossingsTable::CrossingClass lastSeenCrossingClass;
    Pose2D lastSeenCenterCircle;
  
    GT2005SelfLocatorParameters parameters;   
    
    /** eigenvectors of particle distribution covariance matrix */
    Vector2<double> eigenVec0, eigenVec1;

    double varianceCovarianceMatrix[2][2];
    double stdDevRot;
    double averageX;
    double averageY;
    double eigenVal0, eigenVal1, eigenSwap;
    double averageRot;


/* ----------------------------------- private methods here ------------------------------------*/

    /**
    * 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 gaussian(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.
    *
    * enhance maybe by replacing bool noise by a noisefunction?
    */
    void motionUpdate(const Pose2D& odometry,const Pose2D& camera,bool noise);


    /** 
    * The function updates the samples by a single edge of a flag recognized.
    * @param flagFieldPosition 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>& flagFieldPosition,
            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 updates the samples by the recognized landmarks (goal, flag..)
    * triggers updateByFlag and updateByGoalPost
    * @param landmarksPercept The landmarksPercept
    */
    void landmarksObservationUpdate(const LandmarksPercept& landmarksPercept);


    /**calculates the CrossingClass out of the classification (side1..side4) in the LinesPercept
    */
    GT2005LineCrossingsTable::CrossingClass getCrossingClassification(const LinesPercept::LineCrossingPoint& point);

    /** observationUpdate by the center circle
    */
    void updateByCenterCircle(const LinesPercept::CenterCircle& centerCircle);

    /** observationUpdate using the linecrossing percept
    *   @param relative position of the linecrossing
    */
    void updateByCrossing(const LinesPercept::LineCrossingPoint& point);
    /**
    * does an observationupdate with a point on the field (line or goal)
    * @param point a point on a line
    * @param type the type of the line
    */
    void updateByPoint(const LinesPercept::LinePoint& point,GT2005SelfLocatorSample::PerceptType type, LinesPercept::LineType observationTableIndex);

    /** 
    * The function updates the samples by recognized lines
    * @param linesPercept The LinesPercept
    */
    void lineObservationUpdate(const LinesPercept& linesPercept);

    /**
    * Calculates the average possibility for each perceptType and all samples
    * and returns the product over all averages.
    * contains the part of the gt04 resampling method that calculates
    * @return product over all average percepttype-probabilities
    */
    double calcAveragePerceptTypeProbabilities();

    /**
    * redistibution of the sampleSet according to the probabilities of the samples
    */
    void resample();
    

    /**
    * Calculates the localization validity by Standard Deviation
    */
    double calcDistributionValidityByStandardDeviation();

    /**
    * Calculates the localization validity by Entropy
    */
    double calcDistributionValidityByEntropy();

    /**
    * The function calculates a Pose by averaging the sample poses in a sub cube of
    * the pose-room
    */
    RobotPose calcPoseFromSubCube(Cell poseSpace[POSE_SPACE_GRID][POSE_SPACE_GRID][POSE_SPACE_GRID], int x, int y, int r);

    /**
    * 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);


    // Debug drawings

    /**
    * The function draws the current sample set to a debug drawing.
    */
    void draw() const;

    /**
    * Returns a color corresponding to the sample probability.
    */
    Drawings::Color getSampleColor(double probability) const;

    /**
    * The function draws an arrow to the sample set drawing.
    * @param pose The position and direction of the arrow.
    * @param color The color of the arrow.
    */
    void draw(const Pose2D& pose,Drawings::Color color) const;

    /**
    * The function draws an arrow to the test sample drawing.
    * @param pose The position and direction of the arrow.
    * @param color The color of the arrow.
    */
    void drawTestSample(const Pose2D& pose,Drawings::Color color) const;
    
    /**
    * The function calculates the variances and trust values 
    * from the current speed
    */
    void updateVariancesBySpeed(double speed);

    /**
    * The function calculates the trust values from variance and 
    * weight parameters
    */
    void calculateTrust();

    /**
    * Reset all samples to random position
    */
    void resetSamples();

    /**
    * Reset all samples to a given position
    */
    void resetSamples(const Pose2D &pose);

    /**
    * Reset all samples when robot is penalized
    * 25% samples are placed on each side of the field at the center line respectively
    * 50% at random positions
    */
    void resetSamplesWhenPenalized();
};

#endif //GT2005SelfLocator.h

---