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

added astar to smoothing transition

Browse Source
This commit is contained in:
toni
2016-04-11 10:57:29 +02:00
parent 5aad08e17c
commit de8783b9ca
13 changed files with 146 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
code/CMakeLists.txt
View File

@@ -64,7 +64,7 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g
-O2
-O0
-DWITH_TESTS
-DWITH_ASSERTIONS

2
code/CMakeLists.txt.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 3.6.0, 2016-04-01T17:34:46. -->
<!-- Written by QtCreator 3.6.0, 2016-04-11T10:29:08. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

3
code/DijkstraMapper.h
View File

@@ -26,6 +26,9 @@ public:
return d;
}
const std::vector<MyGridNode>& getAllNodes() const {return grid.getNodes();}
decltype(grid.neighbors(MyGridNode())) getNeighbors(const MyGridNode& node) const {return grid.neighbors(node);}
float getHeuristic(const MyGridNode& n1, const MyGridNode& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);}
};
/**

1
code/MyGridNode.h
View File

@@ -21,6 +21,7 @@ struct MyGridNode : public GridNode, public GridPoint {
public:
/** needed ctor */
MyGridNode() : GridNode(), GridPoint() {;}
MyGridNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
};

2
code/Settings.h
View File

@@ -17,7 +17,7 @@ namespace MiscSettings {
const int timeSteps = 500;
const int numParticles = 7500;
const int numParticles = 500;
const int lag = 5;

60
code/eval/FixedLagEvalBase.h
View File

@@ -414,44 +414,44 @@ public:
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 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();
// 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();
// 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");
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 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 << "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 << "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 << "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();
oPlot.close();
}
};

2
code/eval/SmoothingEval1.h
View File

@@ -51,7 +51,7 @@ public:
//create the backward smoothing filter
bf = new K::BackwardSimulation<MyState>(500);
bf = new K::BackwardSimulation<MyState>(50);
//bf = new K::CondensationBackwardFilter<MyState>;
bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));

61
code/eval/SmoothingEvalBase.h
View File

@@ -184,7 +184,8 @@ public:
uint64_t lastTransitionTS = 0;
int64_t start_time = -1;
K::Statistics<double> stats;
K::Statistics<double> statsFiltering;
K::Statistics<double> statsSmoothing;
int cnt = 0;
//stats file
@@ -271,14 +272,14 @@ public:
// 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);
std::cout << stats.asString() << std::endl;
pathEst.push_back(curEst);
const float err = diff.length();
statsFiltering.add(err);
std::cout << statsFiltering.asString() << std::endl;
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
}
// plot
@@ -321,7 +322,7 @@ public:
//File
//std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats");
stats.reset();
//stats.reset();
bf->reset();
MyState estBF;
@@ -345,8 +346,8 @@ public:
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
const float errSmoothed = diffSmoothed.length();
stats.add(errSmoothed);
std::cout << stats.asString() << std::endl;
statsSmoothing.add(errSmoothed);
std::cout << statsSmoothing.asString() << std::endl;
// plot
@@ -382,6 +383,44 @@ public:
//statsout2.close();
// 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_fixed_interval.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
oPlot << "'est_fixed_interval.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' ";
oPlot.close();
}
};

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

