Main Page   Modules   Class Hierarchy   Compound List   File List   Compound Members  

SfMonte.h

00001 //
00002 // SfMonte.h
00003 //
00004 // monte carlo localization class definitions
00005 
00006 //
00007 // Copyright 1999 by Kurt Konolige
00008 //
00009 // The author hereby grants to SRI permission to use this software.
00010 // The author also grants to SRI permission to distribute this software
00011 // to schools for non-commercial educational use only.
00012 //
00013 // The author hereby grants to other individuals or organizations
00014 // permission to use this software for non-commercial
00015 // educational use only.  This software may not be distributed to others
00016 // except by SRI, under the conditions above.
00017 //
00018 // Other than these cases, no part of this software may be used or
00019 // distributed without written permission of the author.
00020 //
00021 // Neither the author nor SRI make any representations about the 
00022 // suitability of this software for any purpose.  It is provided 
00023 // "as is" without express or implied warranty.
00024 //
00025 // Kurt Konolige
00026 // Senior Computer Scientist
00027 // SRI International
00028 // 333 Ravenswood Avenue
00029 // Menlo Park, CA 94025
00030 // E-mail:  konolige@ai.sri.com
00031 
00032 //
00033 // Robot pose class, with probabilities
00034 //
00035 
00036 #ifndef SFMONTE_H
00037 #define SFMONTE_H
00038 
00039 #include "export.h"
00040 
00052 
00053 
00055 void mcSonarInit(void);
00057 void mcSonarInitRes(int res); 
00059 void *mcGetObject(void);
00062 void mcSetMove(int da, int ds, int tm);
00063 
00066 void mcUpdateRobotPose(int on);
00067 
00069 void mcPrintDuringUpdates(int on);
00070 
00072 void mcSetGain(int pct);
00073 
00075 void mcSetGauss(float dx, float dth);
00076 
00078 void mcSetNumSamples(int n);
00079 
00082 class SfPoseP : public ArPose
00083 {
00084  public:
00085   double prob;                  // how certain we are
00086 };
00087 
00088 // Error parameter class
00089 //   We're using systematic error parameters, because the robot
00090 //   seems to adhere to these statistics
00091 
00092 class SfErrParams               // error parameters of the robot
00093 {
00094  public:
00095   double range;                 // range error, m/m
00096   double angle;                 // angle error, rads/rad
00097   double drift;                 // drift error, rads/m
00098   SfErrParams(double r, double a, double d)
00099     { range = r; angle = a; drift = d; }
00100   SfErrParams()
00101     { range = 0; angle = 0; drift = 0; }
00102 };
00103 
00104 
00105 //
00106 // Sample class, for manipulating a set of localization samples
00107 //
00108 
00109 class SfUpdateSampleTask;               // forward ref, update async process
00110 
00111 class SfSamples : public SfGrid
00112 {
00113  public:
00114   SfPoseP *Samples;             // array of poses
00115   SfSamples(int, SfVector &); // constructor
00116   ~SfSamples() {};              // destructor
00117   void AddToFrame(SfFr *f); // add GUI stuff
00118   virtual void draw(SfWin *w); // drawing fn
00119 
00120   int ns;                       // current number of samples, should be private
00121   int ns_max;                   // max number of samples, ditto
00122   void SetNum(int n);   // number of samples
00123   int GetNum() { return ns; } // what we're using
00124   int CurrentSamples() { return ns; } // what we're using
00125 
00126   void SetUniform(double x, double y, double dx, double dy); 
00127   // initialize samples to uniform
00128   void SetUniform();    // use world size
00129   void SetGauss(double mx, double my, double mth, // pos (mm, deg), gauss ellipse
00130                          double th, double axis1, double axis2, double sth);
00131   void SmearSamples(double xydev, double thdev); 
00132   // smears uniformly; in mm and deg
00133   void Move(double dr, double da, double dir); // move by dead reckoning
00134   void MoveError(SfErrParams &e) // set error parameters
00135     { err = e; }
00136   void Update(bool init); // update pose samples 
00137   void Resample();     // resample that puppy 
00138   void Best(int n = 20); // find mean pose (approximate, using n samples)
00139   ArPose *best;                 // what the current best pose is 
00140   //  SfPoseCovar *bestvar;             // best position variance
00141 
00142 
00143   virtual bool Gain(int g); // sets the gain value, as a percent
00144   virtual bool SetGain(int g){ return Gain(g); }; // sets the gain value, as a percent
00145   int Gain()                    // returns gain as a percent
00146     { return (int)(100.01*gain); }
00147   int GetGain()                    // returns gain as a percent
00148     { return (int)(100.01*gain); }
00149 
00150   bool MaxProb(int g)           // sets max prob value for adaptive sampling, percent
00151     { if (g >= 0 && g <= 100)
00152         { maxprob = .01*(double)g; return true; }
00153       else return false;
00154       updateGUI();
00155     }
00156   bool SetMaxProb(int g) { return MaxProb(g); }
00157 
00158   int MaxProb()                 // returns maxprob as a percent
00159     { return (int)(100.01*maxprob); }
00160   int GetMaxProb()
00161     { return (int)(100.01*maxprob); }
00162 
00163   bool MinProb(int g)           // sets min prob value for adaptive sampling, percent
00164     { if (g >= 0 && g <= 100)
00165         { minprob = .01*(double)g; return true; }
00166       else return false;
00167     updateGUI();
00168     }
00169   bool SetMinProb(int g) { return MinProb(g); }
00170 
00171   int MinProb()                 // returns maxprob as a percent
00172     { return (int)(100.01*minprob); }
00173   int GetMinProb()
00174     { return (int)(100.01*minprob); }
00175 
00176 
00177   int MaxSamples()              // return max number of samples allowed
00178     { return (int)maxsamples; }
00179   int GetMaxSamples()           // return max number of samples allowed
00180     { return (int)maxsamples; }
00181 
00182   bool MaxSamples(int n)        // set max samples, less than ns_max
00183     { if (n >= minsamples)
00184         { if (n > ns_max) n = ns_max; maxsamples = n; return true; }
00185       else return false;
00186       updateGUI();
00187     }
00188   bool GetMaxSamples(int n) { return MaxSamples(n); }   // set max samples, less than ns_max
00189 
00190   int MinSamples()              // return min number of samples allowed
00191     { return (int)minsamples; }
00192   int GetMinSamples()           // return min number of samples allowed
00193     { return (int)minsamples; }
00194 
00195   bool MinSamples(int n)        // set min samples
00196     { if (n >= 100 && n <= maxsamples)
00197         { minsamples = n; return true; }
00198       else return false;
00199       updateGUI();
00200     }
00201   bool SetMinSamples(int n) { return MinSamples(n); } // set min samples
00202 
00203   int ProbCalcCycles()          // return number of cycles to calc 
00204     { return (prob_calc_cycles); }
00205   int GetProbCalcCycles()       // return number of cycles to calc 
00206     { return (prob_calc_cycles); }
00207 
00208   void ProbCalcCycles(int n)    // set number of cycles to calc
00209     { prob_calc_cycles = n; }
00210   void SetProbCalcCycles(int n) { ProbCalcCycles(n); } // set number of cycles to calc
00211 
00212   // Functions to be subsumed by subclasses
00213   virtual void PostProbInit() {} // initialize post prob calculation
00214   virtual void PostProb(SfPoseP *, int n) {} // returns posterior prob over poses
00215   virtual void DoMapUpdate() {}  // for map building
00216 
00217   int NumLocalizations(void) { return numLocalizations; } // return number of times we've localized
00218 
00219   // Display flags -- should go through fns
00220   bool displaySamples;          // sample pt display flag
00221   bool displayBest;             // best pose display flag
00222   void *monte_params;           // for parameters pop-up window
00223   void DisplaySamples(bool on)
00224     { displaySamples = on; updateGUI(); }
00225   void DisplayBest(bool on)
00226     { displayBest = on; updateGUI(); }
00227   void DisplayGrid(bool on)
00228     { displayGrid = on; updateGUI(); }
00229 
00230   // Localization params
00231   bool updateFlag;              // whether we're localizing or not
00232   bool updatePoseFlag;          // whether we update robot pose after localize step
00233   void UpdateFlag(bool on)
00234     { updateFlag = on; updateGUI(); }
00235   void UpdatePoseFlag(bool on)
00236     { updatePoseFlag = on; updateGUI(); }
00237 
00238   bool printDuringUpdates;      // whether we want to print out on the updates or not
00239   bool updateMapFlag;           // whether we do map-building, LRF only...
00240   void UpdateMapFlag(bool on)
00241     { updateMapFlag = on; }
00242   int move_ds, move_da, move_time; // how far we need to move, in mm and deg; timeout after moving
00243   void SetMoveDs(int x)
00244     { move_ds = x; updateGUI(); }
00245   void SetMoveDa(int x)
00246     { move_da = x; updateGUI(); }
00247   void SetMoveTime(int x)
00248     { move_time = x; updateGUI(); }
00249 
00250   int new_update;               // how many updates we've done
00251   ArPose rpos;                  // keep track of where we started
00252   void InitUpdate();    // set up parameters
00253   void Globalize();     // initializes globalization process
00254   void ResetAsyncTask();
00255   void KillAsyncTask();
00256 
00257   // update GUI
00258   void updateGUI();
00259 
00260 
00261  private:
00262   SfErrParams err;              // movement error params
00263   SfPoseP *resampled;           // for resampling
00264   double *probs;                // cumulative probs for resampling
00265   SfTime *tt;                   // timer
00266   bool reset_flag;              // if we want to reset samples and re-globalize
00267   int  prob_calc_cycles;        // how many Saphira cycles for async calc, 0 for inf
00268   int state;
00269 
00270  protected:
00271   double gain;                  // how much we trust the sensor
00272 
00273   int numLocalizations;         // how may times we've localized
00274   // async break
00275   bool stop_prob_calc;          // if we want to stop the async prob calculation
00276 
00277   // adaptive sampling
00278   int minsamples, maxsamples;   // bounds for number of samples
00279   double minprob, maxprob;      // bounds for deleting or adding samples
00280 
00281   // localization processing
00282   void Process();               // we check robot position, synchronize with
00283   // localization processing in a thread
00284   ArRecurrentTask *utask;       // asynchronous update process
00285   double move_d, move_dth, move_a; // movement values
00286 };
00287 
00288 // Set this function to initialize GUI parameters, on instantiation
00289 extern void (* sfSampleFrameFn)(SfSamples *x, SfFr *f);
00290 extern void (* sfSampleUpdateFn)(SfSamples *x, SfFr *f);
00291 
00292 //
00293 // SfUpdateSampleTask -- subclass of SfAsyncTask
00294 //
00295 
00296 class SfUpdateSampleTask : public ArRecurrentTask
00297 {
00298  public:
00299   SfUpdateSampleTask(SfSamples *s); // sample instance 
00300   void task();          // async updating task
00301  private:
00302   SfSamples *samples;
00303 };
00304 
00305 
00306 //
00307 // Subclass SfSonarSamples -- uses sonar incidence function for
00308 //   probability calculation
00309 //
00310 
00311 #define MONTENUMSONARS 32
00312 
00313 class SfSonarSamples : public SfSamples
00314 {
00315  public:
00316   SfSonarSamples(int num, SfVector &s); // number of samples, size of grid
00317   void PostProbInit();          // set up LRF readings
00318   void PostProb(SfPoseP *p, int n); // posterior prob calculation
00319   bool Gain(int g);     // have to do this to recalc PSD table
00320 
00321  private:
00322   //  SfSensorReading *sensor;  // which sensor we're using
00323   int sonarRange[MONTENUMSONARS];
00324   int start_sonar;              // which sonar range we use for update
00325 };
00326 
00327 extern SfSamples *montePs;
00328 #define MONTERANGE .02          // range error, m/m
00329 #define MONTEANGLE .05          // angle error, rads/rad
00330 #define MONTEDRIFT .00001               // drift error, rads/mm
00331 #define MONTENUM_SAMPLES 2000
00332 
00333 
00334 #endif // SFMONTE_H

Generated on Tue Nov 12 17:49:34 2002 for Saphira by doxygen1.2.13.1 written by Dimitri van Heesch, © 1997-2001