From 17140f2dbe9e04a3ae21205c592ecf7d7a23b443 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 1 Oct 2019 17:23:59 +0200 Subject: [PATCH] Refactored probabilistic code --- code/mainProb.cpp | 217 ++++++++++++++++++++++++++-------------------- 1 file changed, 122 insertions(+), 95 deletions(-) diff --git a/code/mainProb.cpp b/code/mainProb.cpp index b45b21b..4702451 100644 --- a/code/mainProb.cpp +++ b/code/mainProb.cpp @@ -30,7 +30,7 @@ static float kalman_procNoiseDistStdDev = 1.2f; // standard deviation of distance for process noise static float kalman_procNoiseVelStdDev = 0.1f; // standard deviation of velocity for process noise -static void poorMansKDE(const BBox3& bbox, float sigma, std::array dist, std::array apPos, std::vector>& density, std::pair& maxElem) +static void poorMansKDE(const BBox3& bbox, std::vector>& density, std::pair& maxElem, const std::function& evalProc) { density.clear(); @@ -48,35 +48,99 @@ static void poorMansKDE(const BBox3& bbox, float sigma, std::array dis { const Point2 pos(x, y); - float P = 1.0f; - - for (size_t i = 0; i < 4; i++) - { - // TODO: Was mit nan machen? - if (!isnan(dist[i])) - { - float d = pos.getDistance(apPos[i]) - dist[i]; - float p = Distribution::Normal::getProbability(0, sigma, d); - P *= p; - } - } + double P = evalProc(pos); density.push_back({ pos, P }); } } - auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair a, std::pair b) { + auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair a, std::pair b) { return a.second < b.second; }); maxElem = *maxElement; } +static void computeDensity(const BBox3& bbox, std::vector>& density, std::pair& maxElem, const std::vector& obs, bool useFtm, double sigma) +{ + poorMansKDE(bbox, density, maxElem, [&obs, useFtm, sigma](Point2 pt) { + double p = 1.0; + + for (const WiFiMeasurement& wifi : obs) + { + if (wifi.getNumSuccessfulMeasurements() < 3) + continue; + + const MACAddress& mac = wifi.getAP().getMAC(); + int nucIndex = Settings::nucIndex(mac); + + // compute AP distance + const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position; + Point3 pos = Point3(pt.x, pt.y, 1.3); // smartphone höhe + const float apDist = pos.getDistance(apPos); + + double dist = 0; + if (useFtm) + { + // compute ftm distance + float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; + float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset + + dist = ftmDist; + } + else + { + // compute rssi distance + float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, 2.25 /*rssi_pathloss*/, wifi.getRSSI()); + + dist = rssiDist; + } + + if (dist > 0) + { + double d = apDist - dist; + double x = Distribution::Normal::getProbability(0, 3.5, d); + + p *= x; + } + } + + return p; + }); +} + +static void plotDensity(Plotty& plot, std::vector>& density) +{ + plot.particles.clear(); + + double min = std::numeric_limits::max(); + double max = std::numeric_limits::min(); + + for (auto it = density.begin(); ; std::advance(it, 2)) + { + if (it > density.end()) + break; + + auto p = *it; + + const K::GnuplotPoint3 p3(p.first.x, p.first.y, 0.1); + const double prob = std::pow(p.second, 0.25); + + plot.particles.add(p3, prob); + + if (prob > max) { max = prob; } + if (prob < min) { min = prob; } + } + + plot.splot.getAxisCB().setRange(min, max + 0.000001); +} + static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::string folder) { // reading file Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - Offline::FileReader fr(setup.training[walkIdx]); + Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps); // ground truth std::vector gtPath = setup.gtPath; @@ -113,7 +177,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str plot.setGroundTruth(gtPath); plot.setView(30, 0); plot.plot(); - + // wifi std::array ftmKalmanFilters{ Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), @@ -129,7 +193,9 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(), }; - std::vector data = filterOfflineData(fr); + std::vector obs; + + Timestamp lastTimestamp = Timestamp::fromMS(0); const float sigma = 3.5; const int movAvgWnd = 15; @@ -139,124 +205,85 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str std::vector errorValuesFtm, errorValuesRssi; std::vector timestamps; - for (const WifiMeas& wifi : data) + for (const Offline::Entry& e : fr.getEntries()) { - Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py"); + if (e.type != Offline::Sensor::WIFI_FTM) { + continue; + } - Point2 gtPos = gtInterpolator.get(static_cast(wifi.ts.ms())).xy(); - plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + // TIME + const Timestamp ts = Timestamp::fromMS(e.ts); - float distErrorFtm = 0; - float distErrorRssi = 0; + auto wifiFtm = fr.getWifiFtm()[e.idx].data; + obs.push_back(wifiFtm); - //if (wifi.numSucessMeas() == 4) + if (ts - lastTimestamp >= Timestamp::fromMS(500)) { + // Do update + Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py"); + + Point2 gtPos = gtInterpolator.get(static_cast(ts.ms())).xy(); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + + float distErrorFtm = 0; + float distErrorRssi = 0; + // FTM { - std::array dists = wifi.ftmDists; - - if (Settings::UseKalman) - { - std::cout << "Using Kalman" << "\n"; - - for (size_t i = 0; i < 4; i++) - { - if (!isnan(dists[i])) - { - dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); - } - } - } - BBox3 bbox = FloorplanHelper::getBBox(map); - std::vector> density; - std::pair maxElement; + std::vector> density; + std::pair maxElement; - poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement); + computeDensity(bbox, density, maxElement, obs, true, 3.5); Point2 estPos = maxElement.first; - - plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); + plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); + + // Plot density + //plotDensity(plot, density); // Error distErrorFtm = gtPos.getDistance(estPos); errorStats.ftm.add(distErrorFtm); - - - //std::vector densityX, densityY, densityZ; - //for (const auto& item : density) - //{ - // densityX.push_back(item.first.x); - // densityY.push_back(item.first.y); - // densityZ.push_back(item.second); - //} - - //test.add("densityX", densityX); - //test.add("densityY", densityY); - //test.add("densityZ", densityZ); } // RSSI { - std::array dists = wifi.rssiDists; - - if (Settings::UseKalman) - { - for (size_t i = 0; i < 4; i++) - { - if (!isnan(dists[i])) - { - dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); - } - } - } - - BBox3 bbox = FloorplanHelper::getBBox(map); - std::vector> density; - std::pair maxElement; + std::vector> density; + std::pair maxElement; - poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement); + computeDensity(bbox, density, maxElement, obs, false, 8); Point2 estPos = maxElement.first; - - plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1)); + // Plot density + //plotDensity(plot, density); + // Error distErrorRssi = gtPos.getDistance(estPos); errorStats.rssi.add(distErrorRssi); - - - //std::vector densityX, densityY, densityZ; - //for (const auto& item : density) - //{ - // densityX.push_back(item.first.x); - // densityY.push_back(item.first.y); - // densityZ.push_back(item.second); - //} - - //test.add("densityX", densityX); - //test.add("densityY", densityY); - //test.add("densityZ", densityZ); } - //std::cout << wifi.ts.ms() << " " << distError << "\n"; errorValuesFtm.push_back(distErrorFtm); errorValuesRssi.push_back(distErrorRssi); - timestamps.push_back(wifi.ts.ms()); + timestamps.push_back(ts.ms()); test.add("t", timestamps); test.add("errorFtm", errorValuesFtm); test.add("errorRssi", errorValuesRssi); test.frame(); - } - plot.plot(); - //Sleep(250); - printf(""); + plot.plot(); + //Sleep(250); + printf(""); + + lastTimestamp = ts; + obs.clear(); + } } printErrorStats(errorStats);