@@ -7,6 +7,7 @@
#include <KLib/math/distribution/Uniform.h>
#include <Indoor/nav/dijkstra/Dijkstra.h>
#include <Indoor/nav/a-star/AStar.h>
#include <Indoor/grid/Grid.h>
#include "../MyState.h"
@@ -22,6 +23,8 @@ static double smoothing_walk_sigma = 0.5;
static double smoothing_heading_sigma = 15.0;
static double smoothing_baro_sigma = 0.2;
typedef std::pair<const MyGridNode*, const MyGridNode*> my_key_type;
class MySmoothingTransition : public K::BackwardFilterTransition<MyState> {
private:
@@ -75,6 +78,7 @@ public:
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
std::vector<std::vector<double>> predictionProbabilities;
std::map<my_key_type, double> shortestPathMap;
auto p1 = particles_old.begin();
auto p2 = particles_new.begin();
@@ -94,12 +98,40 @@ public:
const MyGridNode* dst = grid->getNodePtrFor(GridPoint(p1->state.pCur.x, p1->state.pCur.y, p1->state.pCur.z));
const MyGridNode* src = grid->getNodePtrFor(GridPoint(p2->state.pCur.x, p2->state.pCur.y, p2->state.pCur.z));
Dijkstra<MyGridNode> dijkstra;
dijkstra.build(src, dst, DijkstraMapper(*grid));
// Dijkstra<MyGridNode> dijkstra;
// dijkstra.build(src, dst, DijkstraMapper(*grid));
double distDijkstra_m = dijkstra.getNode(*src)->cumWeight;
// double distDijkstra_m = dijkstra.getNode(*src)->cumWeight;
const double distProb = distWalk.getProbability(distDijkstra_m);
AStar<MyGridNode> aStar;
DijkstraMapper dm(*grid);
double distDijkstra_m = 0;
std::vector<const MyGridNode*> shortestPath;
// check if this shortestPath was already calculated
std::map<my_key_type, double>::iterator it;
it = shortestPathMap.find(my_key_type(dst, src));
if(it != shortestPathMap.end()){
distDijkstra_m = it->second;
}
else{
//Dijkstra/A* for shortest path
shortestPath = aStar.get(src, dst, dm);
//get distance walked and getProb using the walking model
for(int i = 0; i < shortestPath.size() - 1; ++i){
distDijkstra_m += dm.getWeightBetween(*shortestPath[i], *shortestPath[i+1]);
}
if(distDijkstra_m != distDijkstra_m) {throw "detected NaN";}
//save distance and nodes in lookup map
#pragma omp critical
shortestPathMap.insert(std::make_pair(my_key_type(dst, src), distDijkstra_m));
}
const double distProb = distWalk.getProbability(distDijkstra_m * 0.01);
//getProb using the angle(heading) between src and dst
// double angle = 0.0;
@@ -127,7 +159,7 @@ public:
//if(distance_m != distance_m) {throw "detected NaN";}
//if(distProb != distProb) {throw "detected NaN";}
if(angle != angle) {throw "detected NaN";}
//if(angle != angle) {throw "detected NaN";}
if(headingProb != headingProb) {throw "detected NaN";}
if(floorProb != floorProb) {throw "detected NaN";}
if(floorProb == 0) {throw "detected NaN";}

BIN
tex/bare_conf.dvi
View File

Binary file not shown.

2
tex/bare_conf.tex
View File

@@ -137,7 +137,7 @@
% not capitalized unless they are the first or last word of the title.
% Linebreaks \\ can be used within to get better formatting as desired.
% Do not put math or special symbols in the title.
\title{On Prior Navigation Knowledge in Multi Sensor Indoor Localisation}
\title{On Monte Carlo Smoothing in Multi Sensor Indoor Localisation}
% author names and affiliations

14
tex/chapters/experiments.tex
View File

@@ -1,3 +1,17 @@
\section{Experiments}
ddd \cite{Ville09} dddd
\begin{itemize}
\item Vorwärtsschritt die Ergebnisse und Probleme beschreiben. Zeitlicher Verzug etc.
\item Fixed-Interval Smoothing mit beiden Methoden. Was wird besser? Warum?
\subitem AUf die schlechte Performance eingehen für was könnte man es nutzen?
\item Fixed-lag Smoothing der beiden Methoden
\subitem erstmal ergebnisse mit 3 unterschiedlichen lags. 5, 30, 50 oder so
\subitem Fehlerplot wie beim fusion paper. wo ist der error besonders hoch, warum? warum wurde er besser?
\subitem welche anwendungen gibt es da jeweils?
\subitem plot mit kurven die den fehler bei den unterschiedlichen lag längen angibt.
\item Fixed-lag gap
\subitem einen offset (gap) im smoothing. was bringt es? sinnvoll?
\end{itemize}

7
tex/chapters/introduction.tex
View File

@@ -1,5 +1,10 @@
\section{Introduction}
Smoothing Smoothing Smoothing
Es gibt viele unterschiedliche Indoor Lösungen die auf blabalbal und blabal aufbauen. particle filter ist etabliert und fast alle nutzen einen. gibt solche und solche methoden. aber alle haben mehr oder wenig ähnliche probleme und herrausforderungen. eine generelle lösunge solceh herrausforderungen zu meistern sind smoothiner methoden.
probleme: multimodalitäten. zeitlicher verzug. falsche messungen. etc etc. all das kann mit wissen aus zukünftigen messungen behoben werden.