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

# Modules/SelfLocator/GT2005SelfLocator/GT2005SelfLocatorSample.cpp

Go to the documentation of this file.

    00001 /*
    00002 * Representation for a single particle(=sample) in the gt2005 selflocator
    00003 * Based on the Sample of the GT2004 SelfLocator
    00004 *
    00005 *  @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
    00006 *  @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
    00007 *
    00008 */
    00009
    00010
    00011 #include "GT2005SelfLocatorSample.h"
    00012
    00013 GT2005SelfLocatorSample::GT2005SelfLocatorSample()
    00014 {
    00015   reset();
    00016 }
    00017
    00018 GT2005SelfLocatorSample::GT2005SelfLocatorSample(const Pose2D& pose) :
    00019   PoseSample(pose)
    00020 {
    00021   reset();
    00022 }
    00023
    00024 void GT2005SelfLocatorSample::reset()
    00025 {
    00026   // initialize the probabilities for all percepts
    00027   // and flag the probability for the sample "UNINITIALIZED"
    00028   for(int i = 0; i < numberOfPerceptTypes; i++)
    00029   {
    00030     perceptProbabilities[i] = UNINITIALIZED_PROBABILITY;// 0.5;
    00031     perceptTimestamps[i] = SystemCall::getCurrentSystemTime();
    00032   }
    00033   probability = UNINITIALIZED_PROBABILITY;
    00034   isUsingOdometry = true;
    00035 }
    00036
    00037 /* updates the probability of the sample -> multiplies all probablities.
    00038 * if a probability is not set, a value dependent of the average is taken
    00039 * @param average pointer to the array of average probabilities for the percept types
    00040 */
    00041 void GT2005SelfLocatorSample::updateProbability(const double* average, const GT2005SelfLocatorParameters& parameters)
    00042 {
    00043   probability = 1;
    00044
    00045   //long now = SystemCall::getCurrentSystemTime();
    00046   for (int i=0; i<numberOfPerceptTypes; i++)
    00047   {
    00048     double alternateProb = max(average[i] - parameters.probNoQualityOffset, 0.000001);
    00049
    00050     /*
    00051     if ((now-perceptTimestamps[i])>PERCEPT_TTL_MS)
    00052     {
    00053       // perceptProbabilities[i] = UNINITIALIZED_PROBABILITY;
    00054       // OUTPUT(idText,text,"percept type "<< i << "ttl exceeded");
    00055     }*/
    00056
    00057    probability *= perceptProbabilities[i] > 1.0 ? alternateProb : perceptProbabilities[i];
    00058   }
    00059 }
    00060
    00061 // sets the probability for a percept. replaces setProbability in GT04SL
    00062 // limits change of probability to +paramProbUpLimit and -paramProbDownLimit
    00063 // differs from gt code by strange part dependent on goal type
    00064 // maybe change interface later (LinesPercept "misuse")
    00065 // TODO: weigh better regognized types stronger
    00066 //       handle "goal problem" (ie only one goal can be seen
    00067 //       but two probabilities are used / was in gt04: mapping goals onto one linetype
    00068 void GT2005SelfLocatorSample::setPerceptProbability(PerceptType type, double value, const GT2005SelfLocatorParameters& parameters)
    00069 {
    00070   if (type>=0 && type < numberOfPerceptTypes){
    00071     double& q = perceptProbabilities[type];
    00072
    00073     perceptTimestamps[type] = SystemCall::getCurrentSystemTime();
    00074
    00075     if(type == goal || type == flag )
    00076     {
    00077       if(q > 1)
    00078         q = value;
    00079       else if(value < q)
    00080         if(value < q - parameters.probFlagDownLimit)
    00081           q -= parameters.probFlagDownLimit;
    00082         else
    00083           q = value;
    00084       else
    00085         if(value > q + parameters.probFlagUpLimit)
    00086           q += parameters.probFlagUpLimit;
    00087         else
    00088           q = value;
    00089     }
    00090     else
    00091     {
    00092       if (q > 1)
    00093         q = value;      // replace illegal or uninitialized values
    00094       else
    00095         if (value<q)        // decreasing probability
    00096           if (value < q - parameters.probLineDownLimit)
    00097             q -=parameters.probLineDownLimit;
    00098           else
    00099             q = value;
    00100         else
    00101           if (value > q + parameters.probLineUpLimit)
    00102             q += parameters.probLineUpLimit;
    00103           else
    00104             q = value;
    00105
    00106     }
    00107   }
    00108 }
    00109
    00110 void GT2005SelfLocatorSample::scaleProbabilities(double factor)
    00111 {
    00112   for(int i = 0; i < numberOfPerceptTypes; ++i)
    00113     if (perceptProbabilities[i] != UNINITIALIZED_PROBABILITY)
    00114       perceptProbabilities[i] *= factor;
    00115 }
    00116
    00117 In& operator>>(In& stream,GT2005SelfLocatorSample& sample)
    00118 {
    00119
    00120   STREAM_REGISTER_BEGIN_EXT(sample);
    00121   STREAM_BASE_EXT( stream, (Pose2D&) sample);
    00122   STREAM_EXT(stream, sample.probability);
    00123   STREAM_ARRAY_EXT(stream, sample.perceptProbabilities);
    00124   STREAM_ARRAY_EXT(stream, sample.perceptTimestamps);
    00125   STREAM_EXT(stream, sample.isUsingOdometry);
    00126   STREAM_EXT(stream, sample.camera);
    00127   STREAM_REGISTER_FINISH();
    00128   return stream;
    00129 }
    00130
    00131 Out& operator<<(Out& stream, const GT2005SelfLocatorSample& sample)
    00132 {
    00133   STREAM_REGISTER_BEGIN_EXT(sample);
    00134   STREAM_BASE_EXT( stream, (Pose2D&) sample);
    00135   STREAM_EXT(stream, sample.probability);
    00136   STREAM_ARRAY_EXT(stream, sample.perceptProbabilities);
    00137   STREAM_ARRAY_EXT(stream, sample.perceptTimestamps);
    00138   STREAM_EXT(stream, sample.isUsingOdometry);
    00139   STREAM_EXT(stream, sample.camera);
    00140   STREAM_REGISTER_FINISH();
    00141   return stream;
    00142 }