Modules/SelfLocator/SlamSelfLocator/SlamSelfLocatorSample.cpp

Go to the documentation of this file.

/*
* TODO: fix strange landmark solution for goals etc
*/


#include "SlamSelfLocatorSample.h"

double SlamSelfLocatorSample::paramProbUpLimit   = 0.01,//0.01,       // per-cycle limit for increasing probabilities
       SlamSelfLocatorSample::paramProbDownLimit = 0.005,//0.005,       // per-cycle limit for decreasing probabilities
       SlamSelfLocatorSample::paramProbDelay = 3;             // delay param for changing probabilities

SlamSelfLocatorSample::SlamSelfLocatorSample()
{
  // initialize the probabilities for all percepts with 50% 
  // and flag the probability for the sample "UNINITIALIZED"
  for(int i = 0; i < numberOfPerceptTypes; i++)
    perceptProbabilities[i] = 0.5;// 0.5;
  probability = UNINITIALIZED_PROBABILITY;
}



SlamSelfLocatorSample::SlamSelfLocatorSample(const Pose2D& pose):PoseSample(pose)
{
  for(int i = 0; i < numberOfPerceptTypes; i++)
  {
    perceptProbabilities[i] = UNINITIALIZED_PROBABILITY;
    perceptTimestamps[i] = SystemCall::getCurrentSystemTime();
  }
  probability = UNINITIALIZED_PROBABILITY;
  // Sample();
}



/*
* update method ignoring invalid or uninitialized values
*
*/
void SlamSelfLocatorSample::updateProbability()
{
  probability = 1;

  for (int i=0; i<numberOfPerceptTypes; i++)
  {
    // why using probuplimit not down?
    // testing: if a probability is uninitialized/invalid it stays and will be omitted 
    // calculating the sample probability
    // omitting yellow goal
    if(perceptProbabilities[i]<=1 ) 
      probability *= perceptProbabilities[i];
  }
  
  // if (probability == 1) probability = UNINITIALIZED_PROBABILITY;
}


/* updates the probability of the sample -> multilplies all probablities.
* if a probability is not set, a value dependent of the average is taken
* @param average pointer to the array of average probabilities for the percept types
*/
void SlamSelfLocatorSample::updateProbability(const double* average)
{
  probability = 1;

  //long now = SystemCall::getCurrentSystemTime();
  for (int i=0; i<numberOfPerceptTypes; i++)
  {
    // why using probuplimit not down?
    // one goal is omitted
    
    double alternateProb = max(average[i] - paramProbDelay * paramProbUpLimit, 0.000001);

    /*
    if ((now-perceptTimestamps[i])>PERCEPT_TTL_MS)
    {
      // perceptProbabilities[i] = NO_PERCEPT_PROBABILITY;
      // OUTPUT(idText,text,"percept type "<< i << "ttl exceeded");
    }*/

    if (i!=skyblueGoal &&  perceptProbabilities[i]!=NO_PERCEPT_PROBABILITY /*&& i!=lineCrossing*/) 
      probability *= perceptProbabilities[i]>1 ? alternateProb : perceptProbabilities[i]; 
  }
}

// sets the probability for a percept. replaces setProbability in GT04SL
// limits change of probability to +paramProbUpLimit and -paramProbDownLimit
// differs from gt code by strange part dependent on goal type
// maybe change interface later (LinesPercept "misuse")
// TODO: weigh better regognized types stronger
//       handle "goal problem" (ie only one goal can be seen
//       but two probabilities are used / was in gt04: mapping goals onto one linetype
void SlamSelfLocatorSample::setPerceptProbability(PerceptType type, double value)
{
  if (type>=0 && type < numberOfPerceptTypes){
    double& q = perceptProbabilities[type];

    // perceptTimestamps[type] = SystemCall::getCurrentSystemTime();

    if((type == skyblueGoal) || type == flag || type == yFieldLine )
    {
      if(q > 1)
        q = value;
      else if(value < q)
        if(value < q - 0.05)
          q -= 0.05;
        else
          q = value;
      else 
        if(value > q + 0.1)
          q += 0.1;
        else
          q = value;
    }
    else 
    { 
      if (q > 1) 
        q = value;      // replace illegal or uninitialized values
      else 
        if (value<q)        // decreasing probability
          if (value < q - paramProbDownLimit) 
            q -=paramProbDownLimit;
          else 
            q = value;
        else 
          if (value > q + paramProbUpLimit) 
            q += paramProbUpLimit;
          else 
            q = value;        

    }

    // only one goal can be seen, so the other has no probability
    if (type==yellowGoal)
    {
      perceptProbabilities[skyblueGoal] = perceptProbabilities[yellowGoal];
      // perceptProbabilities[LinesPercept::yellowGoal] = sqrt(perceptProbabilities[LinesPercept::yellowGoal]);
      // perceptProbabilities[LinesPercept::skyblueGoal] = perceptProbabilities[LinesPercept::yellowGoal];
    } else if (type==skyblueGoal)
    {
      perceptProbabilities[yellowGoal] = perceptProbabilities[skyblueGoal];
      // perceptProbabilities[yellowGoal] = UNINITIALIZED_PROBABILITY;
      // perceptProbabilities[LinesPercept::skyblueGoal] = sqrt(perceptProbabilities[LinesPercept::skyblueGoal]);
      // perceptProbabilities[LinesPercept::yellowGoal] = perceptProbabilities[LinesPercept::skyblueGoal];
    }

  }

}

---