FHWS/museumLoc
Archived
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

commit before new model is implemented

Browse Source
This commit is contained in:
toni
2018-01-17 13:19:16 +01:00
parent bea81eab62
commit f4c598299f
6 changed files with 152 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
main.cpp
View File

@@ -11,7 +11,10 @@
#include <Indoor/sensors/radio/model/WiFiModelFactory.h>
#include <Indoor/sensors/radio/model/WiFiModelFactoryImpl.h>
#include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/smoothing/ForwardFilterHistory.h>
#include <Indoor/smc/smoothing/FastKDESmoothing.h>
//frank
//const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
@@ -20,8 +23,8 @@
//toni
const std::string mapDir = "/home/toni/Documents/programme/localization/IndoorMap/maps/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/DynLag/code/data/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/";
const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/";
const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/";
const std::string errorDir = dataDir + "results/";
/** describes one dataset (map, training, parameter-estimation, ...) */
@@ -87,7 +90,7 @@ struct Data {
Floorplan::IndoorMap* MyState::map;
K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
Stats::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
std::vector<double> kld_data;
@@ -111,7 +114,7 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
// build the grid
std::ifstream inp(setup.grid, std::ifstream::binary);
Grid<MyNode> grid(20);
Grid<MyNode> grid(Settings::Grid::gridSize_cm);
// grid.dat empty? -> build one and save it
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
@@ -152,12 +155,11 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
MyObs obs;
//History of all estimated particles. Used for smoothing
std::vector<std::vector<K::Particle<MyState>>> pfHistory;
std::vector<int64_t> tsHistory;
SMC::ForwardFilterHistory<MyState, MyControl, MyObs> pfHistory;
//filter init
//K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f)));
SMC::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
//SMC::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
//pf.setTransition(std::unique_ptr<PFTransKLDSampling>(new PFTransKLDSampling(grid, &ctrl)));
@@ -166,40 +168,42 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
//resampling
if(Settings::useKLB){
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<MyState>()));
pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingDivergence<MyState>>(new SMC::ParticleFilterResamplingDivergence<MyState>()));
} else {
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.4)));
pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>>(new SMC::ParticleFilterResamplingSimple<MyState>()));
//pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>>(new SMC::ParticleFilterResamplingPercent<MyState>(0.4)));
//pf.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingKLD<MyState>>(new K::ParticleFilterResamplingKLD<MyState>()));
//pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>>(new SMC::ParticleFilterResamplingKLD<MyState>()));
}
pf.setNEffThreshold(0.95);
//estimation
pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationKernelDensity<MyState, 3>>(new SMC::ParticleFilterEstimationKernelDensity<MyState, 3>()));
/** Smoothing Init */
K::BackwardSimulation<MyState> bf(Settings::numBSParticles);
SMC::FastKDESmoothing<MyState, MyControl, MyObs> bf(Settings::numParticles, map, Settings::Grid::gridSize_cm, Settings::KDE::bandwidth);
if(Settings::Smoothing::activated){
//create the backward smoothing filter
bf.setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
bf.setSampler( std::unique_ptr<SMC::CumulativeSampler<MyState>>(new SMC::CumulativeSampler<MyState>()));
bool smoothing_resample = false;
//bf->setNEffThreshold(1.0);
if(smoothing_resample)
bf.setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf.setTransition(std::unique_ptr<BFTrans>( new BFTrans) );
bf.setResampling( std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>>(new SMC::ParticleFilterResamplingSimple<MyState>()) );
//bf.setTransition(std::unique_ptr<BFTrans>( new BFTrans) );
bf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
//Smoothing estimation
bf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
bf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
}
@@ -216,8 +220,8 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
RelativePressure relBaro;
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
K::Statistics<float> errorStats;
K::Statistics<float> errorStatsSmoothing;
Stats::Statistics<float> errorStats;
Stats::Statistics<float> errorStatsSmoothing;
//file writing for error data
const long int t = static_cast<long int>(time(NULL));
@@ -293,8 +297,8 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
plot.setEst(estPos);
plot.setGT(gtPos);
plot.addEstimationNode(estPos);
plot.addParticles(pf.getParticles());
//plot.addEstimationNode(estPos);
//plot.addParticles(pf.getParticles());
/** error calculation stuff */
float err_m = gtPos.getDistance(estPos);
@@ -304,34 +308,36 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
/** smoothing stuff */
if(Settings::Smoothing::activated){
//save the current estimation for later smoothing.
pfHistory.push_back(pf.getNonResamplingParticles());
tsHistory.push_back(ts.ms());
// add everything from the forward step to the history
pfHistory.add(ts, pf.getNonResamplingParticles(), ctrl, obs);
//backward filtering
MyState estBF = est;
if(pfHistory.size() > Settings::Smoothing::lag){
//((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
MyState estBF = bf.update(pfHistory, Settings::Smoothing::lag);
bf.reset();
//use every n-th (std = 1) particle set of the history within a given lag (std = 5)
for(int i = 0; i <= Settings::Smoothing::lag; ++i){
((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
estBF = bf.update(pfHistory[(pfHistory.size() - 1) - i]);
}
// get ground truth position at lag time
Point3 estPosSmoothing = estBF.position.inMeter();
Point3 gtPosSmoothed;
if(pfHistory.size() <= Settings::Smoothing::lag){
gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(pfHistory.getFirstTimestamp().ms()));
} else {
gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(pfHistory.getTimestamp(Settings::Smoothing::lag).ms()));
}
Point3 estPosSmoothing = estBF.position.inMeter();
Point3 gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(tsHistory[(tsHistory.size() - 1) - Settings::Smoothing::lag]));
//plot
plot.addEstimationNodeSmoothed(estPosSmoothing);
plot.addParticles(bf.getbackwardParticles().back());
if(Settings::Smoothing::lag >= pfHistory.size()){
// error between GT and smoothing
float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing);
errorStatsSmoothing.add(errSmoothing_m);
errorFileSmoothing << errSmoothing_m << "\n";
}
// error between GT and smoothing
float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing);
errorStatsSmoothing.add(errSmoothing_m);
errorFileSmoothing << errSmoothing_m << "\n";
}
//plot misc
@@ -426,26 +432,26 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
int main(int argc, char** argv) {
K::Statistics<float> statsAVG;
K::Statistics<float> statsMedian;
K::Statistics<float> statsSTD;
K::Statistics<float> statsQuantil;
K::Statistics<float> tmp;
Stats::Statistics<float> statsAVG;
Stats::Statistics<float> statsMedian;
Stats::Statistics<float> statsSTD;
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
for(int i = 0; i < 10; ++i){
// tmp = run(data.FloorOneToThree, 0, "Wifi-Dongle-Test", Settings::Path_DongleTest::path4);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1);
tmp = run(data.SecondFloorOnly, 0, "KDE-Smoothing-Test", Settings::Path_DongleTest::path1);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());
statsQuantil.add(tmp.getQuantile(0.75));
// tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
std::cout << "Iteration " << i << " completed" << std::endl;;
}