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

change simple transition model

Browse Source 
added klb transition models
added debugging output
This commit is contained in:
toni
2017-04-18 11:18:37 +02:00
parent 1f6df67010
commit fdbd984584
7 changed files with 190 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
code/main.cpp
View File

@@ -166,12 +166,11 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
//init the mode filters
std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>> modes;
std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f));
// mode 1
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, init, Settings::Mode1::modeProbability);
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInit(grid, 1));
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, initMode1, Settings::Mode1::modeProbability);
mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
mode1.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
@@ -181,7 +180,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
modes.push_back(mode1);
// mode 2
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, init, Settings::Mode2::modeProbability);
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInit(grid, 2));
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, initMode2, Settings::Mode2::modeProbability);
mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid)));
mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
mode2.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
@@ -197,7 +197,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
K::InteractingMultipleModelParticleFilter<MyState, MyControl, MyObs> IMMAPF(modes, transitionProbabilityMatrix);
IMMAPF.setMixingSampler(std::unique_ptr<K::MixingSamplerDivergency<MyState, MyControl, MyObs>>(new K::MixingSamplerDivergency<MyState, MyControl, MyObs>()));
IMMAPF.setJointEstimation(std::unique_ptr<K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>>(new K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>()));
IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
//IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransitionNormal>(new ModeProbabilityTransitionNormal(Settings::Mixing::lambda)));
Timestamp lastTimestamp = Timestamp::fromMS(0);
@@ -299,8 +300,9 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
plot.setEst(estPos);
plot.setGT(gtPos);
plot.addParticles1(IMMAPF.getModes()[0].getParticles());
plot.addParticles2(IMMAPF.getModes()[1].getParticles());
plot.addEstimationNode(estPos);
plot.addParticles(IMMAPF.getModes()[0].getParticles());
plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter());
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
@@ -308,10 +310,34 @@ 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";
if(Settings::useKLB){
plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n";
plot.gp << "set label 1003 at screen 0.02, 0.92 'KLD: " << ":" << kld_data.back() << "'\n";
plot.gp << "set label 1004 at screen 0.90, 0.98 'act:" << obs.activity << "'\n";
plot.gp << "set label 1005 at screen 0.90, 0.08 'Prob. Mode1:" << IMMAPF.getModes()[0].getModePosteriorProbability() << "'\n";
plot.gp << "set label 1006 at screen 0.90, 0.06 'Prob. Mode2:" << IMMAPF.getModes()[1].getModePosteriorProbability() << "'\n";
int ones = 0;
int twos = 0;
for(int i = 0; i < IMMAPF.getModes()[0].getParticles().size(); ++i){
int mode1 = IMMAPF.getModes()[0].getParticles()[i].state.curMode;
int mode2 = IMMAPF.getModes()[1].getParticles()[i].state.curMode;
if(mode1 == 1){
++ones;
} else {
++twos;
}
if(mode2 == 1){
++ones;
} else {
++twos;
}
}
plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << obs.activity << "'\n";
plot.gp << "set label 1007 at screen 0.90, 0.04 'Part. Mode1:" << ones << "'\n";
plot.gp << "set label 1008 at screen 0.90, 0.02 'Part. Mode2:" << twos << "'\n";
// error between GT and estimation
float err_m = gtPos.getDistance(estPos);
@@ -362,24 +388,22 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
K::GnuplotPlotElementLines lines;
//save as screenshot for klb
if(Settings::useKLB){
std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_shennendistance.png'\n";
std::string path = evalDir + "/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);
}
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
lines.addSegment(p1, p2);
}
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
std::cout << "finished" << std::endl;
sleep(1);
@@ -399,7 +423,6 @@ int main(int argc, char** argv) {
// run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
// //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
Settings::useKLB = true;
//run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);