00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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;
00086 };
00087
00088
00089
00090
00091
00092 class SfErrParams
00093 {
00094 public:
00095 double range;
00096 double angle;
00097 double drift;
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
00107
00108
00109 class SfUpdateSampleTask;
00110
00111 class SfSamples : public SfGrid
00112 {
00113 public:
00114 SfPoseP *Samples;
00115 SfSamples(int, SfVector &);
00116 ~SfSamples() {};
00117 void AddToFrame(SfFr *f);
00118 virtual void draw(SfWin *w);
00119
00120 int ns;
00121 int ns_max;
00122 void SetNum(int n);
00123 int GetNum() { return ns; }
00124 int CurrentSamples() { return ns; }
00125
00126 void SetUniform(double x, double y, double dx, double dy);
00127
00128 void SetUniform();
00129 void SetGauss(double mx, double my, double mth,
00130 double th, double axis1, double axis2, double sth);
00131 void SmearSamples(double xydev, double thdev);
00132
00133 void Move(double dr, double da, double dir);
00134 void MoveError(SfErrParams &e)
00135 { err = e; }
00136 void Update(bool init);
00137 void Resample();
00138 void Best(int n = 20);
00139 ArPose *best;
00140
00141
00142
00143 virtual bool Gain(int g);
00144 virtual bool SetGain(int g){ return Gain(g); };
00145 int Gain()
00146 { return (int)(100.01*gain); }
00147 int GetGain()
00148 { return (int)(100.01*gain); }
00149
00150 bool MaxProb(int g)
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()
00159 { return (int)(100.01*maxprob); }
00160 int GetMaxProb()
00161 { return (int)(100.01*maxprob); }
00162
00163 bool MinProb(int g)
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()
00172 { return (int)(100.01*minprob); }
00173 int GetMinProb()
00174 { return (int)(100.01*minprob); }
00175
00176
00177 int MaxSamples()
00178 { return (int)maxsamples; }
00179 int GetMaxSamples()
00180 { return (int)maxsamples; }
00181
00182 bool MaxSamples(int n)
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); }
00189
00190 int MinSamples()
00191 { return (int)minsamples; }
00192 int GetMinSamples()
00193 { return (int)minsamples; }
00194
00195 bool MinSamples(int n)
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); }
00202
00203 int ProbCalcCycles()
00204 { return (prob_calc_cycles); }
00205 int GetProbCalcCycles()
00206 { return (prob_calc_cycles); }
00207
00208 void ProbCalcCycles(int n)
00209 { prob_calc_cycles = n; }
00210 void SetProbCalcCycles(int n) { ProbCalcCycles(n); }
00211
00212
00213 virtual void PostProbInit() {}
00214 virtual void PostProb(SfPoseP *, int n) {}
00215 virtual void DoMapUpdate() {}
00216
00217 int NumLocalizations(void) { return numLocalizations; }
00218
00219
00220 bool displaySamples;
00221 bool displayBest;
00222 void *monte_params;
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
00231 bool updateFlag;
00232 bool updatePoseFlag;
00233 void UpdateFlag(bool on)
00234 { updateFlag = on; updateGUI(); }
00235 void UpdatePoseFlag(bool on)
00236 { updatePoseFlag = on; updateGUI(); }
00237
00238 bool printDuringUpdates;
00239 bool updateMapFlag;
00240 void UpdateMapFlag(bool on)
00241 { updateMapFlag = on; }
00242 int move_ds, move_da, move_time;
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;
00251 ArPose rpos;
00252 void InitUpdate();
00253 void Globalize();
00254 void ResetAsyncTask();
00255 void KillAsyncTask();
00256
00257
00258 void updateGUI();
00259
00260
00261 private:
00262 SfErrParams err;
00263 SfPoseP *resampled;
00264 double *probs;
00265 SfTime *tt;
00266 bool reset_flag;
00267 int prob_calc_cycles;
00268 int state;
00269
00270 protected:
00271 double gain;
00272
00273 int numLocalizations;
00274
00275 bool stop_prob_calc;
00276
00277
00278 int minsamples, maxsamples;
00279 double minprob, maxprob;
00280
00281
00282 void Process();
00283
00284 ArRecurrentTask *utask;
00285 double move_d, move_dth, move_a;
00286 };
00287
00288
00289 extern void (* sfSampleFrameFn)(SfSamples *x, SfFr *f);
00290 extern void (* sfSampleUpdateFn)(SfSamples *x, SfFr *f);
00291
00292
00293
00294
00295
00296 class SfUpdateSampleTask : public ArRecurrentTask
00297 {
00298 public:
00299 SfUpdateSampleTask(SfSamples *s);
00300 void task();
00301 private:
00302 SfSamples *samples;
00303 };
00304
00305
00306
00307
00308
00309
00310
00311 #define MONTENUMSONARS 32
00312
00313 class SfSonarSamples : public SfSamples
00314 {
00315 public:
00316 SfSonarSamples(int num, SfVector &s);
00317 void PostProbInit();
00318 void PostProb(SfPoseP *p, int n);
00319 bool Gain(int g);
00320
00321 private:
00322
00323 int sonarRange[MONTENUMSONARS];
00324 int start_sonar;
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