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

refactoring, easy debugging

Browse Source
This commit is contained in:
toni
2017-03-31 18:34:38 +02:00
parent 9b21e5627d
commit a71f082a45
4 changed files with 103 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
code/main.cpp
View File

@@ -170,11 +170,13 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
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::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>()));
if(Settings::useKLB){
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::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<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
}
pf.setNEffThreshold(0.95);
@@ -190,6 +192,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
StepDetection sd;
TurnDetection td;
MotionDetection md;
ActivityButterPressure act;
RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
@@ -236,6 +239,10 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
obs.relativePressure = relBaro.getPressureRealtiveToStart();
obs.sigmaPressure = relBaro.getSigma();
//activity recognition
obs.activity = act.add(ts, fr.getBarometer()[e.idx].data);
//activity for transition
} else if (e.type == Offline::Sensor::LIN_ACC) {
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
@@ -249,31 +256,39 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
obs.currentTime = ts;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
MyState est;
if(Settings::useKLB){
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);
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);
}
double kld = 0.0;
//set probability distributions.
//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;
//update filter
est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld);
kld_data.push_back(kld);
} else {
est = pf.update(&ctrl, obs);
}
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, particleWifi, plot);
kld_data.push_back(kld);
//current ground truth position
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms()));
@@ -298,8 +313,10 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
//plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n";
int minutes = static_cast<int>(ts.sec()) / 60;
plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast<int>(static_cast<int>(ts.sec())%60) << "'\n";
plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld << "'\n";
//plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << ctrl.barometer.act << "'\n";
if(Settings::useKLB){
plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n";
}
plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << obs.activity << "'\n";
// error between GT and estimation
float err_m = gtPos.getDistance(estPos);
@@ -309,7 +326,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
plot.show();
usleep(10*10);
lastTimestamp = ts;
// reset control
@@ -350,23 +366,25 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
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";
//save as screenshot for klb
if(Settings::useKLB){
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){
for(int i=0; i < kld_data.size()-1; ++i){
K::GnuplotPoint2 p1(i, kld_data[i]);
K::GnuplotPoint2 p2(i+1, kld_data[i+1]);
K::GnuplotPoint2 p1(i, kld_data[i]);
K::GnuplotPoint2 p2(i+1, kld_data[i+1]);
lines.addSegment(p1, p2);
lines.addSegment(p1, p2);
}
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
}
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
std::cout << "finished" << std::endl;
sleep(1);
@@ -380,7 +398,7 @@ int main(int argc, char** argv) {
//for(int i = 0; i < 5; ++i){
//run(data.IPIN2017, 0, "ipin2017"); // Nexus Path2
//run(data.IPIN2017, 1, "ipin2017");
run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
//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);