toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added activty plotting

Browse Source 
added particle logging
added gfx to tex
This commit is contained in:
kazu
2016-05-04 19:23:41 +02:00
parent e3d7e83159
commit a6790c3db3
9 changed files with 1588 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
code/eval/FixedLagEvalBase.h
View File

@@ -101,6 +101,8 @@ public:
return GridPoint(p.x, p.y, p.z);
}
GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
@@ -206,6 +208,9 @@ public:
std::vector<float> errorsDistNorm;
std::vector<float> errorsDistSmooth;
// acticity output
std::vector<int> activity;
// number of updates to skip before starting the smoothing process
const int skip = 35;
@@ -321,6 +326,7 @@ public:
const float errDist = gtw.getMinDist(curEst); // minimum distance between estimation and ground-truth
activity.push_back((int)actDet.getCurrentActivity());
// count number of transitions
++cnt;
@@ -355,6 +361,9 @@ public:
}
//use and visualize the smoothed particle set
const Point3 curSmoothedEst = estBF.pCur;
@@ -377,6 +386,7 @@ public:
errorsSmooth.push_back(errSmoothed);
errorsDistSmooth.push_back(errDistSmoothed);
//std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl;
//std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl;
@@ -455,6 +465,12 @@ public:
oError.close();
}
{ // detailled activity over time
std::ofstream oAct("/tmp/act_" + runName + ".dat");
for (int i : activity) {oAct << i << "\n";}
oAct.close();
}
// plot
//vis.clearStates();
vis.gp.setTerminal("png", K::GnuplotSize(2560 * 2.54, 1440 * 2.54) );

9
code/eval/SmoothingEval1.h
View File

