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
00037
00038 #include "Neuro/SaccadeControllers.H"
00039
00040 #include "Component/GlobalOpts.H"
00041 #include "Component/OptionManager.H"
00042 #include "Image/Pixels.H"
00043 #include "Media/MediaOpts.H"
00044 #include "Neuro/NeuroOpts.H"
00045 #include "Neuro/NeuroSimEvents.H"
00046 #include "Neuro/Retina.H"
00047 #include "Neuro/SpatialMetrics.H"
00048 #include "Simulation/SimulationOpts.H"
00049 #include "Simulation/SimEventQueue.H"
00050 #include "Util/MathFunctions.H"
00051 #include "Util/log.H"
00052 #include "rutz/compat_cmath.h"
00053
00054 #include <fstream>
00055
00056
00057
00058
00059 StubSaccadeController::StubSaccadeController(OptionManager& mgr,
00060 const SaccadeBodyPart bodypart) :
00061 SaccadeController(mgr, "Stub Saccade Controller",
00062 "StubSaccadeController", bodypart, 2, 2)
00063 { }
00064
00065
00066 StubSaccadeController::~StubSaccadeController()
00067 { }
00068
00069
00070 void StubSaccadeController::doEvolve(SimEventQueue& q)
00071 { }
00072
00073
00074 void StubSaccadeController::computeWhenNewPercept(SimEventQueue& q)
00075 { }
00076
00077
00078 void StubSaccadeController::computeWhenResetPos(SimEventQueue& q)
00079 { }
00080
00081
00082 Point2D<int> StubSaccadeController::
00083 computeWhenNewDecision(SaccadeState& sacstate, bool& blinkstate,
00084 SimEventQueue& q)
00085 {
00086 sacstate = SACSTATE_UNK;
00087 blinkstate = false;
00088 return Point2D<int>(-1,-1);
00089 }
00090
00091
00092
00093
00094 TrivialSaccadeController::
00095 TrivialSaccadeController(OptionManager& mgr, const SaccadeBodyPart bodypart) :
00096 SaccadeController(mgr, "Trivial Saccade Controller",
00097 "TrivialSaccadeController", bodypart, 2, 2),
00098 itsMinSacLen(bodypart == SaccadeBodyPartEye
00099 ? &OPT_SCeyeMinSacLen
00100 : &OPT_SCheadMinSacLen,
00101 this)
00102 { }
00103
00104
00105 TrivialSaccadeController::~TrivialSaccadeController()
00106 { }
00107
00108
00109 void TrivialSaccadeController::doEvolve(SimEventQueue& q)
00110 { }
00111
00112
00113 void TrivialSaccadeController::computeWhenNewPercept(SimEventQueue& q)
00114 { }
00115
00116
00117 void TrivialSaccadeController::computeWhenResetPos(SimEventQueue& q)
00118 { }
00119
00120
00121 Point2D<int> TrivialSaccadeController::
00122 computeWhenNewDecision(SaccadeState& sacstate, bool& blinkstate,
00123 SimEventQueue& q)
00124 {
00125
00126 blinkstate = false;
00127
00128
00129 Point2D<int> p = getPreviousPercept(0).p;
00130
00131 if (p.isValid())
00132 {
00133
00134 Point2D<int> pp = getPreviousDecision(0).p;
00135 if (pp.isValid() && p.distance(pp) >= itsMinSacLen.getVal())
00136 sacstate = SACSTATE_SAC;
00137 else
00138 sacstate = SACSTATE_FIX;
00139 }
00140 else sacstate = SACSTATE_UNK;
00141
00142 return p;
00143 }
00144
00145
00146
00147
00148 FixedSaccadeController::
00149 FixedSaccadeController(OptionManager& mgr, const SaccadeBodyPart bodypart) :
00150 SaccadeController(mgr, "Fixed Saccade Controller",
00151 "FixedSaccadeController", bodypart, 2, 2),
00152 itsPos(-1, -1)
00153 { }
00154
00155
00156 FixedSaccadeController::~FixedSaccadeController()
00157 { }
00158
00159
00160 void FixedSaccadeController::doEvolve(SimEventQueue& q)
00161 { }
00162
00163
00164 void FixedSaccadeController::computeWhenNewPercept(SimEventQueue& q)
00165 { }
00166
00167
00168 void FixedSaccadeController::computeWhenResetPos(SimEventQueue& q)
00169 {
00170
00171
00172 itsPos = getPreviousPercept(0).p;
00173 }
00174
00175
00176 Point2D<int> FixedSaccadeController::
00177 computeWhenNewDecision(SaccadeState& sacstate, bool& blinkstate,
00178 SimEventQueue& q)
00179 {
00180 if (itsPos.isValid()) sacstate = SACSTATE_FIX;
00181 else sacstate = SACSTATE_UNK;
00182
00183
00184 blinkstate = false;
00185
00186
00187 return itsPos;
00188 }
00189
00190
00191
00192
00193 FrictionSaccadeController::
00194 FrictionSaccadeController(OptionManager& mgr, const SaccadeBodyPart part)
00195 :
00196 SaccadeController(mgr, "Friction Saccade Controller",
00197 "FrictionSaccadeController",
00198 part, 2, 2),
00199 itsSpringK(part == SaccadeBodyPartEye
00200 ? &OPT_SCeyeSpringK
00201 : &OPT_SCheadSpringK,
00202 this),
00203 itsFrictionMu(part == SaccadeBodyPartEye
00204 ? &OPT_SCeyeFrictionMu
00205 : &OPT_SCheadFrictionMu,
00206 this),
00207 itsDims(&OPT_InputFrameDims, this),
00208 itsTimeStep(&OPT_SimulationTimeStep, this),
00209 x(0.0), y(0.0), oldx(0.0), oldy(0.0),
00210 oldt(SimTime::SECS(-1.0))
00211 { }
00212
00213
00214 FrictionSaccadeController::~FrictionSaccadeController()
00215 { }
00216
00217
00218 void FrictionSaccadeController::doEvolve(SimEventQueue& q)
00219 {
00220 const SimTime t = q.now();
00221
00222 if (oldt.secs() < 0.0) return;
00223 if (havePercepts() == false) { oldt = t; return; }
00224
00225
00226 WTAwinner last = getPreviousPercept(0);
00227 double foax = last.p.i + 0.49999;
00228 double foay = last.p.j + 0.49999;
00229
00230
00231
00232
00233 const SimTime dt =
00234 SimTime::computeDeltaT((t - oldt), itsTimeStep.getVal());
00235 double ox = oldx, oy = oldy;
00236 oldx = x; oldy = y;
00237
00238 for (SimTime tt = oldt; tt < t; tt += dt)
00239 {
00240
00241 double k = itsSpringK.getVal(), mu = itsFrictionMu.getVal();
00242 double fx = k * (foax - x);
00243 double fy = k * (foay - y);
00244
00245
00246 double dts = dt.secs();
00247 double newx = 2.0 * x - ox + dts * (dts * fx - mu * (x - ox));
00248 double newy = 2.0 * y - oy + dts * (dts * fy - mu * (y - oy));
00249
00250
00251
00252
00253 ox = x; oy = y; x = newx; y = newy;
00254 }
00255
00256 oldt = t;
00257 }
00258
00259
00260 void FrictionSaccadeController::computeWhenNewPercept(SimEventQueue& q)
00261 {
00262
00263 if (oldt.secs() < 0.0)
00264 {
00265 WTAwinner last = getPreviousPercept(0);
00266 x = oldx = last.p.i + 0.49999;
00267 y = oldy = last.p.j + 0.49999;
00268 oldt = last.t;
00269 }
00270
00271
00272 }
00273
00274
00275 void FrictionSaccadeController::computeWhenResetPos(SimEventQueue& q)
00276 {
00277
00278 Point2D<int> newloc = getPreviousPercept(0).p;
00279
00280
00281
00282 x = newloc.i + 0.49999;
00283 y = newloc.j + 0.49999;
00284 oldx = x; oldy = y; oldt = getPreviousPercept(0).t;
00285 }
00286
00287
00288 Point2D<int> FrictionSaccadeController::
00289 computeWhenNewDecision(SaccadeState& sacstate, bool& blinkstate,
00290 SimEventQueue& q)
00291 {
00292
00293 blinkstate = false;
00294
00295 if (oldt.secs() < 0.0 || havePercepts() == false)
00296 { sacstate = SACSTATE_UNK; return Point2D<int>(-1, -1); }
00297
00298
00299
00300 if (sqrt((x-oldx)*(x-oldx) + (y-oldy)*(y-oldy)) > 0.001)
00301 sacstate = SACSTATE_SMO; else sacstate = SACSTATE_FIX;
00302
00303
00304 Point2D<int> fi = Point2D<int>(int(x), int(y));
00305
00306
00307 fi.clampToDims(itsDims.getVal());
00308
00309
00310 return fi;
00311 }
00312
00313
00314
00315
00316 ThresholdSaccadeController::
00317 ThresholdSaccadeController(OptionManager& mgr, SaccadeBodyPart part)
00318 :
00319 SaccadeController(mgr, "Threshold Saccade Controller",
00320 "ThresholdSaccadeController", part, 2, 2),
00321 itsOdist(part == SaccadeBodyPartEye
00322 ? &OPT_SCeyeThreshMinOvert
00323 : &OPT_SCheadThreshMinOvert,
00324 this),
00325 itsCdist(part == SaccadeBodyPartEye
00326 ? &OPT_SCeyeThreshMaxCovert
00327 : &OPT_SCheadThreshMaxCovert,
00328 this),
00329 itsMinNum(part == SaccadeBodyPartEye
00330 ? &OPT_SCeyeThreshMinNum
00331 : &OPT_SCheadThreshMinNum,
00332 this),
00333 itsSalWeight(part == SaccadeBodyPartEye
00334 ? &OPT_SCeyeThreshSalWeigh
00335 : &OPT_SCheadThreshSalWeigh,
00336 this),
00337 itsMetrics(new SpatialMetrics(mgr))
00338 {
00339 this->addSubComponent(itsMetrics);
00340 }
00341
00342
00343 void ThresholdSaccadeController::start1()
00344 {
00345 resetPqlen(itsMinNum.getVal());
00346 odistsq = float(itsMetrics->getFoveaRadius()) * itsOdist.getVal();
00347 odistsq *= odistsq;
00348 cdistsq = float(itsMetrics->getFoveaRadius()) * itsCdist.getVal();
00349 cdistsq *= cdistsq;
00350 didresetpos = false;
00351
00352 SaccadeController::start1();
00353 }
00354
00355
00356 ThresholdSaccadeController::~ThresholdSaccadeController()
00357 { }
00358
00359
00360 void ThresholdSaccadeController::doEvolve(SimEventQueue& q)
00361 { }
00362
00363
00364 void ThresholdSaccadeController::reset1()
00365 {
00366
00367 didresetpos = false;
00368
00369
00370 SaccadeController::reset1();
00371 }
00372
00373
00374 void ThresholdSaccadeController::checkPercepts(bool& areclose, Point2D<int>& avgp)
00375 {
00376
00377 float ii = 0.0f, jj = 0.0f, w = 0.0f; int idx = 0;
00378 while(1) {
00379 WTAwinner win = getPreviousPercept(idx);
00380 if (win.isValid() == false) break;
00381 float weight = itsSalWeight.getVal() ? win.sv : 1.0f;
00382 ii += float(win.p.i) * weight;
00383 jj += float(win.p.j) * weight;
00384 w += weight; ++ idx;
00385 }
00386 if (w) { avgp.i = int(ii / w + 0.4999f); avgp.j = int(jj / w + 0.4999f); }
00387 else { avgp.i = -1; avgp.j = -1; }
00388
00389
00390 idx = 0; areclose = true;
00391 while(1) {
00392 WTAwinner win = getPreviousPercept(idx);
00393 if (win.isValid() == false) break;
00394 if (avgp.squdist(win.p) > cdistsq) { areclose = false; break; }
00395 ++ idx;
00396 }
00397 }
00398
00399
00400 void ThresholdSaccadeController::computeWhenNewPercept(SimEventQueue& q)
00401 { }
00402
00403
00404 void ThresholdSaccadeController::computeWhenResetPos(SimEventQueue& q)
00405 {
00406
00407 resetloc = getPreviousPercept(0).p;
00408 didresetpos = true;
00409
00410
00411
00412
00413 }
00414
00415
00416 Point2D<int> ThresholdSaccadeController::
00417 computeWhenNewDecision(SaccadeState& sacstate, bool& blinkstate,
00418 SimEventQueue& q)
00419 {
00420
00421 blinkstate = false;
00422
00423
00424 if (didresetpos)
00425 { didresetpos = false; sacstate = SACSTATE_FIX; return resetloc; }
00426
00427
00428 if (havePercepts() == false)
00429 { sacstate = SACSTATE_UNK; return Point2D<int>(-1, -1); }
00430
00431
00432 bool areclose; Point2D<int> avgp;
00433 checkPercepts(areclose, avgp);
00434 const Point2D<int> prev = getPreviousDecision(0).p;
00435
00436
00437
00438
00439
00440 if (areclose && haveDecisions() == false)
00441 { sacstate = SACSTATE_SAC; return avgp; }
00442 if (areclose && avgp.squdist(prev) >= odistsq)
00443 { sacstate = SACSTATE_SAC; return avgp; }
00444
00445
00446
00447
00448
00449 if (prev.isValid()) sacstate = SACSTATE_FIX; else sacstate = SACSTATE_UNK;
00450 return prev;
00451 }
00452
00453
00454
00455
00456 ThresholdFrictionSaccadeController::
00457 ThresholdFrictionSaccadeController(OptionManager& mgr,
00458 SaccadeBodyPart part)
00459 :
00460 SaccadeController(mgr,
00461 "Threshold-Friction Saccade Controller",
00462 "ThresholdFrictionSaccadeController",
00463 part, 2, 2),
00464 itsMaxIdle(part == SaccadeBodyPartEye
00465 ? &OPT_SCeyeMaxIdleSecs
00466 : &OPT_SCheadMaxIdleSecs,
00467 this),
00468 itsMetrics(new SpatialMetrics(mgr)),
00469 tsc(new ThresholdSaccadeController(mgr, part)),
00470 fsc(new FrictionSaccadeController(mgr, part))
00471 {
00472 this->addSubComponent(itsMetrics);
00473 addSubComponent(tsc);
00474 addSubComponent(fsc);
00475 }
00476
00477
00478 ThresholdFrictionSaccadeController::~ThresholdFrictionSaccadeController()
00479 { }
00480
00481
00482 void ThresholdFrictionSaccadeController::doEvolve(SimEventQueue& q)
00483 {
00484
00485 fsc->evolve(q);
00486
00487
00488 tsc->evolve(q);
00489 }
00490
00491
00492 void ThresholdFrictionSaccadeController::
00493 computeWhenNewPercept(SimEventQueue& q)
00494 {
00495
00496 WTAwinner win = getPreviousPercept(0);
00497
00498
00499 tsc->setPercept(win, q);
00500
00501
00502
00503
00504
00505
00506 bool areclose; Point2D<int> avgp; tsc->checkPercepts(areclose, avgp);
00507 if (areclose)
00508 {
00509 const Point2D<int> flast = fsc->getPreviousPercept(0).p;
00510 if (avgp.isValid() && avgp != flast)
00511 {
00512 win.p = avgp;
00513 fsc->setPercept(win, q);
00514 }
00515 }
00516 }
00517
00518
00519 void ThresholdFrictionSaccadeController::computeWhenResetPos(SimEventQueue& q)
00520 {
00521
00522 const WTAwinner win = getPreviousPercept(0);
00523
00524
00525 fsc->resetPos(win.p, q);
00526
00527
00528 tsc->resetPos(win.p, q);
00529 }
00530
00531
00532 Point2D<int> ThresholdFrictionSaccadeController::
00533 computeWhenNewDecision(SaccadeState& sacstate, bool& blinkstate,
00534 SimEventQueue& q)
00535 {
00536 const SimTime t = q.now();
00537
00538
00539 blinkstate = false;
00540
00541
00542 Point2D<int> pf = fsc->getDecision(q);
00543
00544
00545 Point2D<int> pt = tsc->getDecision(q);
00546
00547
00548
00549
00550
00551
00552 if (pt.isValid() ||
00553 (pt.isValid() == false &&
00554 pf.isValid() == false &&
00555 haveDecisions() &&
00556 t - getPreviousDecision(0).t >
00557 itsMaxIdle.getVal()))
00558 {
00559
00560 if (pt.isValid() == false)
00561 {
00562 if (SeC<SimEventRetinaImage> e =
00563 q.check<SimEventRetinaImage>(this, SEQ_ANY))
00564 pt = e->center();
00565 else CLFATAL("Cannot home to center without a retina image!");
00566
00567
00568 tsc->resetPos(pt, q);
00569 }
00570
00571
00572
00573 fsc->resetPos(pt, q);
00574
00575
00576 sacstate = SACSTATE_SAC;
00577
00578
00579 return pt;
00580 }
00581
00582
00583
00584
00585
00586
00587 if (pf.isValid())
00588 {
00589
00590 sacstate = fsc->getState();
00591 return pf;
00592 }
00593
00594
00595
00596
00597 if (tsc->getState() == SACSTATE_SAC) sacstate = SACSTATE_SAC;
00598 else sacstate = fsc->getState();
00599 return getPreviousDecision(0).p;
00600 }
00601
00602
00603
00604
00605 MonkeySaccadeController::MonkeySaccadeController(OptionManager& mgr,
00606 SaccadeBodyPart part)
00607 :
00608 SaccadeController(mgr, "Monkey Saccade Controller",
00609 "MonkeySaccadeController", part, 2, 2),
00610 fsc(new FrictionSaccadeController(mgr, part)),
00611 itsMetrics(new SpatialMetrics(mgr))
00612 {
00613 oldt = SimTime::ZERO();
00614 addSubComponent(fsc);
00615 addSubComponent(itsMetrics);
00616 }
00617
00618
00619 MonkeySaccadeController::~MonkeySaccadeController()
00620 { }
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663 void MonkeySaccadeController::computeWhenNewPercept(SimEventQueue& q)
00664 {
00665
00666 WTAwinner win = getPreviousPercept(0);
00667
00668
00669 fsc->setPercept(win, q);
00670 }
00671
00672
00673 void MonkeySaccadeController::doEvolve(SimEventQueue& q)
00674 {
00675 const SimTime t = q.now();
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715 oldt = t;
00716 }
00717
00718
00719 void MonkeySaccadeController::computeWhenResetPos(SimEventQueue& q)
00720 {
00721
00722 WTAwinner win = getPreviousPercept(0);
00723
00724
00725 fsc->resetPos(win.p, q);
00726
00727
00728
00729 }
00730
00731
00732 Point2D<int> MonkeySaccadeController::
00733 computeWhenNewDecision(SaccadeState& sacstate, bool& blinkstate,
00734 SimEventQueue& q)
00735 {
00736
00737
00738
00739 Point2D<int> p = fsc->getDecision(q);
00740 if (p.isValid()) return p; else return getPreviousDecision(0).p;
00741 }
00742
00743
00744
00745
00746
00747
00748