diff --git a/FileReader.h b/FileReader.h deleted file mode 100644 index 6b10c89..0000000 --- a/FileReader.h +++ /dev/null @@ -1,252 +0,0 @@ -#ifndef FILEREADER_H -#define FILEREADER_H - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -class FileReader { - -public: - - template struct TS { - const uint64_t ts; - T data; - TS(const uint64_t ts) : ts(ts) {;} - TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;} - }; - - enum class Sensor { - ACC, - GYRO, - WIFI, - POS, - BARO, - BEACON, - }; - - /** entry for one sensor */ - struct Entry { - Sensor type; - uint64_t ts; - int idx; - Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;} - }; - - std::vector> groundTruth; - std::vector> wifi; - std::vector> beacon; - std::vector> acc; - std::vector> gyro; - std::vector> barometer; - - /** ALL entries */ - std::vector entries; - -public: - - FileReader(const std::string& file) { - parse(file); - } - - const std::vector& getEntries() const {return entries;} - - - const std::vector>& getGroundTruth() const {return groundTruth;} - - const std::vector>& getWiFiGroupedByTime() const {return wifi;} - - const std::vector>& getBeacons() const {return beacon;} - - const std::vector>& getAccelerometer() const {return acc;} - - const std::vector>& getGyroscope() const {return gyro;} - - const std::vector>& getBarometer() const {return barometer;} - -private: - - void parse(const std::string& file) { - - std::ifstream inp(file); - if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);} - - while(!inp.eof() && !inp.bad()) { - - uint64_t ts; - char delim; - int idx = -1; - std::string data; - - inp >> ts; - inp >> delim; - inp >> idx; - inp >> delim; - inp >> data; - - if (idx == 8) {parseWiFi(ts, data);} - else if (idx == 9) {parseBeacons(ts, data);} - else if (idx == 99) {parseGroundTruth(ts, data);} - else if (idx == 0) {parseAccelerometer(ts, data);} - else if (idx == 3) {parseGyroscope(ts, data);} - else if (idx == 5) {parseBarometer(ts, data);} - - // TODO: this is a hack... - // the loop is called one additional time after the last entry - // and keeps the entries of entry - - } - - inp.close(); - - } - - void parseAccelerometer(const uint64_t ts, const std::string& data) { - - const int pos1 = data.find(';'); - const int pos2 = data.find(';', pos1+1); - - const std::string x = data.substr(0, pos1); - const std::string y = data.substr(pos1+1, pos2-pos1-1); - const std::string z = data.substr(pos2+1); - - TS elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z))); - acc.push_back(elem); - entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1)); - - } - - void parseGyroscope(const uint64_t ts, const std::string& data) { - - const int pos1 = data.find(';'); - const int pos2 = data.find(';', pos1+1); - - const std::string x = data.substr(0, pos1); - const std::string y = data.substr(pos1+1, pos2-pos1-1); - const std::string z = data.substr(pos2+1); - - TS elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z))); - gyro.push_back(elem); - entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1)); - - } - - void parseWiFi(const uint64_t ts, const std::string& data) { - - std::string tmp = data; - - // add new wifi reading - wifi.push_back(TS(ts, WiFiMeasurements())); - entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1)); - - // process all APs - while(!tmp.empty()) { - - std::string mac = tmp.substr(0, 17); - tmp = tmp.substr(17); - assert(tmp[0] == ';'); tmp = tmp.substr(1); - - std::string freq = tmp.substr(0, 4); - tmp = tmp.substr(4); - assert(tmp[0] == ';'); tmp = tmp.substr(1); - - int pos = tmp.find(';'); - std::string rssi = tmp.substr(0, pos); - tmp = tmp.substr(pos); - assert(tmp[0] == ';'); tmp = tmp.substr(1); - - // append AP to current scan-entry - WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts)); - wifi.back().data.entries.push_back(e); - } - - } - - void parseBeacons(const uint64_t ts, const std::string& data) { - - std::string tmp = data; - - std::string mac = tmp.substr(0, 17); - tmp = tmp.substr(17); - assert(tmp[0] == ';'); tmp = tmp.substr(1); - - std::string rssi = tmp; - - //yes the timestamp is redundant here, but in case of multiusage... - TS e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi))); - beacon.push_back(e); - entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1)); - - } - - void parseGroundTruth(const uint64_t ts, const std::string& data) { - - const int pos1 = data.find(';'); - std::string gtIndex = data.substr(0, pos1); - - TS elem(ts, std::stoi(gtIndex)); - groundTruth.push_back(elem); - - } - - void parseBarometer(const uint64_t ts, const std::string& data) { - - const int pos1 = data.find(';'); - - const std::string hPa = data.substr(0, pos1); - - TS elem(ts, BarometerData(std::stof(hPa))); - barometer.push_back(elem); - entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); - - } - - const Interpolator getGroundTruthPath(Floorplan::IndoorMap& map, std::vector gtPath) const { - - // finde alle positionen der waypoints im gtPath aus map - std::unordered_map waypointsMap; - for(Floorplan::Floor* f : map.floors){ - for (Floorplan::POI* poi : f->pois){ - waypointsMap.insert({std::stoi(poi->name), poi->pos}); - } - } - - // bringe aufgenommene gt punkte der app in unordered_map - std::unordered_map waypointsApp; - for(TS val : groundTruth){ - waypointsApp.insert({val.data, val.ts}); - } - - // bringe diese in richtige reihenfolge und füge timestamp hinzu - Interpolator interpol; - for(int id : gtPath){ - auto itMap = waypointsMap.find(id); - if(itMap == waypointsMap.end()) {throw "not found";} - - auto itApp = waypointsApp.find(id); - if(itApp == waypointsApp.end()) {throw "not found";} - - interpol.add(itApp->second, itMap->second); - - } - - return interpol; - } - -}; - -#endif // FILEREADER_H diff --git a/Settings.h b/Settings.h index 85b4bc7..fdc0f52 100644 --- a/Settings.h +++ b/Settings.h @@ -10,9 +10,9 @@ namespace Settings { const int numParticles = 5000; namespace IMU { - const float turnSigma = 1.5; // 3.5 - const float stepLength = 0.80; - const float stepSigma = 0.40; //toni changed + const float turnSigma = 2.5; // 3.5 + const float stepLength = 1.00; + const float stepSigma = 0.15; //toni changed } const float smartphoneAboveGround = 1.3; @@ -32,9 +32,9 @@ namespace Settings { } namespace WiFiModel { - constexpr float sigma = 13.0; + constexpr float sigma = 8.0; /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ - constexpr float TXP = -48; + constexpr float TXP = -46; constexpr float EXP = 2.5; constexpr float WAF = -5.0; @@ -47,10 +47,10 @@ namespace Settings { } namespace BeaconModel { - constexpr float sigma = 13.0; - constexpr float TXP = -48; + constexpr float sigma = 8.0; + constexpr float TXP = -71; constexpr float EXP = 1.5; - constexpr float WAF = -8.0; //-5 + constexpr float WAF = -20.0; //-5 //20?? } namespace MapView3D { diff --git a/filter/Logic.h b/filter/Logic.h index 2873668..56599f3 100644 --- a/filter/Logic.h +++ b/filter/Logic.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -30,7 +31,7 @@ #include "../Settings.h" -/** particle-filter init */ +/** particle-filter init randomly distributed within the building*/ struct PFInit : public K::ParticleFilterInitializer { Grid& grid; @@ -51,6 +52,35 @@ struct PFInit : public K::ParticleFilterInitializer { }; +/** particle-filter init with fixed position*/ +struct PFInitFixed : public K::ParticleFilterInitializer { + + Grid& grid; + GridPoint startPos; + float headingDeg; + + PFInitFixed(Grid& grid, GridPoint startPos, float headingDeg) : + grid(grid), startPos(startPos), headingDeg(headingDeg) {;} + + virtual void initialize(std::vector>& particles) override { + + Distribution::Normal norm(0.0f, 1.5f); + + for (K::Particle& p : particles) { + + GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f); + + GridPoint startPos = grid.getNodeFor(pos); + p.state.position = startPos; // scatter arround the start position + p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed heading + p.state.heading.error = 0; + p.state.relativePressure = 0; // start with a relative pressure of 0 + + } + } + +}; + /** particle-filter transition */ struct PFTrans : public K::ParticleFilterTransition { @@ -60,6 +90,7 @@ struct PFTrans : public K::ParticleFilterTransition { WalkModuleHeading modHeadUgly; // stupid WalkModuleHeadingControl modHead; + WalkModuleHeadingVonMises modHeadMises; WalkModuleNodeImportance modImportance; WalkModuleSpread modSpread; WalkModuleFavorZ modFavorZ; @@ -68,9 +99,10 @@ struct PFTrans : public K::ParticleFilterTransition { std::minstd_rand gen; - PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) { + PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) { - walker.addModule(&modHead); + //walker.addModule(&modHead); + walker.addModule(&modHeadMises); //walker.addModule(&modSpread); // might help in some situations! keep in mind! //walker.addModule(&modHeadUgly); @@ -136,7 +168,11 @@ struct PFEval : public K::ParticleFilterEvaluation { /** probability for BEACONS */ inline double getBEACON(const MyObs& observation, const GridPoint& point){ - return beaconProbability.getProbability(point.inMeter(), observation.currentTime, observation.beacons); + + //consider adding the persons height + Point3 p = point.inMeter() + Point3(0,0,1.3); + + return beaconProbability.getProbability(p, observation.currentTime, observation.beacons); } /** probability for Barometer */ @@ -156,12 +192,13 @@ struct PFEval : public K::ParticleFilterEvaluation { const double pWifi = getWIFI(observation, wifiObs, p.state.position); const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure); + const double pBeacon = getBEACON(observation, p.state.position); //small checks - _assertNotNAN(pWifi, "pups"); + _assertNotNAN(pWifi, "Wifi prob is nan"); _assertNot0(pBaroPressure,"pBaroPressure is null"); - const double prob = pWifi*pBaroPressure; + const double prob = pWifi * pBaroPressure * pBeacon; p.weight = prob; sum += (prob); diff --git a/filter/Structs.h b/filter/Structs.h index 3b3282a..cdd6329 100644 --- a/filter/Structs.h +++ b/filter/Structs.h @@ -21,9 +21,11 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpre static Floorplan::IndoorMap* map; - float relativePressure = 0; + float relativePressure = 0.0f; + GridPoint positionOld; + MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;} MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;} @@ -53,6 +55,9 @@ struct MyControl { /** number of steps since the last transition */ int numStepsSinceLastTransition = 0; + /** current motion delta angle*/ + int motionDeltaAngle_rad = 0; + /** reset the control-data after each transition */ void resetAfterTransition() { turnSinceLastTransition_rad = 0; diff --git a/main.cpp b/main.cpp index 27bd53d..3b02dff 100755 --- a/main.cpp +++ b/main.cpp @@ -1,4 +1,3 @@ -#include "FileReader.h" #include #include "filter/Structs.h" @@ -15,12 +14,13 @@ #include #include - +#include #include #include #include +#include #include #include #include @@ -62,13 +62,13 @@ struct DataSetup { std::string wifiParams; int minWifiOccurences; VAPGrouper::Mode vapMode; - int buildingNum; + std::string grid; }; /** all configured datasets */ struct Data { - DataSetup SHL = { + DataSetup BERKWERK = { mapDir + "SHL/SHL25.xml", @@ -86,7 +86,27 @@ struct Data { dataDir + "bergwerk/wifiParams.txt", 40, VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, - 10 + mapDir + "SHL/grid25.dat" + }; + + DataSetup IPIN2015 = { + + mapDir + "SHL/SHL_IPIN2015_gt.xml", + + { + dataDir + "ipin2015/galaxy/Path0/1433581471902.csv", + dataDir + "ipin2015/nexus/Path0/1433606195078.csv", + dataDir + "ipin2015/galaxy/Path1/1433587749492.csv", + dataDir + "ipin2015/nexus/Path1/1433606670723.csv", + dataDir + "ipin2015/galaxy/Path2/1433581471902.csv", + dataDir + "ipin2015/nexus/Path2/1433607251262.csv", + dataDir + "eiszeit/path2/1479986737368.csv" + }, + + dataDir + "bergwerk/wifiParams.txt", + 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, + mapDir + "SHL/grid_IPIN2015_gt.dat" }; } data; @@ -105,22 +125,25 @@ void run(DataSetup setup, int numFile, std::string folder) { Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); BeaconModelLogDistCeiling beaconModel(map); - BeaconModelLogDistCeiling::APEntry beacon1(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); - beaconModel.addBeacon(MACAddress("48:EF:8D:77:66:DF"), beacon1); + beaconModel.loadBeaconsFromMap(map, Settings::BeaconModel::TXP, Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); + Assert::isFalse(beaconModel.getAllBeacons().empty(), "no Beacons stored within the map.xml"); - BeaconModelLogDistCeiling::APEntry beacon2(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); - beaconModel.addBeacon(MACAddress("6F:5F:39:0C:51:E4"), beacon2); - - BeaconModelLogDistCeiling::APEntry beacon3(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); - beaconModel.addBeacon(MACAddress("49:23:D8:7F:E8:D2"), beacon3); - - //beaconModel.loadBeaconsFromMap(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); - Assert::isFalse(beaconModel.getAllBeacons().empty(), "no AccessPoints stored within the map.xml"); // build the grid + std::ifstream inp(setup.grid, std::ifstream::binary); Grid grid(20); - GridFactory factory(grid); - factory.build(map); + + // grid.dat empty? -> build one and save it + if (!inp.good() || (inp.peek()&&0) || inp.eof()) { + std::ofstream onp; + onp.open(setup.grid); + GridFactory factory(grid); + factory.build(map); + grid.write(onp); + } else { + grid.read(inp); + } + // add node-importance Importance::addImportance(grid); @@ -128,14 +151,16 @@ void run(DataSetup setup, int numFile, std::string folder) { // stamp WiFi signal-strengths onto the grid WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround); - // reading file FileReader fr(setup.training[numFile]); - //std::vector gt; + // doing ground truth stuff - //fr.getGroundTruthPath(map, gt_1); + std::vector path_0 = {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 path_1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; + std::vector path_2 = {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}; + Interpolator gtInterpolator = fr.getGroundTruthPath(map, path_1); Plotti plot; @@ -151,20 +176,21 @@ void run(DataSetup setup, int numFile, std::string folder) { ctrl.resetAfterTransition(); MyObs obs; - int numParticles = 5000; + int numParticles = 10000; PFEval* eval = new PFEval(WiFiModel, beaconModel, grid); //filter init //std::unique_ptr init = - K::ParticleFilterHistory pf(numParticles, std::unique_ptr(new PFInit(grid))); + //K::ParticleFilterHistory pf(numParticles, std::unique_ptr(new PFInit(grid))); + K::ParticleFilterHistory pf(numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 1080.0f), 90.0f))); pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); pf.setEvaluation(std::unique_ptr(eval)); //resampling - //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); - pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.04))); - pf.setNEffThreshold(0.85); + pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); + //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.4))); + pf.setNEffThreshold(0.95); //estimation //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); @@ -177,11 +203,13 @@ void run(DataSetup setup, int numFile, std::string folder) { StepDetection sd; TurnDetection td; + MotionDetection md; + RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); K::Statistics errorStats; - //file writing for offline competition + //file writing for error data long int t = static_cast(time(NULL)); std::ofstream errorFile; errorFile.open (errorDir + folder + "/error_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); @@ -209,22 +237,34 @@ void run(DataSetup setup, int numFile, std::string folder) { } else if (e.type == FileReader::Sensor::GYRO) { const FileReader::TS& _gyr = fr.getGyroscope()[e.idx]; - const float delta = td.addGyroscope(ts, _gyr.data); - ctrl.turnSinceLastTransition_rad += delta; + const float delta_gyro = td.addGyroscope(ts, _gyr.data); + + ctrl.turnSinceLastTransition_rad += delta_gyro; } else if (e.type == FileReader::Sensor::BARO) { relBaro.add(ts, fr.getBarometer()[e.idx].data); obs.relativePressure = relBaro.getPressureRealtiveToStart(); obs.sigmaPressure = relBaro.getSigma(); + + } else if (e.type == FileReader::Sensor::LIN_ACC) { + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + + } else if (e.type == FileReader::Sensor::GRAVITY) { + md.addGravity(ts, fr.getGravity()[e.idx].data); + Eigen::Vector2f curVec = md.getCurrentMotionAxis(); + ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad(); } - if (ts.ms() - lastTimestamp.ms() > 500) { + if (ts.ms() - lastTimestamp.ms() > 500) { obs.currentTime = ts; MyState est = pf.update(&ctrl, obs); Point3 estPos = est.position.inMeter(); + //current ground truth position + Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())); + plot.pInterest.clear(); // plotting stuff @@ -238,7 +278,7 @@ void run(DataSetup setup, int numFile, std::string folder) { //plot.debugProb(grid, std::bind(&PFEval::getALL, eval, std::placeholders::_1, std::placeholders::_2), obs); plot.setEst(estPos); - //plot.setGT(mapPos); + plot.setGT(gtPos); plot.addEstimationNode(estPos); plot.addParticles(pf.getParticles()); //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; @@ -249,12 +289,12 @@ void run(DataSetup setup, int numFile, std::string folder) { //plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << ctrl.barometer.act << "'\n"; // error between GT and estimation - //float err_m = mapPos.getDistance(estPos); - // errorStats.add(err_m); - //errorFile << err_m << "\n"; + float err_m = gtPos.getDistance(estPos); + errorStats.add(err_m); + errorFile << err_m << "\n"; plot.show(); - usleep(100*10); + usleep(33*10); lastTimestamp = ts; @@ -267,6 +307,9 @@ void run(DataSetup setup, int numFile, std::string folder) { errorFile.close(); + std::cout << "Statistical Analysis: " << std::endl; + std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << std::endl; + //Write the current plotti buffer into file std::ofstream plotFile; plotFile.open(errorDir + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); @@ -295,7 +338,7 @@ void run(DataSetup setup, int numFile, std::string folder) { int main(int argc, char** argv) { //Testing files - run(data.SHL, 6, "EVAL"); // Nexus vor - + //run(data.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor + run(data.IPIN2015, 3, "EVALIPIN2015"); // Nexus Path2 }