diff --git a/competition/src/competition/Optimizer.h b/competition/src/competition/Optimizer.h index e3260f8..ecb9bc7 100644 --- a/competition/src/competition/Optimizer.h +++ b/competition/src/competition/Optimizer.h @@ -61,6 +61,7 @@ public: Floorplan::IndoorMap* map; Scaler scaler; + VAPGrouper::Mode vapMode; std::vector records; // /** debug only */ @@ -98,7 +99,7 @@ public: public: - Optimizer(Floorplan::IndoorMap* map, Scaler scaler) : map(map), scaler(scaler) {;} + Optimizer(Floorplan::IndoorMap* map, Scaler scaler, VAPGrouper::Mode vapMode) : map(map), scaler(scaler), vapMode(vapMode) {;} /** add the given record */ void addRecord(const std::string& file) { @@ -157,7 +158,7 @@ public: wifiMap.clear(); // how to combine VAPs - const VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM); + const VAPGrouper vg(vapMode, VAPGrouper::Aggregation::MAXIMUM); // parse each training data file @@ -311,8 +312,8 @@ public: std::cout << "use more rounds for production" << std::endl; LeOpt opt(valRegion); - opt.setPopulationSize(250); // USE MORE FOR PRODUCTION - opt.setNumIerations(40); + opt.setPopulationSize(500); // USE MORE FOR PRODUCTION + opt.setNumIerations(100); opt.calculateOptimum(func, (float*) ¶ms); // using LeOpt = K::NumOptAlgoGenetic; diff --git a/competition/src/competition/Plotti.h b/competition/src/competition/Plotti.h index 79169ce..aa81c33 100644 --- a/competition/src/competition/Plotti.h +++ b/competition/src/competition/Plotti.h @@ -198,7 +198,7 @@ struct Plotti { int i = 0; for (const K::Particle& p : particles) { if (++i % 25 != 0) {continue;} - K::GnuplotPoint3 pos(p.state.pos.x_cm, p.state.pos.y_cm, p.state.pos.z_cm); + K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); pParticles.add(pos / 100.0f); } } diff --git a/competition/src/competition/Scaler.h b/competition/src/competition/Scaler.h index 25dd6b4..f9bc0ec 100644 --- a/competition/src/competition/Scaler.h +++ b/competition/src/competition/Scaler.h @@ -4,6 +4,13 @@ #include #include +struct IPIN { + double lat; + double lon; + double floorNr; + IPIN(const double lat, const double lon, const double floorNr) : lat(lat), lon(lon), floorNr(floorNr) {;} +}; + class Scaler { private: @@ -15,43 +22,73 @@ private: double rotDeg; double mPerPx; + Point2 mapCenter_m; + + double refLat; + double m_per_deg_lat; + double m_per_deg_lon; + + const float fixedFloorHeight = 4.0f; + public: - + /** image width, image height, map-center (lat/lon), map-rotation, meter<->pixel conversion factor */ Scaler(int w, int h, double cenLat, double cenLon, double rotDeg, double mPerPx) : w(w), h(h), cenLat(cenLat), cenLon(cenLon), rotDeg(rotDeg), mPerPx(mPerPx) { - ; + + mapCenter_m = Point2( (w*mPerPx/2), (h*mPerPx/2) ); + + refLat = cenLat / 180.0 * M_PI; + m_per_deg_lat = 111132.954 - 559.822 * std::cos( 2.0 * refLat ) + 1.175 * std::cos( 4.0 * refLat); + m_per_deg_lon = 111132.954 * std::cos ( refLat ); + } + + + /** convert from (x,y,z) to (lat,lon,floorNr) */ + IPIN toIPIN3(const float x, const float y, const float z) const { + + Point2 pos(x,y); + + // move to (0,0) + pos -= mapCenter_m; + + // undo the rotation + pos = pos.rotated(-rotDeg / 180 * M_PI); + + const double lat = cenLat + (pos.y / m_per_deg_lat); + const double lon = cenLon + (pos.x / m_per_deg_lon); + const double floorNr = z / fixedFloorHeight; + + return IPIN(lat, lon, floorNr); + + } + + IPIN toIPIN3(const Point3 p) const { + return toIPIN3(p.x, p.y, p.z); + } + Point3 convert3D(double lat, double lon, float floorNr) const { Point2 p2 = convert2D(lat, lon); - const float fixedFloorHeight = 4.0f; - return Point3(p2.x, p2.y, floorNr * fixedFloorHeight); } - Point2 convert2D(double lat, double lon) const { - - const double refLat = cenLat / 180.0 * M_PI; - const double m_per_deg_lat = 111132.954 - 559.822 * std::cos( 2.0 * refLat ) + 1.175 * std::cos( 4.0 * refLat); - const double m_per_deg_lon = 111132.954 * std::cos ( refLat ); - - - const double y_m = +(lat-cenLat) * m_per_deg_lat; const double x_m = +(lon-cenLon) * m_per_deg_lon; + // rotate (our map is axis aligned) Point2 pos(x_m, y_m); pos = pos.rotated(rotDeg / 180 * M_PI); - const Point2 center( (w*mPerPx/2), (h*mPerPx/2) ); - pos += center; + // apply movement + pos += mapCenter_m; return pos; diff --git a/competition/src/competition/filter/Logic.h b/competition/src/competition/filter/Logic.h index c1ff4bd..ad25bef 100644 --- a/competition/src/competition/filter/Logic.h +++ b/competition/src/competition/filter/Logic.h @@ -10,7 +10,7 @@ #include #include #include -#include +//#include #include #include @@ -20,9 +20,9 @@ #include "../Scaler.h" #include "Structs.h" -struct MyWalkState : public WalkState, public WalkStateHeading, public WalkStateRelativePressure { - MyWalkState(const GridPoint& gp, const Heading head, const float relativePressure) : WalkState(gp), WalkStateHeading(head), WalkStateRelativePressure(relativePressure) {;} -}; +//struct MyWalkState : public WalkState, public WalkStateHeading, public WalkStateRelativePressure { +// MyWalkState(const GridPoint& gp, const Heading head, const float relativePressure) : WalkState(gp), WalkStateHeading(head, 0), WalkStateRelativePressure(relativePressure) {;} +//}; struct MyNode : public GridPoint, public GridNode, public GridNodeImportance { MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;} @@ -79,9 +79,10 @@ struct PFInit : public K::ParticleFilterInitializer { for (K::Particle& p : particles) { int idx = rand() % grid.getNumNodes(); - p.state.pos = grid[idx]; // random position - p.state.head = (rand() % 360) / 180.0 * M_PI; // random heading - p.state.relPres = 0; // start with a relative pressure of 0 + p.state.position = grid[idx]; // random position + p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading + p.state.heading.error = 0; + p.state.relPres = 0; // start with a relative pressure of 0 } } @@ -93,20 +94,20 @@ struct PFTrans : public K::ParticleFilterTransition { Grid& grid; - GridWalker walker; + GridWalker walker; // GridWalkerMulti walker; - WalkModuleHeading modHeadUgly; // stupid - WalkModuleHeadingControl modHead; - WalkModuleNodeImportance modImportance; - WalkModuleRelativePressureControl modPressure; - WalkModuleSpread modSpread; - WalkModuleFavorZ modFavorZ; + WalkModuleHeading modHeadUgly; // stupid + WalkModuleHeadingControl modHead; + WalkModuleNodeImportance modImportance; +// WalkModuleRelativePressureControl modPressure; + WalkModuleSpread modSpread; + WalkModuleFavorZ modFavorZ; //WalkModuleWiFi modWifi; - PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl), modPressure(ctrl, 0.100) { + PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, 3.5f) {//, modPressure(ctrl, 0.100) { walker.addModule(&modHead); //walker.addModule(&modHeadUgly); walker.addModule(&modImportance); @@ -128,18 +129,23 @@ struct PFTrans : public K::ParticleFilterTransition { tmpSum1 += p.state.relPres; - // transfer from state to walkstate - const MyWalkState start(grid.getNodeFor(p.state.pos), p.state.head, p.state.relPres); - const float var = distStep.draw(); const float dist = control->numSteps * var * 0.75; // 75cm + variance for every detected step - const MyWalkState end = walker.getDestination(grid, start, dist); + // --------- OLD + // transfer from state to walkstate +// const MyWalkState start(grid.getNodeFor(p.state.pos), p.state.head, p.state.relPres); + +// const MyWalkState end = walker.getDestination(grid, start, dist); // transfer from walkstate to state - p.state.relPres = end.pressureRelToT0; - p.state.pos = end.startPos; - p.state.head = end.startHeading.getRAD(); +// p.state.relPres = end.pressureRelToT0; +// p.state.pos = end.position; +// p.state.head = end.heading.direction.getRAD(); + + // -------------- NEW + // update the particle in-place + p.state = walker.getDestination(grid, p.state, dist); tmpSum2 += p.state.relPres; ++tmpCnt; @@ -196,7 +202,7 @@ struct PFEval : public K::ParticleFilterEvaluation { for (K::Particle& p : particles) { - const double prob = getALL(observation, p.state.pos.inMeter()); + const double prob = getALL(observation, p.state.position.inMeter()); p.weight = prob; sum += prob; diff --git a/competition/src/competition/filter/Structs.h b/competition/src/competition/filter/Structs.h index e4a0f5d..95e060c 100644 --- a/competition/src/competition/filter/Structs.h +++ b/competition/src/competition/filter/Structs.h @@ -8,54 +8,77 @@ #include #include -struct MyState { +#include +#include + +struct MyState : public WalkState, public WalkStateHeading { static Floorplan::IndoorMap* map; - GridPoint pos; // current position - float head; // current heading +// GridPoint pos; // current position +// float head; // current heading + + // CURRENTLY OBSOLETE (did not work as expected) float relPres; // current pressure relative to t0 - MyState() : pos(), head(0) {;} + MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0) {;} + + MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0) {;} - MyState(GridPoint pos, float head) : pos(pos), head(head) {;} MyState& operator += (const MyState& o) { - this->pos += o.pos; - this->head += o.head; + this->position += o.position; return *this; } MyState& operator /= (const double d) { - this->pos /= d; - this->head /= d; + this->position /= d; return *this; } MyState operator * (const double d) const { - return MyState(pos*d, head*d); + return MyState(this->position*d); } bool belongsToRegion(const MyState& o) const { - return pos.inMeter().getDistance(o.pos.inMeter()) < 3.0; + return position.inMeter().getDistance(o.position.inMeter()) < 3.0; } +// MyState() : pos(), head(0) {;} +// MyState(GridPoint pos, float head) : pos(pos), head(head) {;} +// MyState& operator += (const MyState& o) { +// this->pos += o.pos; +// this->head += o.head; +// return *this; +// } +// MyState& operator /= (const double d) { +// this->pos /= d; +// this->head /= d; +// return *this; +// } +// MyState operator * (const double d) const { +// return MyState(pos*d, head*d); +// } +// bool belongsToRegion(const MyState& o) const { +// return pos.inMeter().getDistance(o.pos.inMeter()) < 3.0; +// } - MyState(const float* params) { - pos.x_cm = params[0]; - pos.y_cm = params[1]; - pos.z_cm = params[2]; - } - void fillKernelDenstityParameters(float* params) const { - params[0] = pos.x_cm; - params[1] = pos.y_cm; - params[2] = pos.z_cm; - } - float getKernelDensityProbability(const float* params) const { - GridPoint gp(params[0], params[1], params[2]); - const float dist = gp.getDistanceInMeter(pos); - if (dist > 6.0) {return 0;} - const bool iSect = FloorplanHelper::intersectsObstacle(map, Point3(pos.x_cm,pos.y_cm,pos.z_cm)/100, Point3(params[0],params[1],params[2])/100); - if (iSect) {return 0;} - return Distribution::Normal::getProbability(0, 3.25, dist); - } + +// MyState(const float* params) { +// pos.x_cm = params[0]; +// pos.y_cm = params[1]; +// pos.z_cm = params[2]; +// } +// void fillKernelDenstityParameters(float* params) const { +// params[0] = pos.x_cm; +// params[1] = pos.y_cm; +// params[2] = pos.z_cm; +// } +// float getKernelDensityProbability(const float* params) const { +// GridPoint gp(params[0], params[1], params[2]); +// const float dist = gp.getDistanceInMeter(pos); +// if (dist > 6.0) {return 0;} +// const bool iSect = FloorplanHelper::intersectsObstacle(map, Point3(pos.x_cm,pos.y_cm,pos.z_cm)/100, Point3(params[0],params[1],params[2])/100); +// if (iSect) {return 0;} +// return Distribution::Normal::getProbability(0, 3.25, dist); +// } }; diff --git a/competition/src/competition/filter/WiFi.h b/competition/src/competition/filter/WiFi.h index b595d48..4c130df 100644 --- a/competition/src/competition/filter/WiFi.h +++ b/competition/src/competition/filter/WiFi.h @@ -14,6 +14,8 @@ class WiFiModelX : public WiFiModel { private: + VAPGrouper::Mode vapMode; + std::unordered_map aps; std::vector ceilingsAtHeight_m; @@ -21,6 +23,10 @@ private: public: + WiFiModelX (VAPGrouper::Mode vapMode) : vapMode(vapMode) { + ; + } + void write(const std::string& file) { std::ofstream out(file); for (auto it : aps) { @@ -108,7 +114,7 @@ public: if (scan.entries.empty()) {return 1.0;} - VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM); + VAPGrouper vg(vapMode, VAPGrouper::Aggregation::MAXIMUM); double prob = 0; for (const WiFiMeasurement& ap : scan.entries) { diff --git a/competition/src/competition/main.cpp b/competition/src/competition/main.cpp index 786cbed..5a8ac48 100755 --- a/competition/src/competition/main.cpp +++ b/competition/src/competition/main.cpp @@ -4,9 +4,6 @@ #include "filter/Structs.h" #include "Plotti.h" - - - #include "filter/WiFi.h" #include "filter/Logic.h" @@ -29,10 +26,6 @@ #include "Optimizer.h" #include "Scaler.h" - - - - #include #include @@ -45,7 +38,36 @@ #include +void testScaler() { + Scaler s(100, 100, 50, 10, 0.231, 1.0); + + { + const Point3 p3 = s.convert3D(50,10,0); + Assert::isNear(p3.x, 50.0f, 0.1f, "err"); + Assert::isNear(p3.y, 50.0f, 0.1f, "err"); + Assert::isNear(p3.z, 0.0f, 0.1f, "err"); + } + + const double sd = 0.00000001; + + { + const Point3 p3 = s.convert3D(50.01, 10.01, 0); + const IPIN ii = s.toIPIN3(p3.x, p3.y, p3.z); + Assert::isNear(ii.lat, 50.01, sd, "err"); + Assert::isNear(ii.lon, 10.01, sd, "err"); + Assert::isNear(ii.floorNr, 0.0, sd, "err"); + } + + { + const Point3 p3 = s.convert3D(49.99, 9.98, 0); + const IPIN ii = s.toIPIN3(p3.x, p3.y, p3.z); + Assert::isNear(ii.lat, 49.99, sd, "err"); + Assert::isNear(ii.lon, 9.98, sd, "err"); + Assert::isNear(ii.floorNr, 0.0, sd, "err"); + } + +} @@ -60,6 +82,7 @@ struct DataSetup { std::vector training; std::string wifiParams; int minWifiOccurences; + VAPGrouper::Mode vapMode; Scaler scaler; }; @@ -79,6 +102,7 @@ struct Data { dataDir + "car/wifiParams.txt", 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, Scaler (1282, 818, 40.31320308, -3.48367648, -8.77680000, 0.05207600) @@ -99,6 +123,7 @@ struct Data { dataDir + "uah/wifiParams.txt", 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, Scaler (1869, 1869, 40.51312440, -3.34959080, -40.73112000, 0.07596002) @@ -113,22 +138,44 @@ struct Data { dataDir + "uji-ti/logfile_UJITI_R2_NEXUS5.txt", }, - dataDir + "uah/wifiParams.txt", + dataDir + "uji-ti/wifiParams.txt", 15, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, Scaler (748, 1046, 39.99322125, -0.06862410, 29.57638723, 0.08556493) }; + DataSetup UJI_UB = { + + mapDir + "UJI-UB/UJI-UB4.xml", + + { + dataDir + "uji-ub/logfile_UJIUB_R1n_S3.txt", + dataDir + "uji-ub/logfile_UJIUB_R1r_S3.txt", + dataDir + "uji-ub/logfile_UJIUB_R2n_S3.txt", + dataDir + "uji-ub/logfile_UJIUB_R2r_S3.txt", + dataDir + "uji-ub/logfile_UJIUB_R3_S3.txt", + }, + + dataDir + "uji-ub/wifiParams.txt", + 12, + VAPGrouper::Mode::DISABLED, + + // BEWARE! not all images have the same size! + Scaler (4362, 1959, 39.99380328, -0.07389222, 29.54974856, 0.01700000) + + }; + } data; /** perform wifi AP optimization */ -void optimize(Floorplan::IndoorMap* map, Scaler& scaler, const int minWifiOccurences, const std::vector trainingFiles, const std::string outFile) { +void optimize(Floorplan::IndoorMap* map, Scaler& scaler, VAPGrouper::Mode vapMode, const int minWifiOccurences, const std::vector trainingFiles, const std::string outFile) { // the optimizer to determine values for each AP - Optimizer opti(map, scaler); + Optimizer opti(map, scaler, vapMode); // all training-data-files (measurements at a known ground-truth) opti.addRecords(trainingFiles); @@ -144,7 +191,7 @@ void optimize(Floorplan::IndoorMap* map, Scaler& scaler, const int minWifiOccure int errCnt = 0; std::string modelFile = outFile; - WiFiModelX model; + WiFiModelX model(vapMode); int numValid = 0; int numInvalid = 0; @@ -196,23 +243,24 @@ Floorplan::IndoorMap* MyState::map; int main(int argc, char** argv) { + testScaler(); // the dataset to use - DataSetup setup = data.UJI_TI; + DataSetup setup = data.CAR; // load the floorplan Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); MyState::map = map; // optimize (and save) wifi parameters - //optimize(map, setup.scaler, setup.minWifiOccurences, setup.training, setup.wifiParams); + //optimize(map, setup.scaler, setup.vapMode, setup.minWifiOccurences, setup.training, setup.wifiParams); // testing map->floors[0]->obstacles.clear(); // fetch previously optimized wifi paramters - WiFiModelX model; + WiFiModelX model(setup.vapMode); model.read(setup.wifiParams, map); // build the grid @@ -225,7 +273,7 @@ int main(int argc, char** argv) { - FileReader fr(setup.training[0]); + FileReader fr(setup.training[3]); //partikel gehen nicht schnell genug nach oben oder unten @@ -251,7 +299,7 @@ int main(int argc, char** argv) { MyControl ctrl; ctrl.numSteps = 0; MyObs obs; - int numParticles = 3000; + int numParticles = 10000; PFEval* eval = new PFEval(setup.scaler, model); @@ -299,18 +347,18 @@ int main(int argc, char** argv) { obs.wifis = fr.getWiFiGroupedByTime()[e.idx].data; } else if (e.type == FileReader::Sensor::ACC) { - if (sd.isStep(ts, fr.getAccelerometer()[e.idx].data)) { + if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { ++ctrl.numSteps; tt.pos.x += std::cos(tt.rad) * 0.7; tt.pos.y += std::sin(tt.rad) * 0.7; } const FileReader::TS& _acc = fr.getAccelerometer()[e.idx]; - td.addAccelerometer(Timestamp::fromSec(_acc.ts), _acc.data); + td.addAccelerometer(Timestamp::fromSec(ts), _acc.data); } else if (e.type == FileReader::Sensor::GYRO) { const FileReader::TS& _gyr = fr.getGyroscope()[e.idx]; //td.update(ctrl.turnAngle, e.ts * 1000, fr.getGyroscope()[e.idx].data); - const float delta = td.addGyroscope(Timestamp::fromSec(_gyr.ts), _gyr.data); + const float delta = td.addGyroscope(Timestamp::fromSec(ts), _gyr.data); ctrl.turnAngle += delta; tt.rad += delta; @@ -327,7 +375,7 @@ int main(int argc, char** argv) { } - if (ms - lastUpdateMS > 1000) { + if (ms - lastUpdateMS > 500) { obs.curTS = e.ts; @@ -341,7 +389,7 @@ int main(int argc, char** argv) { MyState est = pf.update(&ctrl, obs); - Point3 estPos = est.pos.inMeter(); + Point3 estPos = est.position.inMeter(); plot.pInterest.clear();