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

started changes for implementing fast smoothing

Browse Source 
different new evaluations
small changes in transition
This commit is contained in:
toni
2017-11-11 01:41:51 +01:00
parent 232175c3dc
commit bea81eab62
5 changed files with 168 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
main.cpp
View File

@@ -20,7 +20,8 @@
//toni
const std::string mapDir = "/home/toni/Documents/programme/localization/IndoorMap/maps/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/DynLag/code/data/";
const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/";
const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/";
const std::string errorDir = dataDir + "results/";
/** describes one dataset (map, training, parameter-estimation, ...) */
@@ -68,6 +69,20 @@ struct Data {
mapDir + "grid_Stock_1-3_03.dat"
};
DataSetup MotionAxisTest = {
mapDir + "SHL40_noElevator.xml",
{
dataDir + "Path0_0.csv"
},
mapDir + "wifi_fp_all.dat",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
mapDir + "grid_SHL40_noElevator.dat"
};
} data;
Floorplan::IndoorMap* MyState::map;
@@ -129,7 +144,7 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
plot.addStairs(map);
plot.gp << "set autoscale xy\n";
//plot.addGrid(grid);
plot.splot.getView().setEnabled(false);
// init ctrl and observation
MyControl ctrl;
@@ -141,8 +156,8 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
std::vector<int64_t> tsHistory;
//filter init
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
//K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)));
//K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
//pf.setTransition(std::unique_ptr<PFTransKLDSampling>(new PFTransKLDSampling(grid, &ctrl)));
@@ -159,7 +174,7 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingKLD<MyState>>(new K::ParticleFilterResamplingKLD<MyState>()));
}
pf.setNEffThreshold(0.85);
pf.setNEffThreshold(0.95);
//estimation
pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -192,9 +207,11 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
Timestamp lastTimestamp = Timestamp::fromMS(0);
StepDetection sd;
TurnDetection td;
PoseDetection pd;
TurnDetection td(&pd);
MotionDetection md;
ActivityButterPressure act;
//ActivityDetector act;
RelativePressure relBaro;
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
@@ -234,7 +251,7 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
++ctrl.numStepsSinceLastTransition;
}
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
td.addAccelerometer(ts, _acc.data);
pd.addAccelerometer(ts, _acc.data);
} else if (e.type == Offline::Sensor::GYRO) {
const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
@@ -248,7 +265,8 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
obs.sigmaPressure = relBaro.getSigma();
//activity recognition
obs.activity = act.add(ts, fr.getBarometer()[e.idx].data);
act.add(ts, fr.getBarometer()[e.idx].data);
obs.activity = act.get();
//activity for transition
} else if (e.type == Offline::Sensor::LIN_ACC) {
@@ -322,7 +340,16 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
if(Settings::useKLB){
plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n";
}
plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << obs.activity << "'\n";
plot.gp << "set label 1002 at screen 0.95, 0.98 'act:" << static_cast<int>(obs.activity) << "'\n";
//draw gyro angle and motion angle
//turn angle plot
static float angleSumTurn = 0; angleSumTurn += ctrl.turnSinceLastTransition_rad;
plot.showAngle(1, angleSumTurn + M_PI, Point2(0.9, 0.9), "Turn: ");
//motion angle plot
static float angleSumMotion = 0; angleSumMotion += ctrl.motionDeltaAngle_rad;
plot.showAngle(2, angleSumMotion + M_PI, Point2(0.9, 0.8), "Motion: ");
/** Draw everything */
plot.show();
@@ -387,6 +414,7 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
plot.splot.getView().setEnabled(false);
}
std::cout << "finished" << std::endl;
@@ -406,7 +434,13 @@ int main(int argc, char** argv) {
for(int i = 0; i < 10; ++i){
tmp = run(data.FloorOneToThree, 2, "Wifi-Dongle-Test", Settings::Path_DongleTest::path6);
// tmp = run(data.FloorOneToThree, 0, "Wifi-Dongle-Test", Settings::Path_DongleTest::path4);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());