Added plotta, added probabilistic code
This commit is contained in:
316
code/mainProb.cpp
Normal file
316
code/mainProb.cpp
Normal file
@@ -0,0 +1,316 @@
|
||||
#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, float sigma, std::array<float, 4> dist, std::array<Point2, 4> apPos, std::vector<std::pair<Point2, float>>& density, std::pair<Point2, float>& maxElem)
|
||||
{
|
||||
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);
|
||||
|
||||
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<float>::getProbability(0, sigma, d);
|
||||
P *= p;
|
||||
}
|
||||
}
|
||||
|
||||
density.push_back({ pos, P });
|
||||
}
|
||||
}
|
||||
|
||||
auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair<Point2, float> a, std::pair<Point2, float> b) {
|
||||
return a.second < b.second;
|
||||
});
|
||||
|
||||
maxElem = *maxElement;
|
||||
}
|
||||
|
||||
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]);
|
||||
|
||||
// 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 distance = 0;
|
||||
for (int i = 1; i < gtEntries.size(); ++i) {
|
||||
distance += gtEntries[i].value.getDistance(gtEntries[i - 1].value);
|
||||
}
|
||||
|
||||
std::cout << "Distance of Path: " << distance << 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::array<Kalman, 4> ftmKalmanFilters{
|
||||
Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
|
||||
Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
|
||||
Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev),
|
||||
Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev)
|
||||
};
|
||||
|
||||
std::array<Point2, 4> apPositions{
|
||||
Settings::data.CurrentPath.NUCs.at(Settings::NUC1).position.xy(),
|
||||
Settings::data.CurrentPath.NUCs.at(Settings::NUC2).position.xy(),
|
||||
Settings::data.CurrentPath.NUCs.at(Settings::NUC3).position.xy(),
|
||||
Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(),
|
||||
};
|
||||
|
||||
std::vector<WifiMeas> data = filterOfflineData(fr);
|
||||
|
||||
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 WifiMeas& wifi : data)
|
||||
{
|
||||
Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");
|
||||
|
||||
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
|
||||
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
|
||||
|
||||
float distErrorFtm = 0;
|
||||
float distErrorRssi = 0;
|
||||
|
||||
//if (wifi.numSucessMeas() == 4)
|
||||
{
|
||||
// FTM
|
||||
{
|
||||
std::array<float, 4> dists = wifi.ftmDists;
|
||||
|
||||
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<std::pair<Point2, float>> density;
|
||||
std::pair<Point2, float> maxElement;
|
||||
|
||||
poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement);
|
||||
|
||||
Point2 estPos = maxElement.first;
|
||||
|
||||
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
|
||||
plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
|
||||
|
||||
// Error
|
||||
distErrorFtm = gtPos.getDistance(estPos);
|
||||
errorStats.ftm.add(distErrorFtm);
|
||||
|
||||
|
||||
//std::vector<float> 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<float, 4> 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<std::pair<Point2, float>> density;
|
||||
std::pair<Point2, float> maxElement;
|
||||
|
||||
poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement);
|
||||
|
||||
Point2 estPos = maxElement.first;
|
||||
|
||||
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
|
||||
plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));
|
||||
|
||||
// Error
|
||||
distErrorRssi = gtPos.getDistance(estPos);
|
||||
errorStats.rssi.add(distErrorRssi);
|
||||
|
||||
|
||||
//std::vector<float> 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());
|
||||
|
||||
test.add("t", timestamps);
|
||||
test.add("errorFtm", errorValuesFtm);
|
||||
test.add("errorRssi", errorValuesRssi);
|
||||
test.frame();
|
||||
}
|
||||
|
||||
plot.plot();
|
||||
//Sleep(250);
|
||||
printf("");
|
||||
}
|
||||
|
||||
std::cout << "Walk error:" << "\n";
|
||||
std::cout << "[m] " << " mean \t stdDev median" << "\n";
|
||||
|
||||
std::cout << "FTM " << errorStats.ftm.getAvg() << "\t" << errorStats.ftm.getStdDev() << "\t" << errorStats.ftm.getMedian() << "\n";
|
||||
std::cout << "RSSI " << errorStats.rssi.getAvg() << "\t" << errorStats.rssi.getStdDev() << "\t" << errorStats.rssi.getMedian() << "\n";
|
||||
std::cout << std::endl;
|
||||
|
||||
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 < 6; walkIdx++)
|
||||
{
|
||||
std::cout << "Executing walk " << walkIdx << "\n";
|
||||
for (int i = 0; i < 1; ++i)
|
||||
{
|
||||
std::cout << "Start of iteration " << i << "\n";
|
||||
|
||||
tmp = run(Settings::data.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;
|
||||
}
|
||||
Reference in New Issue
Block a user