Modules/SelfLocator/GT2005SelfLocator/GT2005SelfLocatorSample.cpp

Go to the documentation of this file.

/*
* Representation for a single particle(=sample) in the gt2005 selflocator
* Based on the Sample of the GT2004 SelfLocator
*
*  @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
*  @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
*
*/


#include "GT2005SelfLocatorSample.h"

GT2005SelfLocatorSample::GT2005SelfLocatorSample()
{
  reset();
}

GT2005SelfLocatorSample::GT2005SelfLocatorSample(const Pose2D& pose) : 
  PoseSample(pose)
{
  reset();
}

void GT2005SelfLocatorSample::reset()
{
  // initialize the probabilities for all percepts
  // and flag the probability for the sample "UNINITIALIZED"
  for(int i = 0; i < numberOfPerceptTypes; i++)
  {
    perceptProbabilities[i] = UNINITIALIZED_PROBABILITY;// 0.5;
    perceptTimestamps[i] = SystemCall::getCurrentSystemTime();
  }
  probability = UNINITIALIZED_PROBABILITY;
  isUsingOdometry = true;
}

/* updates the probability of the sample -> multiplies 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 GT2005SelfLocatorSample::updateProbability(const double* average, const GT2005SelfLocatorParameters& parameters)
{
  probability = 1;

  //long now = SystemCall::getCurrentSystemTime();
  for (int i=0; i<numberOfPerceptTypes; i++)
  {
    double alternateProb = max(average[i] - parameters.probNoQualityOffset, 0.000001);

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

   probability *= perceptProbabilities[i] > 1.0 ? 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 GT2005SelfLocatorSample::setPerceptProbability(PerceptType type, double value, const GT2005SelfLocatorParameters& parameters)
{
  if (type>=0 && type < numberOfPerceptTypes){
    double& q = perceptProbabilities[type];

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

    if(type == goal || type == flag )
    {
      if(q > 1)
        q = value;
      else if(value < q)
        if(value < q - parameters.probFlagDownLimit)
          q -= parameters.probFlagDownLimit;
        else
          q = value;
      else 
        if(value > q + parameters.probFlagUpLimit)
          q += parameters.probFlagUpLimit;
        else
          q = value;
    }
    else 
    { 
      if (q > 1) 
        q = value;      // replace illegal or uninitialized values
      else 
        if (value<q)        // decreasing probability
          if (value < q - parameters.probLineDownLimit) 
            q -=parameters.probLineDownLimit;
          else 
            q = value;
        else 
          if (value > q + parameters.probLineUpLimit) 
            q += parameters.probLineUpLimit;
          else 
            q = value;        

    }
  }
}

void GT2005SelfLocatorSample::scaleProbabilities(double factor)
{
  for(int i = 0; i < numberOfPerceptTypes; ++i)
    if (perceptProbabilities[i] != UNINITIALIZED_PROBABILITY)
      perceptProbabilities[i] *= factor;
}

In& operator>>(In& stream,GT2005SelfLocatorSample& sample)
{

  STREAM_REGISTER_BEGIN_EXT(sample);
  STREAM_BASE_EXT( stream, (Pose2D&) sample);
  STREAM_EXT(stream, sample.probability);
  STREAM_ARRAY_EXT(stream, sample.perceptProbabilities);
  STREAM_ARRAY_EXT(stream, sample.perceptTimestamps);
  STREAM_EXT(stream, sample.isUsingOdometry);
  STREAM_EXT(stream, sample.camera);
  STREAM_REGISTER_FINISH();
  return stream;
}

Out& operator<<(Out& stream, const GT2005SelfLocatorSample& sample)
{
  STREAM_REGISTER_BEGIN_EXT(sample);
  STREAM_BASE_EXT( stream, (Pose2D&) sample);
  STREAM_EXT(stream, sample.probability);
  STREAM_ARRAY_EXT(stream, sample.perceptProbabilities);
  STREAM_ARRAY_EXT(stream, sample.perceptTimestamps);
  STREAM_EXT(stream, sample.isUsingOdometry);
  STREAM_EXT(stream, sample.camera);
  STREAM_REGISTER_FINISH();
  return stream;
}

---