Modules/SelfLocator/SlamSelfLocator/SlamSelfLocator.h

Go to the documentation of this file.

/**
* @file Modules/SelfLocator/SlamSelfLocator.h
* 
* This file contains a class for self-localization based on the Monte Carlo approach 
* using goals, landmarks and field lines.
*
* @author 
*/

#ifndef __SlamSelfLocator_h_
#define __SlamSelfLocator_h_

#include "../SelfLocator.h"
#include "../LinesTables2004.h"
#include "SlamSelfLocatorSample.h"
#include "SlamSampleTemplateGenerator.h"
#include "SlamLineCrossingsTable.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/ColorClasses.h"
#include "Tools/Math/Geometry.h"
#include "Tools/TRingBufferWithSum.h"
//#include "Representations/MSHSelfLocatorRequest.h"

class SlamSelfLocator : public SelfLocator, public LinesTables2004
{

  public:
    /*------------------------------ public variables ---------------------------------*/

    // parameter prepared for evolution

    // motion update parameters not yet adapted

    // observation update parameters
    // trust = weight / (variance)

    static double quasiZero,
                  maximumTrust,
      
      
                  translationNoise,
                  rotationNoise,
                  movedDistWeight,
                  movedAngleWeight,
                  majorDirTransWeight,
                  minorDirTransWeight,

                  headHeight,               // is calculated dynamically


                  linePointZAngleVariance,
                  linePointZAngleMotionDependentVariance,
                  linePointYAngleVariance,
                  linePointYAngleMotionDependentVariance,
                  linePointZAngleMotionDependency,
                  linePointYAngleMotionDependency,

                  crossingZAngleVariance,
                  crossingZAngleMotionDependentVariance,
                  crossingYAngleVariance,
                  crossingYAngleMotionDependentVariance,
                  crossingZAngleMotionDependency,
                  crossingYAngleMotionDependency,
                  
                  centerCircleZAngleVariance,
                  centerCircleZAngleMotionDependentVariance,
                  centerCircleYAngleVariance,
                  centerCircleYAngleMotionDependentVariance,
                  centerCircleOrientationAngleVariance,
                  centerCircleOrientationAngleMotionDependentVariance,
                  centerCircleZAngleMotionDependency,
                  centerCircleYAngleMotionDependency,
                  centerCircleOrientationAngleMotionDependency,

          flagZAngleVariance,
                  flagZAngleMotionDependentVariance,
                  flagYAngleVariance,
                  flagYAngleMotionDependentVariance,
                  flagZAngleMotionDependency,
                  flagYAngleMotionDependency,
                  
                  goalZAngleVariance,
                  goalZAngleMotionDependentVariance,
                  goalYAngleVariance,
                  goalYAngleMotionDependentVariance,
                  goalZAngleMotionDependency,
                  goalYAngleMotionDependency,

                  linePointWeight,
                  linePointZAngleTrust,
                  linePointYAngleTrust,

                  crossingWeight,
                  crossingZAngleTrust,
                  crossingYAngleTrust,

                  centerCircleWeight,
                  centerCircleZAngleTrust,
                  centerCircleYAngleTrust,
                  centerCircleOrientationAngleTrust,

                  flagWeight,
                  flagZAngleTrust,
                  flagYAngleTrust,

                  goalWeight,
                  goalZAngleTrust,
                  goalYAngleTrust;

    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
    };

    SampleSet<SlamSelfLocatorSample, SAMPLES_MAX> sampleSet;    /**< the sampleSet */
    bool teamColorBlue;

    /*------------------------------ public methods -----------------------------------*/
    /** 
    * Constructor.
    * @param interfaces The paramters of the SelfLocator module.
    */
    SlamSelfLocator(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. */
        SlamSelfLocatorSample* 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 variables 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 */
    };

    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[SlamSelfLocatorSample::numberOfPerceptTypes]; /**< the average probability for each PerceptType and all Samples*/

    SlamSampleTemplateGenerator sampleTemplateGenerator;                        /**< this class encapsules template generation */
    LinesPercept::LineType types[LinesPercept::numberOfLineTypes];            /**< types and number of types are needed for the template generator */
    int numberOfTypes;

    SlamLineCrossingsTable lineCrossingsTable;

    TRingBufferWithSum<double,HEAD_HEIGHT_SMOOTHING_FRAMENUMBER> headHeightBuffer;

    bool odometryPoseResetted;
    Pose2D odometryPose;
    RobotPose candidates[NUM_OF_CALCULATED_POSES];

    /** testing and debugging stuff */
    SlamSelfLocatorSample* testSample;
    Vector2<double> lastModelCrossing, lastSeenCrossing;
    SlamLineCrossingsTable::CrossingClass lastSeenCrossingClass;
    Pose2D lastSeenCenterCircle;

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


    void updateByNotSeenFlag(Flag::FlagType type);

    void updateByNotSeenGoal(colorClass goalColor);

    /** 
    * 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
    */
    SlamLineCrossingsTable::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,SlamSelfLocatorSample::PerceptType type);


  /** slam challenge stuff */
  void updateByEstimatedDirection();

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

    double calcDistributionValidity();

    /**
    * 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 completely taken from gt04

    /**
    * 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 Pose2D& pose,Drawings::Color color) 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 calculates the variances and trust values 
    * from the current speed
    */
    void updateVariancesBySpeed(double speed);

};

#endif //SlamSelfLocator.h

---