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

# Modules/SelfLocator/GT2005StableSelfLocator/GT2005StableSelfLocatorSample.cpp

Go to the documentation of this file.

    00001 /*
    00002 * TODO: fix strange landmark solution for goals etc
    00003 */
    00004
    00005
    00006 #include "GT2005StableSelfLocatorSample.h"
    00007
    00008 double GT2005StableSelfLocatorSample::paramProbUpLimit   = 0.01,//0.01,       // per-cycle limit for increasing probabilities
    00009        GT2005StableSelfLocatorSample::paramProbDownLimit = 0.005,//0.005,       // per-cycle limit for decreasing probabilities
    00010        GT2005StableSelfLocatorSample::paramProbDelay = 3;             // delay param for changing probabilities
    00011
    00012 GT2005StableSelfLocatorSample::GT2005StableSelfLocatorSample()
    00013 {
    00014   // initialize the probabilities for all percepts with 50%
    00015   // and flag the probability for the sample "UNINITIALIZED"
    00016   for(int i = 0; i < numberOfPerceptTypes; i++)
    00017     perceptProbabilities[i] = 0.5;// 0.5;
    00018   probability = UNINITIALIZED_PROBABILITY;
    00019 }
    00020
    00021
    00022
    00023 GT2005StableSelfLocatorSample::GT2005StableSelfLocatorSample(const Pose2D& pose):PoseSample(pose)
    00024 {
    00025   for(int i = 0; i < numberOfPerceptTypes; i++)
    00026   {
    00027     perceptProbabilities[i] = UNINITIALIZED_PROBABILITY;
    00028     perceptTimestamps[i] = SystemCall::getCurrentSystemTime();
    00029   }
    00030   probability = UNINITIALIZED_PROBABILITY;
    00031   // Sample();
    00032 }
    00033
    00034
    00035
    00036 /*
    00037 * update method ignoring invalid or uninitialized values
    00038 *
    00039 */
    00040 void GT2005StableSelfLocatorSample::updateProbability()
    00041 {
    00042   probability = 1;
    00043
    00044   for (int i=0; i<numberOfPerceptTypes; i++)
    00045   {
    00046     // why using probuplimit not down?
    00047     // testing: if a probability is uninitialized/invalid it stays and will be omitted
    00048     // calculating the sample probability
    00049     // omitting yellow goal
    00050     if(perceptProbabilities[i]<=1 )
    00051       probability *= perceptProbabilities[i];
    00052   }
    00053
    00054   // if (probability == 1) probability = UNINITIALIZED_PROBABILITY;
    00055 }
    00056
    00057
    00058 /* updates the probability of the sample -> multilplies all probablities.
    00059 * if a probability is not set, a value dependent of the average is taken
    00060 * @param average pointer to the array of average probabilities for the percept types
    00061 */
    00062 void GT2005StableSelfLocatorSample::updateProbability(const double* average)
    00063 {
    00064   probability = 1;
    00065
    00066   //long now = SystemCall::getCurrentSystemTime();
    00067   for (int i=0; i<numberOfPerceptTypes; i++)
    00068   {
    00069     // why using probuplimit not down?
    00070     // one goal is omitted
    00071
    00072     double alternateProb = max(average[i] - paramProbDelay * paramProbUpLimit, 0.000001);
    00073
    00074     /*
    00075     if ((now-perceptTimestamps[i])>PERCEPT_TTL_MS)
    00076     {
    00077       // perceptProbabilities[i] = NO_PERCEPT_PROBABILITY;
    00078       // OUTPUT(idText,text,"percept type "<< i << "ttl exceeded");
    00079     }*/
    00080
    00081     if (i!=skyblueGoal &&  perceptProbabilities[i]!=NO_PERCEPT_PROBABILITY /*&& i!=lineCrossing*/)
    00082       probability *= perceptProbabilities[i]>1 ? alternateProb : perceptProbabilities[i];
    00083   }
    00084 }
    00085
    00086 // sets the probability for a percept. replaces setProbability in GT04SL
    00087 // limits change of probability to +paramProbUpLimit and -paramProbDownLimit
    00088 // differs from gt code by strange part dependent on goal type
    00089 // maybe change interface later (LinesPercept "misuse")
    00090 // TODO: weigh better regognized types stronger
    00091 //       handle "goal problem" (ie only one goal can be seen
    00092 //       but two probabilities are used / was in gt04: mapping goals onto one linetype
    00093 void GT2005StableSelfLocatorSample::setPerceptProbability(PerceptType type, double value)
    00094 {
    00095   if (type>=0 && type < numberOfPerceptTypes){
    00096     double& q = perceptProbabilities[type];
    00097
    00098     // perceptTimestamps[type] = SystemCall::getCurrentSystemTime();
    00099
    00100     if((type == skyblueGoal) || type == flag || type == yFieldLine )
    00101     {
    00102       if(q > 1)
    00103         q = value;
    00104       else if(value < q)
    00105         if(value < q - 0.05)
    00106           q -= 0.05;
    00107         else
    00108           q = value;
    00109       else
    00110         if(value > q + 0.1)
    00111           q += 0.1;
    00112         else
    00113           q = value;
    00114     }
    00115     else
    00116     {
    00117       if (q > 1)
    00118         q = value;      // replace illegal or uninitialized values
    00119       else
    00120         if (value<q)        // decreasing probability
    00121           if (value < q - paramProbDownLimit)
    00122             q -=paramProbDownLimit;
    00123           else
    00124             q = value;
    00125         else
    00126           if (value > q + paramProbUpLimit)
    00127             q += paramProbUpLimit;
    00128           else
    00129             q = value;
    00130
    00131     }
    00132
    00133     // only one goal can be seen, so the other has no probability
    00134     if (type==yellowGoal)
    00135     {
    00136       perceptProbabilities[skyblueGoal] = perceptProbabilities[yellowGoal];
    00137       // perceptProbabilities[LinesPercept::yellowGoal] = sqrt(perceptProbabilities[LinesPercept::yellowGoal]);
    00138       // perceptProbabilities[LinesPercept::skyblueGoal] = perceptProbabilities[LinesPercept::yellowGoal];
    00139     } else if (type==skyblueGoal)
    00140     {
    00141       perceptProbabilities[yellowGoal] = perceptProbabilities[skyblueGoal];
    00142       // perceptProbabilities[yellowGoal] = UNINITIALIZED_PROBABILITY;
    00143       // perceptProbabilities[LinesPercept::skyblueGoal] = sqrt(perceptProbabilities[LinesPercept::skyblueGoal]);
    00144       // perceptProbabilities[LinesPercept::yellowGoal] = perceptProbabilities[LinesPercept::skyblueGoal];
    00145     }
    00146
    00147   }
    00148
    00149 }