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

too many things... sorry :D

Browse Source
This commit is contained in:
toni
2017-03-30 18:52:49 +02:00
parent 212573cba2
commit 9b21e5627d
3 changed files with 281 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
code/Settings.h
View File

@@ -7,7 +7,7 @@
namespace Settings {
const int numParticles = 5000;
const int numParticles = 10000;
namespace IMU {
const float turnSigma = 2.5; // 3.5
@@ -46,7 +46,7 @@ namespace Settings {
const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
}
namespace WiFiModelOptimized {
namespace WiFiModel_{
constexpr float sigma = 8.0;
/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
constexpr float TXP = -64.5905;
@@ -95,7 +95,7 @@ namespace Settings {
}
namespace Paths_IPIN2017 {
const std::vector<int> path1 = {0, 1, 2, 3, 4, 5, 6, 7, 700, 8, 9, 10};
const std::vector<int> path1 = {0, 1, 2, 3, 4, 5, 6, 700, 7, 8, 9, 10};
const std::vector<int> path2 = {11, 12, 3, 2, 1, 0, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 11};
const std::vector<int> path3 = {31, 32, 33, 34, 35, 36, 37, 38};
}

221
code/filter/KLB.h Normal file
View File

@@ -0,0 +1,221 @@
#ifndef KLB_H
#define KLB_H
#include <chrono>
#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <KLib/math/statistics/Statistics.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/FixedFrequencyInterpolator.h>
#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/math/divergence/JensenShannon.h>
#include <Indoor/data/Timestamp.h>
#include <KLib/math/filter/particles/Particle.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
//#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationKernelDensity.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingDivergence.h>
#include "Structs.h"
#include "../Plotti.h"
#include "Logic.h"
#include "../Settings.h"
static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& samplesWifi){
Distribution::KernelDensity<double, MyState> parzen([&](MyState state){
int size = particles.size();
double prob = 0;
#pragma omp parallel for reduction(+:prob) num_threads(6)
for(int i = 0; i < size; ++i){
double distance = particles[i].state.position.getDistanceInCM(state.position);
prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
}
return prob;
;});
std::vector<double> probsWifiV;
std::vector<double> probsParticleV;
//just for plottingstuff
std::vector<K::Particle<MyState>> samplesParticles;
const int step = 4;
int i = 0;
for(K::Particle<MyState> particle : samplesWifi){
if(++i % step != 0){continue;}
MyState state(GridPoint(particle.state.position.x_cm, particle.state.position.y_cm, particle.state.position.z_cm));
double probiParticle = parzen.getProbability(state);
probsParticleV.push_back(probiParticle);
double probiwifi = particle.weight;
probsWifiV.push_back(probiwifi);
//samplesParticles.push_back(K::Particle<MyState>(state, probiParticle));
}
//make vectors
Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
//get divergence
//double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
//plotti
//plot.debugDistribution1(samplesWifi);
//plot.debugDistribution1(samplesParticles);
//estimate the mean
//K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
//const MyState estWifi = estimateWifi.estimate(samplesWifi);
//plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
return kld;
}
static double kldFromMultivariatNormal(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& particleWifi){
//kld: particle die resampling hatten nehmen und nv daraus schätzen. vergleiche mit wi-fi
//todo put this in depletionhelper.h
Point3 estPos = state.position.inMeter();
//this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and
//no inverse matrix is possible
std::mt19937_64 rng;
// initialize the random number generator with time-dependent seed
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)};
rng.seed(ss);
// initialize a uniform distribution between -0.0001 and 0.0001
std::uniform_real_distribution<double> unif(-0.0001, 0.0001);
//create a gauss dist for the current particle approx.
Eigen::MatrixXd m(particles.size(), 3);
for(int i = 0; i < particles.size(); ++i){
m(i,0) = (particles[i].state.position.x_cm / 100.0) + unif(rng);
m(i,1) = (particles[i].state.position.y_cm / 100.0) + unif(rng);
m(i,2) = (particles[i].state.position.z_cm / 100.0) + unif(rng);
}
Eigen::VectorXd mean(3);
mean << estPos.x, estPos.y, estPos.z;
Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(m, mean);
//create a gauss dist for wifi
Eigen::MatrixXd covWifi(3,3);
covWifi << Settings::WiFiModel::sigma, 0, 0,
0, Settings::WiFiModel::sigma, 0,
0, 0, 0.01;
// //calc wi-fi prob for every node and get mean vector
// WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model);
// const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
// std::vector<MyNode> allNodes = grid.getNodes();
// std::vector<K::Particle<MyState>> particleWifi;
// //problem! dadurch das ich nur die nodes nehme, verschiebt sich der mittelwert natürlich in die mitte des gebäudes und nicht an den rand
// //muss also die verteilung über mehr nodes oder sampling erstellen!! mittelwert fehler!!!!
// //#pragma omp parallel for num_threads(6)
// for(MyNode node : allNodes){
// double prob = wiFiProbability.getProbability(node, ts, wifiObs);
// K::Particle<MyState> tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob);
// //#pragma omp critical
// particleWifi.push_back(tmp);
// }
// std::vector<double> floors;
// floors.push_back(0.0);
// floors.push_back(4.0);
// floors.push_back(7.4);
// floors.push_back(10.8);
// #pragma omp parallel for num_threads(6)
// for(int x = -20; x < 100; ++x){
// for(int y = -20; y < 75; ++y){
// for(double z : floors){
// double X = x;// / 10.0;
// double Y = y;// / 10.0;
// double Z = z;// / 10.0;
// Point3 pt(X,Y,Z);
// double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), ts, wifiObs);
// K::Particle<MyState> tmp (MyState(GridPoint(X * 100.0, Y * 100.0, Z * 100.0)), prob);
// #pragma omp critical
// particleWifi.push_back(tmp);
// }
// }
// }
//estimate the mean
K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
const MyState estWifi = estimateWifi.estimate(particleWifi);
//get matrix with wifi particles
// Eigen::MatrixXd mW(particleWifi.size(), 3);
// for(int i = 0; i < particleWifi.size(); ++i){
// mW(i,0) = particleWifi[i].state.position.x_cm / 100.0;
// mW(i,1) = particleWifi[i].state.position.y_cm / 100.0;
// mW(i,2) = estWifi.position.z_cm / 100.0;
// }
Eigen::VectorXd meanWifi(3);
meanWifi << estWifi.position.x_cm / 100.0, estWifi.position.y_cm / 100.0, estWifi.position.z_cm / 100.0;
Distribution::NormalDistributionN normWifi(meanWifi, covWifi);
//Distribution::NormalDistributionN normWifi = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mW, meanWifi);
//get the kld distance
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
//plot.debugDistribution1(particleWifi);
//plot.drawNormalN1(normParticle);
//plot.drawNormalN2(normWifi);
//plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
return kld;
}
#endif // KLB_H

98
code/main.cpp
View File

@@ -100,7 +100,7 @@ struct Data {
Floorplan::IndoorMap* MyState::map;
void run(DataSetup setup, int numFile, std::string folder) {
void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
std::vector<double> kld_data;
@@ -143,7 +143,7 @@ void run(DataSetup setup, int numFile, std::string folder) {
Offline::FileReader fr(setup.training[numFile]);
//interpolator for ground truth
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, Settings::Paths_IPIN2017::path1);
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
//gnuplot plot
Plotti plot;
@@ -165,19 +165,23 @@ void run(DataSetup setup, int numFile, std::string folder) {
//filter init
//std::unique_ptr<PFInit> 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(1120.0f, 750.0f, 1080.0f), 90.0f)));
//K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
pf.setEvaluation(std::unique_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
//resampling
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
//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<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<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.95)));
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>()));
@@ -191,6 +195,10 @@ void run(DataSetup setup, int numFile, std::string folder) {
K::Statistics<float> errorStats;
//calc wi-fi prob for every node and get mean vector
WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, WiFiModel);
//file writing for error data
long int t = static_cast<long int>(time(NULL));
std::ofstream errorFile;
@@ -241,17 +249,34 @@ void run(DataSetup setup, int numFile, std::string folder) {
obs.currentTime = ts;
MyState est = pf.update(&ctrl, obs);
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
std::vector<MyNode> allNodes = grid.getNodes();
std::vector<K::Particle<MyState>> particleWifi;
for(MyNode node : allNodes){
double prob = wiFiProbability.getProbability(node, ts, wifiObs);
K::Particle<MyState> tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob);
particleWifi.push_back(tmp);
}
if(kld_data.empty()){
kld_data.push_back(0.0);
}
std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = getKernelDensityProbability;
//std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = kldFromMultivariatNormal;
double kld = 0.0;
MyState est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld);
Point3 estPos = est.position.inMeter();
//double kld = getKernelDensityProbability(pf, WiFiModel, obs, grid, ts, plot);
double kld = kldFromMultivariatNormal(pf, estPos, WiFiModel, obs, grid, ts, plot);
//double kld = kldFromMultivariatNormal(pf, estPos, particleWifi, plot);
kld_data.push_back(kld);
//current ground truth position
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms()));
/** plotting stuff */
plot.pInterest.clear();
@@ -289,25 +314,6 @@ void run(DataSetup setup, int numFile, std::string folder) {
// reset control
ctrl.resetAfterTransition();
//plot image files
// for(int i = 0; i < map->floors.size(); ++i){
// plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t++), i);
// plot.show();
// usleep(10*10);
// }
// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t++), 90);
// plot.show();
// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t++), 0);
// plot.show();
// plot.printOverview("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t++));
// plot.show();
// //reset to qt window
// plot.gp << "set terminal qt size 800,600\n";
}
}
@@ -323,20 +329,20 @@ void run(DataSetup setup, int numFile, std::string folder) {
plot.saveToFile(plotFile);
plotFile.close();
// for(int i = 0; i < map->floors.size(); ++i){
// plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i);
// plot.show();
// usleep(1000*10);
// }
for(int i = 0; i < map->floors.size(); ++i){
plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i);
plot.show();
usleep(10*10);
}
// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90);
// plot.show();
plot.printSideView("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90);
plot.show();
// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0);
// plot.show();
plot.printSideView("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0);
plot.show();
// plot.printOverview("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t));
// plot.show();
plot.printOverview("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t));
plot.show();
//draw kld
@@ -344,6 +350,11 @@ void run(DataSetup setup, int numFile, std::string folder) {
K::GnuplotPlot plotkld;
K::GnuplotPlotElementLines lines;
//save as screenshot
std::string path = "/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_shennendistance.png'\n";
for(int i=0; i < kld_data.size()-1; ++i){
K::GnuplotPoint2 p1(i, kld_data[i]);
@@ -353,11 +364,11 @@ void run(DataSetup setup, int numFile, std::string folder) {
}
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
sleep(1000);
std::cout << "finished" << std::endl;
sleep(1);
}
@@ -367,7 +378,12 @@ int main(int argc, char** argv) {
//run(data.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor
//for(int i = 0; i < 5; ++i){
run(data.IPIN2017, 0, "ipin2017"); // Nexus Path2
//run(data.IPIN2017, 0, "ipin2017"); // Nexus Path2
//run(data.IPIN2017, 1, "ipin2017");
run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
//}
}