355 lines
11 KiB
C++
355 lines
11 KiB
C++
#include "main.h"
|
||
|
||
#include <array>
|
||
#include <memory>
|
||
#include <thread>
|
||
#include <filesystem>
|
||
#include <chrono>
|
||
#include <iostream>
|
||
|
||
#include <Indoor/math/stats/Statistics.h>
|
||
|
||
#include <Indoor/floorplan/v2/FloorplanReader.h>
|
||
#include <Indoor/sensors/offline/FileReader.h>
|
||
#include <Indoor/sensors/offline/Sensors.h>
|
||
#include <Indoor/sensors/radio/model/LogDistanceModel.h>
|
||
#include <Indoor/geo/Heading.h>
|
||
#include <Indoor/geo/Point2.h>
|
||
#include <Indoor/data/Timestamp.h>
|
||
#include <Indoor/math/MovingAVG.h>
|
||
|
||
#include "FtmKalman.h"
|
||
|
||
#include "Settings.h"
|
||
|
||
#include "Plotty.h"
|
||
#include "Plotta.h"
|
||
|
||
#include "misc.h"
|
||
|
||
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, std::vector<std::pair<Point2, double>>& density, std::pair<Point2, double>& maxElem, const std::function<double(Point2 pt)>& evalProc)
|
||
{
|
||
density.clear();
|
||
|
||
const float stepsize = 0.2;
|
||
|
||
const float minX = bbox.getMin().x - 5;
|
||
const float minY = bbox.getMin().y - 5;
|
||
|
||
const float maxX = bbox.getMax().x + 5;
|
||
const float maxY = bbox.getMax().y + 5;
|
||
|
||
for (float y = minY; y < maxY; y += stepsize)
|
||
{
|
||
for (float x = minX; x < maxX; x += stepsize)
|
||
{
|
||
const Point2 pos(x, y);
|
||
|
||
double P = evalProc(pos);
|
||
|
||
density.push_back({ pos, P });
|
||
}
|
||
}
|
||
|
||
auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair<Point2, double> a, std::pair<Point2, double> b) {
|
||
return a.second < b.second;
|
||
});
|
||
|
||
maxElem = *maxElement;
|
||
}
|
||
|
||
static void computeDensity(const BBox3& bbox, std::vector<std::pair<Point2, double>>& density, std::pair<Point2, double>& maxElem, const std::vector<WiFiMeasurement>& 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::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::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::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<double>::getProbability(0, 3.5, d);
|
||
|
||
p *= x;
|
||
}
|
||
}
|
||
|
||
return p;
|
||
});
|
||
}
|
||
|
||
static void plotDensity(Plotty& plot, std::vector<std::pair<Point2, double>>& density)
|
||
{
|
||
plot.particles.clear();
|
||
|
||
double min = std::numeric_limits<double>::max();
|
||
double max = std::numeric_limits<double>::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<float> 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], setup.HasNanoSecondTimestamps);
|
||
|
||
// ground truth
|
||
std::vector<int> gtPath = setup.gtPath;
|
||
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
|
||
CombinedStats<float> errorStats;
|
||
|
||
//calculate distance of path
|
||
std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
|
||
double gtTotalDistance = 0;
|
||
Stats::Statistics<double> gtWalkingSpeed;
|
||
for (int i = 1; i < gtEntries.size(); ++i) {
|
||
double distance = gtEntries[i].value.getDistance(gtEntries[i - 1].value);
|
||
double timeDiff = static_cast<double>(gtEntries[i].key - gtEntries[i - 1].key);
|
||
|
||
double walkingSpeed = distance / (timeDiff/1000.0f); // m / s
|
||
|
||
gtWalkingSpeed.add(walkingSpeed);
|
||
gtTotalDistance += distance;
|
||
}
|
||
|
||
std::cout << "Distance of Path: " << gtTotalDistance << std::endl;
|
||
std::cout << "GT walking speed: " << gtWalkingSpeed.getAvg() << "m/s (" << gtWalkingSpeed.getAvg()*3.6f << "km/h)" << std::endl;
|
||
|
||
// debug show
|
||
//MeshPlotter dbg;
|
||
//dbg.addFloors(map);
|
||
//dbg.addOutline(map);
|
||
//dbg.addMesh(mesh);
|
||
////dbg.addDijkstra(mesh);
|
||
//dbg.draw();
|
||
|
||
Plotty plot(map);
|
||
plot.buildFloorplan();
|
||
plot.setGroundTruth(gtPath);
|
||
plot.setView(30, 0);
|
||
plot.plot();
|
||
|
||
// wifi
|
||
|
||
std::vector<WiFiMeasurement> obs;
|
||
|
||
Timestamp lastTimestamp = Timestamp::fromMS(0);
|
||
|
||
//const float sigma = 3.5;
|
||
//const int movAvgWnd = 15;
|
||
//std::array<MovingAVG<float>, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
|
||
//std::array<MovingAVG<float>, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
|
||
|
||
std::vector<float> errorValuesFtm, errorValuesRssi;
|
||
std::vector<int> timestamps;
|
||
|
||
for (const Offline::Entry& e : fr.getEntries())
|
||
{
|
||
if (e.type != Offline::Sensor::WIFI_FTM) {
|
||
continue;
|
||
}
|
||
|
||
// TIME
|
||
const Timestamp ts = Timestamp::fromMS(e.ts);
|
||
|
||
auto wifiFtm = fr.getWifiFtm()[e.idx].data;
|
||
obs.push_back(wifiFtm);
|
||
|
||
if (ts - lastTimestamp >= Timestamp::fromMS(500))
|
||
{
|
||
// Do update
|
||
Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");
|
||
|
||
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
|
||
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
|
||
|
||
float distErrorFtm = 0;
|
||
float distErrorRssi = 0;
|
||
|
||
// FTM
|
||
{
|
||
BBox3 bbox = FloorplanHelper::getBBox(map);
|
||
std::vector<std::pair<Point2, double>> density;
|
||
std::pair<Point2, double> maxElement;
|
||
|
||
computeDensity(bbox, density, maxElement, obs, true, 3.5);
|
||
|
||
Point2 estPos = maxElement.first;
|
||
//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);
|
||
}
|
||
|
||
// RSSI
|
||
{
|
||
BBox3 bbox = FloorplanHelper::getBBox(map);
|
||
std::vector<std::pair<Point2, double>> density;
|
||
std::pair<Point2, double> maxElement;
|
||
|
||
computeDensity(bbox, density, maxElement, obs, false, 8);
|
||
|
||
Point2 estPos = maxElement.first;
|
||
//plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));
|
||
|
||
// Plot density
|
||
//plotDensity(plot, density);
|
||
|
||
// Error
|
||
distErrorRssi = gtPos.getDistance(estPos);
|
||
errorStats.rssi.add(distErrorRssi);
|
||
}
|
||
|
||
// draw wifi ranges
|
||
plot.clearDistanceCircles();
|
||
|
||
for (size_t i = 0; i < obs.size(); i++)
|
||
{
|
||
WiFiMeasurement wifi2 = obs[i];
|
||
|
||
Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position;
|
||
|
||
K::GnuplotColor color;
|
||
switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID)
|
||
{
|
||
case 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break;
|
||
case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break;
|
||
case 3: color = K::GnuplotColor::fromRGB(255, 255, 0); break;
|
||
default: color = K::GnuplotColor::fromRGB(255, 0, 0); break;
|
||
}
|
||
|
||
plot.addDistanceCircle(apPos.xy(), wifi2.getFtmDist(), color);
|
||
}
|
||
|
||
errorValuesFtm.push_back(distErrorFtm);
|
||
errorValuesRssi.push_back(distErrorRssi);
|
||
timestamps.push_back(ts.ms());
|
||
|
||
test.add("t", timestamps);
|
||
test.add("errorFtm", errorValuesFtm);
|
||
test.add("errorRssi", errorValuesRssi);
|
||
test.frame();
|
||
|
||
plot.plot();
|
||
//Sleep(250);
|
||
printf("");
|
||
|
||
lastTimestamp = ts;
|
||
obs.clear();
|
||
}
|
||
}
|
||
|
||
printErrorStats(errorStats);
|
||
|
||
return errorStats;
|
||
}
|
||
|
||
|
||
|
||
int mainProp(int argc, char** argv)
|
||
{
|
||
// global stats
|
||
CombinedStats<float> statsAVG;
|
||
CombinedStats<float> statsMedian;
|
||
CombinedStats<float> statsSTD;
|
||
CombinedStats<float> statsQuantil;
|
||
CombinedStats<float> tmp;
|
||
|
||
std::string evaluationName = "prologic/tmp";
|
||
|
||
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
|
||
{
|
||
std::cout << "Executing walk " << walkIdx << "\n";
|
||
for (int i = 0; i < 1; ++i)
|
||
{
|
||
std::cout << "Start of iteration " << i << "\n";
|
||
|
||
tmp = run(Settings::CurrentPath, walkIdx, evaluationName);
|
||
|
||
statsAVG.ftm.add(tmp.ftm.getAvg());
|
||
statsMedian.ftm.add(tmp.ftm.getMedian());
|
||
statsSTD.ftm.add(tmp.ftm.getStdDev());
|
||
statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75));
|
||
|
||
statsAVG.rssi.add(tmp.rssi.getAvg());
|
||
statsMedian.rssi.add(tmp.rssi.getMedian());
|
||
statsSTD.rssi.add(tmp.rssi.getStdDev());
|
||
statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75));
|
||
|
||
std::cout << "Iteration " << i << " completed" << std::endl;
|
||
}
|
||
}
|
||
|
||
std::cout << "==========================================================" << std::endl;
|
||
std::cout << "Average of all statistical data FTM: " << std::endl;
|
||
std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl;
|
||
std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl;
|
||
std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl;
|
||
std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl;
|
||
std::cout << "==========================================================" << std::endl;
|
||
|
||
std::cout << "==========================================================" << std::endl;
|
||
std::cout << "Average of all statistical data RSSI: " << std::endl;
|
||
std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl;
|
||
std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl;
|
||
std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl;
|
||
std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl;
|
||
std::cout << "==========================================================" << std::endl;
|
||
|
||
|
||
return 0;
|
||
} |