@@ -2,6 +2,7 @@
#define SMOOTHINGEVAL1_H
#include "SmoothingEvalBase.h"
#include "SmoothingEvalBaseLOG.h"
#include "FixedLagEvalBase.h"
#include "../DijkstraMapper.h"
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
@@ -21,7 +22,7 @@
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h>
class SmoothingEval1 : public SmoothingEvalBase {
class SmoothingEval1 : public SmoothingEvalBaseLOG {
public:
@@ -51,9 +52,9 @@ public:
//create the backward smoothing filter
bf = new K::BackwardSimulation<MyState>(MiscSettings::numBSParticles);
//bf = new K::CondensationBackwardFilter<MyState>;
bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
//bf = new K::BackwardSimulation<MyState>(MiscSettings::numBSParticles);
bf = new K::CondensationBackwardFilter<MyState>;
//bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
}

93
code/eval/SmoothingEvalBase.h → code/eval/SmoothingEvalBaseLOG.h
View File

@@ -1,5 +1,5 @@
#ifndef SMOOTHINGEVALBASE_H
#define SMOOTHINGEVALBASE_H
#ifndef SMOOTHINGEVALBASELOG_H
#define SMOOTHINGEVALBASELOG_H
#include "../Settings.h"
#include "../Helper.h"
@@ -40,7 +40,11 @@
#include "../frank/OrientationSensorReader.h"
class SmoothingEvalBase {
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
class SmoothingEvalBaseLOG {
protected:
@@ -79,7 +83,73 @@ protected:
public:
SmoothingEvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {
void dumpParticles(const std::string& mode, const int idx, const std::vector<K::Particle<MyState>>& particles, const Point3 curEst) {
std::string a = "/tmp/" + runName;
std::string b = a + "/particles";
std::string c = b + "/" + mode + "_" + std::to_string(idx) + ".dat";
std::string d = b + "/" + "plot" + "_" + std::to_string(idx) + ".gp";
mkdir(a.c_str(), 0777);
mkdir(b.c_str(), 0777);
std::string file = c;
// std::unordered_map<uint64_t, float> map;
// for (const K::Particle<MyState>& p : particles) {
// const float dz = std::abs(p.state.pCur.z - curEst.z);
// //if (dz > 20) {continue;} // skip particles on other levels
// const uint64_t idx = (uint64_t) p.state.pCur.z * 0xFFFFFFFFUL + (int) p.state.pCur.y * 0xFFFF + (int) p.state.pCur.x;
// //const int x = idx & 0xFFFF; if (x - p.state.pCur.x > 10) {throw 1;} // sanity check
// //const int y = idx / 0xFFFF; if (y - p.state.pCur.y > 10) {throw 1;} // sanity check
// map[idx] += p.weight;
// }
// how to plot
std::ofstream plot(d);
plot << "splot\\\n";
plot << "'filtered_" << (idx) << ".dat' every ::2 using 1:2:3:4 with points palette pt 3 notitle,\\\n";
plot << "'filtered_" << (idx) << ".dat' every ::0::0 using 1:2:3:4 with points palette pt 3 ps 5 title 'avg',\\\n";
plot << "'filtered_" << (idx) << ".dat' every ::1::1 using 1:2:3:4 with points palette pt 3 ps 3 title 'avg50',\\\n";
plot << "'smoothed_" << (idx) << ".dat' every ::2 using ($$1+1000):2:3:4 with points palette pt 7 notitle,\\\n";
plot << "'smoothed_" << (idx) << ".dat' every ::0::0 using ($$1+1000):2:3:4 with points palette pt 6 ps 5 title 'avg',\\\n";
plot << "'smoothed_" << (idx) << ".dat' every ::1::1 using ($$1+1000):2:3:4 with points palette pt 6 ps 3 title 'avg50',\\\n";
std::ofstream out(file);
// first two entries are different : avg and avg 50%
K::ParticleFilterEstimationWeightedAverage<MyState> avg;
K::ParticleFilterEstimationOrderedWeightedAverage<MyState> avg50(0.5);
const MyState sAvg = avg.estimate((std::vector<K::Particle<MyState>>&) particles);
const MyState sAvg50 = avg50.estimate((std::vector<K::Particle<MyState>>&) particles);
out << sAvg.pCur.x << " " << sAvg.pCur.y << " " << sAvg.pCur.z << " 0\n";
out << sAvg50.pCur.x << " " << sAvg50.pCur.y << " " << sAvg50.pCur.z << " 0\n";
for (const K::Particle<MyState>& p : particles) {
const float dz = std::abs(p.state.pCur.z - curEst.z);
if (dz > 20) {continue;} // skip particles on other levels
out << p.state.pCur.x << " " << p.state.pCur.y << " " << p.state.pCur.z << " " << p.weight << "\n";
}
// for (auto it : map) {
// const uint64_t idx = it.first;
// const int x = idx % 0xFFFF;
// const int y = idx / 0xFFFF % 0xFFFF;
// const int z = idx / 0xFFFF / 0xFFFF;
// out << x << " " << y << " " << z << " " << it.second << "\n";
// }
out.close();
}
void addPlotDump(const int idx, const std::string txt) {
std::string c = "/tmp/" + runName + "/" + "particles" + "/plot_" + std::to_string(idx) + ".gp";
std::ofstream out(c, std::fstream::app);
out << txt;
out.close();
}
SmoothingEvalBaseLOG() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {
// build the grid
Helper::buildTheGrid(grid, floors);
@@ -283,6 +353,7 @@ public:
obs.currentActivity = actDet.getCurrentActivity();
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
@@ -307,6 +378,7 @@ public:
++cnt;
// skip the first 24 scans due to uniform distribution start
if (cnt > 35) {
statsFiltering.add(err);
@@ -319,7 +391,10 @@ public:
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
}
dumpParticles("filtered", cnt-36, pf->getNonResamplingParticles(), curEst);
}
// plot
vis.clearStates();
@@ -376,6 +451,12 @@ public:
smoothedEst.push_back(estBF.pCur);
}
for(int t = 0; t < smoothedEst.size(); ++t){
int idx = smoothedEst.size() - 1 - t;
const Point3 curSmoothedEst = smoothedEst[idx];
dumpParticles("smoothed", t, bf->getbackwardParticles()[idx], curSmoothedEst);
}
std::reverse(smoothedEst.begin(), smoothedEst.end());
for(int t = 0; t < smoothedEst.size(); ++t){
@@ -429,6 +510,8 @@ public:
usleep(1000*33);
}
//statsout2.close();
{ // detailled error-description (normally filtered)