many changes
This commit is contained in:
308
pf/EvalWalk.h
Normal file
308
pf/EvalWalk.h
Normal file
@@ -0,0 +1,308 @@
|
||||
#ifndef EVALWALK_H
|
||||
#define EVALWALK_H
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilter.h>
|
||||
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
|
||||
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
|
||||
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
||||
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
|
||||
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
||||
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
||||
|
||||
#include "../PlotErrFunc.h"
|
||||
|
||||
#include <thread>
|
||||
|
||||
#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
|
||||
#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
|
||||
#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"
|
||||
|
||||
#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
|
||||
#include "Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h"
|
||||
|
||||
#include "Indoor/sensors/radio/VAPGrouper.h"
|
||||
#include "Indoor/sensors/imu/StepDetection.h"
|
||||
#include "Indoor/sensors/imu/TurnDetection.h"
|
||||
|
||||
#include "Indoor/floorplan/v2/Floorplan.h"
|
||||
#include "Indoor/floorplan/v2/FloorplanReader.h"
|
||||
#include "Indoor/floorplan/v2/FloorplanHelper.h"
|
||||
#include "Indoor/floorplan/v2/FloorplanCeilings.h"
|
||||
|
||||
#include "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h"
|
||||
|
||||
#include "Indoor/sensors/offline/FileReader.h"
|
||||
#include "../Helper.h"
|
||||
#include "PF.h"
|
||||
|
||||
#include <Indoor/sensors/offline/FilePlayer.h>
|
||||
#include <Indoor/sensors/offline/FileReader.h>
|
||||
#include <Indoor/sensors/offline/Listener.h>
|
||||
|
||||
class EvalWalk : public Offline::Listener {
|
||||
|
||||
Grid<MyGridNode>* grid;
|
||||
K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
|
||||
|
||||
std::string runName;
|
||||
WiFiModelLogDistCeiling wifiModel;
|
||||
|
||||
Plotty plotty;
|
||||
|
||||
Offline::FileReader reader;
|
||||
Offline::FilePlayer player;
|
||||
Offline::FileReader::GroundTruth groundTruthLive;
|
||||
|
||||
Timestamp lastTransition;
|
||||
|
||||
MyObservation curObs;
|
||||
MyControl curCtrl;
|
||||
MyState curEst;
|
||||
|
||||
StepDetection stepDetect;
|
||||
TurnDetection turnDetect;
|
||||
|
||||
std::vector<Point3> groundTruth;
|
||||
|
||||
Floorplan::IndoorMap* map;
|
||||
|
||||
public:
|
||||
|
||||
EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map) {
|
||||
|
||||
const std::string saveFile = Settings::pathData + "/grid.dat";
|
||||
grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm);
|
||||
|
||||
plotty.buildFloorplan();
|
||||
|
||||
// deserialize grid
|
||||
std::ifstream inp(saveFile, std::ofstream::binary);
|
||||
if (inp) {
|
||||
grid->read(inp);
|
||||
inp.close();
|
||||
} else {
|
||||
// build the grid
|
||||
GridFactory<MyGridNode> gf(*grid);
|
||||
gf.build(map);
|
||||
Importance::addImportance(*grid);
|
||||
std::ofstream out(saveFile, std::ofstream::binary);
|
||||
grid->write(out);
|
||||
out.close();
|
||||
}
|
||||
|
||||
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)) );
|
||||
|
||||
// TODO: flexible model
|
||||
wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false);
|
||||
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel) );
|
||||
pf->setEvaluation( std::move(eval) );
|
||||
|
||||
wifiModel.saveXML("/tmp/test.xml");
|
||||
wifiModel.loadXML("/tmp/test.xml");
|
||||
|
||||
// resampling step?
|
||||
pf->setNEffThreshold(0.5);
|
||||
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
|
||||
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
|
||||
|
||||
// state estimation step
|
||||
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
|
||||
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
|
||||
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void walk1() {
|
||||
|
||||
runName = "path2_forward_simple";
|
||||
std::string path = Settings::path1a;
|
||||
groundTruth = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path1);
|
||||
|
||||
//GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
||||
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
|
||||
|
||||
reader.open(path);
|
||||
groundTruthLive = reader.getGroundTruth(map, Settings::GroundTruth::path1);
|
||||
player.setReader(&reader);
|
||||
player.setListener(this);
|
||||
player.start();
|
||||
|
||||
// wait for completion
|
||||
player.join();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) override {
|
||||
|
||||
const float delta_rad = turnDetect.addGyroscope(ts, data);
|
||||
curCtrl.turnSinceLastTransition_rad += delta_rad;
|
||||
|
||||
}
|
||||
|
||||
virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) override {
|
||||
turnDetect.addAccelerometer(ts, data);
|
||||
const bool step = stepDetect.add(ts, data);
|
||||
if (step) {
|
||||
++curCtrl.numStepsSinceLastTransition;
|
||||
}
|
||||
gotSensorData(ts);
|
||||
}
|
||||
|
||||
virtual void onGravity(const Timestamp ts, const GravityData data) override {
|
||||
;
|
||||
}
|
||||
|
||||
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) override {
|
||||
std::cout << "WIFI" << std::endl;
|
||||
curObs.wifi = data;
|
||||
}
|
||||
|
||||
virtual void onBarometer(const Timestamp ts, const BarometerData data) override {
|
||||
;
|
||||
}
|
||||
|
||||
virtual void onGPS(const Timestamp ts, const GPSData data) override {
|
||||
curObs.gps = data;
|
||||
}
|
||||
|
||||
virtual void onCompass(const Timestamp ts, const CompassData data) override {
|
||||
const float newAzimuth =- data.azimuth + M_PI/2; // oriented towards north for our map
|
||||
const float newAzimuth_safe = Angle::makeSafe_2PI(newAzimuth);
|
||||
const float diff = Angle::getSignedDiffRAD_2PI(curCtrl.compassAzimuth_rad, newAzimuth_safe);
|
||||
curCtrl.compassAzimuth_rad += diff * 0.01;
|
||||
curCtrl.compassAzimuth_rad = Angle::makeSafe_2PI(curCtrl.compassAzimuth_rad);
|
||||
//curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
/** called when any sensor has received new data */
|
||||
void gotSensorData(const Timestamp ts) {
|
||||
curObs.currentTime = ts;
|
||||
filterUpdateIfNeeded();
|
||||
}
|
||||
|
||||
/** check whether its time for a filter update, and if so, execute the update and return true */
|
||||
bool filterUpdateIfNeeded() {
|
||||
|
||||
static float avgSum = 0;
|
||||
static int avgCount = 0;
|
||||
|
||||
// fixed update rate based on incoming sensor data
|
||||
// allows working with live data and faster for offline data
|
||||
const Timestamp diff = curObs.currentTime - lastTransition;
|
||||
if (diff >= Settings::Filter::updateEvery) {
|
||||
|
||||
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
|
||||
const Timestamp err = diff - Settings::Filter::updateEvery;
|
||||
lastTransition = curObs.currentTime - err;
|
||||
|
||||
const Timestamp ts1 = Timestamp::fromUnixTime();
|
||||
filterUpdate();
|
||||
const Timestamp ts2 = Timestamp::fromUnixTime();
|
||||
const Timestamp tsDiff = ts2-ts1;
|
||||
|
||||
//const QString filterTime = QString::number(tsDiff.ms());
|
||||
//avgSum += tsDiff.ms(); ++avgCount;
|
||||
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
|
||||
return true;
|
||||
|
||||
} else {
|
||||
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
K::Statistics<float> statsErr;
|
||||
|
||||
|
||||
/** perform a filter-update (called from a background-loop) */
|
||||
void filterUpdate() {
|
||||
|
||||
static PlotErrFunc pef("\\small{error (m)}", "\\small{updates (\\%)}");
|
||||
|
||||
std::cout << "update" << std::endl;
|
||||
|
||||
MyControl ctrlCopy = curCtrl;
|
||||
static float absHead = M_PI/2; absHead += ctrlCopy.turnSinceLastTransition_rad;
|
||||
|
||||
//lastEst = curEst;
|
||||
curEst = pf->update(&curCtrl, curObs);
|
||||
|
||||
const Point3 curGT = groundTruthLive.get(lastTransition);
|
||||
|
||||
plotty.setCurEst(curEst.position.inMeter());
|
||||
plotty.setGroundTruth(curGT);
|
||||
|
||||
// error between ground-truth and estimation
|
||||
const float estRealErr = curEst.position.inMeter().getDistance(curGT);
|
||||
statsErr.add(estRealErr);
|
||||
pef.clear();
|
||||
pef.add("", statsErr);
|
||||
pef.plot();
|
||||
|
||||
std::cout << statsErr.asString() << std::endl;
|
||||
|
||||
const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
|
||||
plotty.pathEst.add(p3/100);
|
||||
|
||||
plotty.particles.clear();
|
||||
for (const auto p : pf->getParticles()) {
|
||||
const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
||||
plotty.particles.add(p3/100);
|
||||
}
|
||||
|
||||
// GT
|
||||
plotty.pathReal.clear();
|
||||
for (const Point3 pt : groundTruth) {
|
||||
plotty.pathReal.add(K::GnuplotPoint3(pt.x, pt.y, pt.z));
|
||||
}
|
||||
|
||||
std::string title =
|
||||
" time " + std::to_string(curObs.currentTime.sec()) +
|
||||
" steps: " + std::to_string(ctrlCopy.numStepsSinceLastTransition) +
|
||||
" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad);
|
||||
plotty.setTitle(title);
|
||||
|
||||
{
|
||||
Point2 cen(0.1, 0.9);
|
||||
Point2 dir(std::cos(absHead), std::sin(absHead));
|
||||
Point2 arr = cen + dir * 0.1;
|
||||
plotty.gp << "set arrow 1 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
|
||||
dir = Point2(std::cos(ctrlCopy.compassAzimuth_rad), std::sin(ctrlCopy.compassAzimuth_rad));
|
||||
arr = cen + dir * 0.05;
|
||||
plotty.gp << "set arrow 2 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
|
||||
}
|
||||
|
||||
|
||||
// plot
|
||||
plotty.plot();
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||||
|
||||
curCtrl.resetAfterTransition();
|
||||
|
||||
// const MyGridNode* node = grid->getNodePtrFor(curEst.position);
|
||||
// if (node) {
|
||||
// try {
|
||||
// pathToDest = trans->modDestination.getShortestPath(*node);
|
||||
// } catch (...) {;}
|
||||
// }
|
||||
// mainController->getMapView()->showGridImportance();
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // EVALWALK_H
|
||||
14
pf/PF.h
14
pf/PF.h
@@ -21,6 +21,7 @@
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h>
|
||||
|
||||
#include "../Settings.h"
|
||||
|
||||
@@ -108,7 +109,7 @@ struct MyControl {
|
||||
int numStepsSinceLastTransition = 0;
|
||||
|
||||
/** absolute heading in radians */
|
||||
float compass_azimuth = 0;
|
||||
float compassAzimuth_rad = 0;
|
||||
|
||||
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
|
||||
/** currently detected activity */
|
||||
@@ -163,19 +164,24 @@ public:
|
||||
GridWalker<MyGridNode, MyState> walker;
|
||||
|
||||
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
|
||||
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
|
||||
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
|
||||
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
|
||||
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
|
||||
|
||||
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modRelHead;
|
||||
WalkModuleAbsoluteHeadingControl<MyGridNode, MyState, MyControl> modAbsHead;
|
||||
|
||||
std::minstd_rand gen;
|
||||
|
||||
public:
|
||||
|
||||
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl) {
|
||||
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma), modAbsHead(&ctrl, Settings::IMU::absHeadSigma), modDestination(*grid), modActivity(&ctrl) {
|
||||
|
||||
|
||||
walker.addModule(&modRelHead);
|
||||
walker.addModule(&modAbsHead);
|
||||
|
||||
//walker.addModule(&modFavorZ);
|
||||
walker.addModule(&modHeading);
|
||||
//walker.addModule(&modImportance);
|
||||
//walker.addModule(&modActivity);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user