#ifndef EVALBASE_H #define EVALBASE_H #include "../Settings.h" #include "../Helper.h" #include "../Vis.h" #include #include #include "GroundTruthWay.h" #include "../particles/MyState.h" #include "../particles/MyObservation.h" #include "../particles/MyEvaluation.h" #include "../particles/MyTransition.h" #include "../particles/MyInitializer.h" #include "../reader/SensorReader.h" #include "../reader/SensorReaderStep.h" #include "../reader/SensorReaderTurn.h" #include "../lukas/TurnObservation.h" #include "../lukas/StepObservation.h" #include "../toni/BarometerSensorReader.h" #include "../frank/WiFiSensorReader.h" #include "../frank/BeaconSensorReader.h" class EvalBase { protected: Grid grid; Helper::FHWSFloors floors; Vis vis; K::ParticleFilter* pf; SensorReader* sr; SensorReaderTurn* srt; SensorReaderStep* srs; std::string runName; GroundTruthWay gtw; std::vector way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; std::vector way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; std::vector way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29}; public: EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) { // build the grid Helper::buildTheGrid(grid, floors); // setup the visualisation vis.addFloor(floors.f0, floors.h0); vis.addFloor(floors.f1, floors.h1); vis.addFloor(floors.f2, floors.h2); vis.addFloor(floors.f3, floors.h3); } GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map& waypoints, std::vector ids) { // construct the ground-truth-path by using all contained waypoint ids std::vector path; for (int id : ids) { auto it = waypoints.find(id); assert(it != waypoints.end()); path.push_back(it->second); } // new created the timed path GroundTruthWay gtw; int i = 0; while (sr.hasNext()) { const SensorEntry se = sr.getNext(); if (se.data.empty()) {continue;} // why necessary?? if (se.idx == 99) { gtw.add(se.ts, path[i]); ++i; } } // ensure the sensor-data contained usable timestamps for the ground-truth mapping assert(i>0); sr.rewind(); return gtw; } void run() { // sensor numbers const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_linearAcceleration = 2; std::list turn_observations; std::list step_observations; //Create an BarometerSensorReader BarometerSensorReader baroSensorReader; //Read all turn Observations while(srt->hasNext()) { SensorEntryTurn set = srt->getNext(); TurnObservation to; to.ts = set.ts; to.delta_heading = set.delta_heading; to.delta_motion = set.delta_motion; turn_observations.push_back(to); } //Step Observations while(srs->hasNext()) { SensorEntryStep ses = srs->getNext(); StepObservation so; so.ts = ses.ts; step_observations.push_back(so); } // the to-be-evaluated observation MyObservation obs; std::vector pathEst; uint64_t lastTransitionTS = 0; int64_t start_time = -1; K::Statistics stats; int cnt = 0; // process each sensor reading while(sr->hasNext()) { // get the next sensor reading from the CSV const SensorEntry se = sr->getNext(); //start_time needed for time calculation of steps and turns obs.latestSensorDataTS = se.ts; if (start_time == -1) {start_time = se.ts;} int64_t current_time = se.ts - start_time; switch(se.idx) { case s_wifi: { obs.wifi = WiFiSensorReader::readWifi(se); break; } case s_beacons: { BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se); if (!boe.mac.empty()) { obs.beacons.entries.push_back(boe); } // add the observed beacon obs.beacons.removeOld(obs.latestSensorDataTS); break; } case s_barometer: { obs.barometer = baroSensorReader.readBarometer(se); break; } case s_linearAcceleration:{ baroSensorReader.readVerticalAcceleration(se); break; } } // scheduled transition every 500 ms if (lastTransitionTS == 0) {lastTransitionTS = se.ts;} for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) { //Steps are sorted in the list by timestamp. //If the current observation timestamp is bigger/equal //to the current step timestamp, use this step as observation //and remove it from the list. //The new first timestamp in the list will be then be the next one (timestamp-wise) StepObservation so; if(current_time >= step_observations.front().ts && !step_observations.empty()) { so.step = true; so.ts = current_time; obs.step = &so; step_observations.pop_front(); } else { so.step = false; so.ts = current_time; obs.step = &so; } TurnObservation to; //same principal as for steps is applied for turns if(current_time >= turn_observations.front().ts && !turn_observations.empty()) { to = turn_observations.front(); obs.turn = &to; turn_observations.pop_front(); } else { to.delta_heading = 0.0; to.delta_motion = 0.0; obs.turn = &to; } // timed updates ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS); // update the particle filter (transition + eval), estimate a new current position and add it to the estimated path const MyState est = pf->update(nullptr, obs); const Point3 curEst = est.pCur; // error calculation. compare ground-truth to estimation const Point3 curGT = gtw.getPosAtTime(se.ts - 750); const Point3 diff = curEst - curGT; // skip the first 8 scans due to uniform distribution start if (++cnt > 8) { pathEst.push_back(curEst); const float err = diff.length(); stats.add(err); std::cout << stats.asString() << std::endl; } // plot vis.clearStates(); for (const K::Particle p : pf->getParticles()) {vis.addState(p.state.walkState);} vis.setTimestamp(se.ts); vis.addGroundTruth(gtw); vis.addEstPath(pathEst); vis.setEstAndShould(curEst, curGT); vis.show();; } } { // vis.setShowParticles(false); // vis.setShowTime(false); // vis.setShowCurPos(false); // vis.debugProcess(0, pathEst, gtw, pf, layers); // std::ofstream out("/tmp/" + runName + ".data"); // out << vis.getDataset(); // out.close(); } sleep(1000); } }; #endif // EVALBASE_H