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

3
.gitignore vendored
View File

@@ -19,9 +19,6 @@ code/CMakeLists.txt.user
code-build/
measurements/
##cmake
code/CMakeLists.txt.user
## Core latex/pdflatex auxiliary files:
*.aux
*.lof

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();
}
};

23
code/eval/SmoothingEval1.h
View File

@@ -52,6 +52,7 @@ public:
//create the backward smoothing filter
bf = new K::BackwardSimulation<MyState>(500);
//bf = new K::CondensationBackwardFilter<MyState>;
bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
@@ -118,21 +119,29 @@ public:
// Hier dauert sehr laaaaaaaannnnnnnggge @Frank
void fixedIntervallDijkstraTransPath4(){
runName = "fixedIntervallSimpleTrans";
bool smoothing_resample = false;
smoothing_time_delay = 1;
runName = "fixedIntervallDijkstra";
BarometerEvaluation::barometerSigma = 0.10;
sr = new SensorReader("./measurements/bergwerk/path4/nexus/1454776525797.csv"); // forward
srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/Steps2.txt");
sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
//Smoothing using Simple Trans
//Smoothing Variables
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Dijkstra
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );

33
code/eval/SmoothingEvalBase.h
View File

@@ -34,6 +34,7 @@
#include "../frank/OrientationSensorReader.h"
static int smoothing_time_delay = 0;
float stepSize = 0.71;
class SmoothingEvalBase {
@@ -237,7 +238,7 @@ public:
while (!step_observations.empty() && current_time > step_observations.front().ts) {
const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
obs.step->steps++;
ctrl.walked_m = obs.step->steps * 0.71;
ctrl.walked_m = obs.step->steps * stepSize;
}
// process all occurred steps
@@ -268,8 +269,8 @@ public:
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
// skip the first 10 scans due to uniform distribution start
if (++cnt > 10) {
// skip the first 24 scans due to uniform distribution start
if (++cnt > 24) {
pathEst.push_back(curEst);
const float err = diff.length();
stats.add(err);
@@ -319,20 +320,28 @@ public:
// ========================= //
//File
std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats");
//std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats");
stats.reset();
bf->reset();
for(int i = pfHistory.size() - 1; i > 0; i -= smoothing_time_delay){
//Set time
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
MyState estBF;
int currentParticleSetToSmoothAtTimet = 0;
while(currentParticleSetToSmoothAtTimet != pfHistory.size() - 1){
//iterate thru all particle sets from T to t (currentParticleSetToSmoothAtTimeT)
for(int i = pfHistory.size() - 1; i >= currentParticleSetToSmoothAtTimet; i -= smoothing_time_delay){
//Set time
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
estBF = bf->update(pfHistory[i]);
}
const MyState estBF = bf->update(pfHistory[i]);
const Point3 curSmoothedEst = estBF.pCur;
smoothedEst.push_back(curSmoothedEst);
// error calculation. compare ground-truth to estimation
const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[i]);
const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[currentParticleSetToSmoothAtTimet]);
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
const float errSmoothed = diffSmoothed.length();
@@ -346,7 +355,7 @@ public:
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
vis.setTimestamp(tsHistory[i]);
vis.setTimestamp(tsHistory[currentParticleSetToSmoothAtTimet]);
vis.addGroundTruth(gtw);
vis.addEstPath(smoothedEst);
vis.setEstAndShould(curSmoothedEst, curGTSmoothed);
@@ -367,9 +376,11 @@ public:
// prevent gnuplot errors
usleep(1000*33);
++currentParticleSetToSmoothAtTimet;
}
statsout2.close();
//statsout2.close();
}

5
code/main.cpp
View File

@@ -89,9 +89,12 @@ int main(void) {
// testModelWalk();
SmoothingEval1 eval;
eval.fixedIntervallSimpleTransPath4();
eval.fixedIntervallDijkstraTransPath4();
eval.run();
//Eval1 eval;
//eval.bergwerk_path4_nexus_multi();
//eval.run();
// {SmoothingEval1 eval; eval.bergwerk_path1_nexus_simple(); eval.run();}
// //{SmoothingEval1 eval; eval.bergwerk_path1_nexus_imp(); eval.run();}

17
code/particles/smoothing/MySmoothingTransition.h
View File

@@ -102,11 +102,18 @@ public:
const double distProb = distWalk.getProbability(distDijkstra_m);
//getProb using the angle(heading) between src and dst
double angle = 0.0;
if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
}
const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
// double angle = 0.0;
// if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
// angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
// }
// const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
// is the heading change similiar to the measurement?
double p2AngleDeg = p2->state.walkState.heading.getRAD() * 180/3.14159265359;
double p1AngleDeg = p1->state.walkState.heading.getRAD() * 180/3.14159265359;
double diffDeg = p2AngleDeg - p1AngleDeg;
const double headingProb = K::NormalDistribution::getProbability(p1->state.angularHeadingChange, smoothing_heading_sigma, diffDeg);
//assert(headingProb != 0.0);
//assert(distProb != 0.0);

BIN
tex/bare_conf.dvi
View File

Binary file not shown.