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

work in progress.

Browse Source
This commit is contained in:
toni
2016-04-05 18:06:23 +02:00
parent 5f81da109a
commit aeaa4cb59d
7 changed files with 107 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
code/eval/FixedLagEvalBase.h
View File

@@ -8,6 +8,7 @@
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/smoothing/BackwardSimulation.h>
#include <KLib/math/filter/smoothing/CondensationBackwardFilter.h>
#include <KLib/math/filter/smoothing/sampling/ParticleTrajectorieSampler.h>
#include <KLib/math/filter/smoothing/sampling/CumulativeSampler.h>
#include <KLib/math/statistics/Statistics.h>
@@ -69,7 +70,7 @@ protected:
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint!
std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint!
float stepSize = 0.71;
@@ -287,12 +288,12 @@ public:
const float err = diff.length();
// skip the first 24 scans due to uniform distribution start
if (++cnt > 24) {
if (++cnt > 35) {
statsFiltering.add(err);
std::cout << "Filtering: " << statsFiltering.asString() << std::endl;
std::cout << "Filtering: " << se.ts << " " << statsFiltering.asString() << std::endl;
}
if(cnt > 19){
if(cnt > 35){
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
@@ -301,12 +302,12 @@ public:
//fixed-lag smoothing
MyState estBF;
if(pfHistory.size() >= MiscSettings::lag){
if(pfHistory.size() > MiscSettings::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 < MiscSettings::lag; i += MiscSettings::fixedLagGap){
for(int i = 0; i <= MiscSettings::lag; i += MiscSettings::fixedLagGap){
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
estBF = bf->update(pfHistory[(pfHistory.size() - 1) - i]);
@@ -319,12 +320,13 @@ public:
smoothedEst.push_back(curSmoothedEst);
// error calculation. compare ground-truth to estimation
const Point3 curGTSmoothed = gtw.getPosAtTime(se.ts - (MiscSettings::lag * MiscSettings::timeSteps));
//const Point3 curGTSmoothed = gtw.getPosAtTime(se.ts - (MiscSettings::lag * MiscSettings::timeSteps));
const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[(tsHistory.size() - 1) - MiscSettings::lag]);
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
const float errSmoothed = diffSmoothed.length();
statsSmoothing.add(errSmoothed);
std::cout << "Smoothing: " << statsSmoothing.asString() << std::endl;
std::cout << "Smoothing: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl;
//plot
vis.clearStates();
@@ -353,9 +355,9 @@ public:
//std::cout << "Measurement: "<< std::fmod(((obs.orientation.values[0] * 180/3.14159265359) + 720 + 60), 360) << std::endl;
std::cout << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl;
std::cout << "EstimationS: " << estBF.avgAngle << std::endl;
std::cout << "EstimationF: " << est.avgAngle << std::endl;
// std::cout << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl;
// std::cout << "EstimationS: " << estBF.avgAngle << std::endl;
// std::cout << "EstimationF: " << est.avgAngle << std::endl;
//vis.gp << "set label 113 ' EstAngle:" << avgAngleRad << "' at screen 0.1,0.15\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
@@ -410,6 +412,46 @@ public:
// prevent gnuplot errors
usleep(1000*33);
// // append error for each run to a file
// std::ofstream oTError("/tmp/errorsFilter.txt", std::ios_base::app);
// oTError << runName << "\n\t"; statsFiltering.appendTo(oTError); oTError << "\n\n";
// oTError.close();
// // append error for each run to a file
// std::ofstream oSError("/tmp/errorsSmoothing.txt", std::ios_base::app);
// oSError << runName << "\n\t"; statsSmoothing.appendTo(oSError); oSError << "\n\n";
// oSError.close();
// // plot-data
// std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close();
// std::ofstream oEst("/tmp/est_" + runName + ".dat"); vis.estPath.addDataTo(oEst); oEst.close();
// std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close();
// std::ofstream oPlot("/tmp/plot_" + runName + ".gp");
// oPlot << "set terminal eps size 3.4,2\n";
// oPlot << "set output '" << runName << ".eps'\n";
// oPlot << "set termoption dashlength 0.5\n";
// oPlot << "set ticslevel 0\n";
// oPlot << "set view equal xy\n";
// oPlot << "set zrange [0:2200]\n";
// oPlot << "set multiplot layout 1,1 scale 2.7,2.7 offset 0,0.23\n";
// oPlot << "set view 72,33\n";
// oPlot << "unset border\n";
// oPlot << "unset xtics\n";
// oPlot << "unset ytics\n";
// oPlot << "unset ztics\n";
// oPlot << "splot \\\n";
// oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n";
// oPlot << "'path_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
// oPlot << "'est_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' ";
// oPlot.close();
}
};