324 lines
11 KiB
C++
324 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 "Settings.h"
|
|
|
|
#include "Plotty.h"
|
|
#include "Plotta.h"
|
|
|
|
|
|
#include "trilateration.h"
|
|
#include "misc.h"
|
|
|
|
|
|
|
|
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);
|
|
// APs Positions
|
|
for (auto& nucConfig : setup.NUCs)
|
|
{
|
|
plot.addCircle(10000 + nucConfig.second.ID, nucConfig.second.position.xy(), 0.1);
|
|
}
|
|
plot.plot();
|
|
|
|
|
|
// Output dir
|
|
auto outputDir = std::filesystem::path(Settings::outputDir);
|
|
outputDir.append(Settings::CurrentPath.name + "_" + std::to_string(walkIdx));
|
|
|
|
if (!std::filesystem::exists(outputDir)) {
|
|
std::filesystem::create_directories(outputDir);
|
|
}
|
|
|
|
std::vector<WiFiMeasurement> obs;
|
|
|
|
Timestamp lastTimestamp = Timestamp::fromMS(0);
|
|
|
|
Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py");
|
|
//plotta.add("apPos", apPositions);
|
|
|
|
const int movAvgWnd = 10;
|
|
std::unordered_map<MACAddress, MovingAVG<float>> movAvgsFtm;
|
|
std::unordered_map<MACAddress, MovingAVG<float>> movAvgsRssi;
|
|
|
|
for (auto& nucConfig : setup.NUCs)
|
|
{
|
|
movAvgsFtm.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
|
|
movAvgsRssi.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
|
|
}
|
|
|
|
std::vector<float> errorValuesFtm, errorValuesRssi;
|
|
std::vector<int> timestamps;
|
|
|
|
std::vector<Point2> estPathFtm, estPathRssi;
|
|
|
|
for (const Offline::Entry& e : fr.getEntries())
|
|
{
|
|
if (e.type != Offline::Sensor::WIFI_FTM && e.type != Offline::Sensor::GROUND_TRUTH) {
|
|
continue;
|
|
}
|
|
|
|
// TIME
|
|
const Timestamp ts = Timestamp::fromMS(e.ts);
|
|
|
|
if (e.type == Offline::Sensor::WIFI_FTM)
|
|
{
|
|
auto wifiFtm = fr.getWifiFtm()[e.idx].data;
|
|
obs.push_back(wifiFtm);
|
|
}
|
|
|
|
if (ts - lastTimestamp >= Timestamp::fromMS(500))
|
|
{
|
|
// Do update
|
|
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
|
|
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
|
|
|
|
std::unordered_map<MACAddress, std::pair<float, float>> apPosDistMap;
|
|
|
|
for (const WiFiMeasurement& wifi : obs)
|
|
{
|
|
if (wifi.getNumSuccessfulMeasurements() < 3)
|
|
continue;
|
|
|
|
const MACAddress& mac = wifi.getAP().getMAC();
|
|
float ftm_offset = setup.NUCs.at(mac).ftm_offset;
|
|
float ftmDist = wifi.getFtmDist() + ftm_offset;
|
|
|
|
float rssi_pathloss = setup.NUCs.at(mac).rssi_pathloss;
|
|
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
|
|
|
|
movAvgsFtm[mac].add(ftmDist);
|
|
movAvgsRssi[mac].add(rssiDist);
|
|
|
|
apPosDistMap[mac] = { movAvgsFtm[mac].get(), movAvgsRssi[mac].get() };
|
|
}
|
|
|
|
if (apPosDistMap.size() > 3)
|
|
{
|
|
// Do update for real
|
|
std::vector<MACAddress> macs;
|
|
std::vector<Point2> apPositions;
|
|
std::vector<float> ftmDists;
|
|
std::vector<float> rssiDists;
|
|
|
|
for (const auto& kvp : apPosDistMap)
|
|
{
|
|
macs.push_back(kvp.first);
|
|
apPositions.push_back(setup.NUCs.at(kvp.first).position.xy());
|
|
ftmDists.push_back(kvp.second.first);
|
|
rssiDists.push_back(kvp.second.second);
|
|
}
|
|
|
|
Point2 estFtmPos = Trilateration::levenbergMarquardt(apPositions, ftmDists);
|
|
Point2 estRssiPos = Trilateration::levenbergMarquardt(apPositions, rssiDists);
|
|
|
|
// Error
|
|
float distErrorFtm = gtPos.getDistance(estFtmPos);
|
|
errorStats.ftm.add(distErrorFtm);
|
|
estPathFtm.push_back(estFtmPos);
|
|
|
|
float distErrorRssi = gtPos.getDistance(estRssiPos);
|
|
errorStats.rssi.add(distErrorRssi);
|
|
estPathRssi.push_back(estRssiPos);
|
|
|
|
errorValuesFtm.push_back(distErrorFtm);
|
|
errorValuesRssi.push_back(distErrorRssi);
|
|
timestamps.push_back(ts.ms());
|
|
|
|
plotta.add("t", timestamps);
|
|
plotta.add("errorFtm", errorValuesFtm);
|
|
plotta.add("errorRssi", errorValuesRssi);
|
|
plotta.frame();
|
|
|
|
// Plot
|
|
plot.setCurEst(Point3(estFtmPos.x, estFtmPos.y, 0.1));
|
|
plot.addEstimationNode(Point3(estFtmPos.x, estFtmPos.y, 0.1));
|
|
plot.addEstimationNode2(Point3(estRssiPos.x, estRssiPos.y, 0.1));
|
|
|
|
// draw wifi ranges
|
|
if (Settings::PlotCircles)
|
|
{
|
|
plot.clearDistanceCircles();
|
|
plot.splot.getCustom().str("");
|
|
|
|
for (size_t i = 0; i < 20; i++)
|
|
{
|
|
plot.splot.getCustom() << "unset label " << i << "\n";
|
|
}
|
|
|
|
for (size_t i = 0; i < ftmDists.size(); i++)
|
|
{
|
|
plot.addDistanceCircle(apPositions[i], ftmDists[i], K::GnuplotColor::fromRGB(255, 0, 0));
|
|
plot.addDistanceCircle(apPositions[i], rssiDists[i], K::GnuplotColor::fromRGB(0, 255, 0));
|
|
|
|
// Distance labels
|
|
std::stringstream ss;
|
|
ss << setup.nuc(macs[i]).ID << ": " << ftmDists[i] << "m";
|
|
|
|
plot.addLabel(ss.str(), Point3(70, i*5, 0), i);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Png Output
|
|
//if (Settings::PlotToPng)
|
|
//{
|
|
// plot.gp.setTerminal("png", K::GnuplotSize(1280, 720));
|
|
// auto pngPath = outputDir / "png" / "trilat" / "frame.png";
|
|
|
|
// // clear folder
|
|
// //std::filesystem::remove_all(pngPath);
|
|
// forceDirectories(pngPath.parent_path());
|
|
// //std::filesystem::create_directory(pngPath);
|
|
|
|
// plot.gp.setOutput(appendFileSuffixToPath(pngPath, ts.ms()).string());
|
|
//}
|
|
|
|
plot.plot();
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
|
obs.clear();
|
|
lastTimestamp = ts;
|
|
}
|
|
|
|
printf("");
|
|
}
|
|
|
|
plotta.add("estPathFtm", estPathFtm);
|
|
plotta.add("estPathRssi", estPathRssi);
|
|
plotta.frame();
|
|
|
|
printErrorStats(errorStats);
|
|
|
|
return errorStats;
|
|
}
|
|
|
|
|
|
int mainTrilat(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/trilat";
|
|
|
|
std::vector<Settings::DataSetup> setupsToRun = {
|
|
//Settings::data.Path5,
|
|
//Settings::data.Path7,
|
|
//Settings::data.Path8,
|
|
//Settings::data.Path9,
|
|
//Settings::data.Path10,
|
|
//Settings::data.Path11
|
|
//Settings::data.Path20,
|
|
Settings::data.Path21,
|
|
//Settings::data.Path22,
|
|
};
|
|
|
|
for (Settings::DataSetup setupToRun : setupsToRun)
|
|
{
|
|
Settings::CurrentPath = setupToRun;
|
|
|
|
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 << "Results for path " << Settings::CurrentPath.name << 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;
|
|
}
|