326 lines
11 KiB
C++
326 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, 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 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::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)
|
|
{
|
|
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<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] " << std::setw(10) << "mean" << std::setw(10) << "stdDev" << std::setw(10) << "median" << "\n";
|
|
|
|
std::cout << "FTM " << std::setw(10) << errorStats.ftm.getAvg() << std::setw(10) << errorStats.ftm.getStdDev() << std::setw(10) << errorStats.ftm.getMedian() << "\n";
|
|
std::cout << "RSSI " << std::setw(10) << errorStats.rssi.getAvg() << std::setw(10) << errorStats.rssi.getStdDev() << std::setw(10) << 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;
|
|
} |