370 lines
13 KiB
C++
370 lines
13 KiB
C++
#ifndef FLOGIC_H
|
|
#define FLOGIC_H
|
|
|
|
#include <Indoor/grid/Grid.h>
|
|
|
|
#include <Indoor/grid/walk/v2/GridWalker.h>
|
|
#include <Indoor/grid/walk/v2/GridWalkerMulti.h>
|
|
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingVonMises.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
|
|
|
|
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
|
|
|
|
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
|
|
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
|
|
|
|
#include <Indoor/math/distribution/Uniform.h>
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
|
|
|
#include "Structs.h"
|
|
#include <omp.h>
|
|
#include "../Settings.h"
|
|
|
|
|
|
#include <random>
|
|
#include <iterator>
|
|
|
|
template <typename RandomGenerator = std::default_random_engine>
|
|
struct random_selector
|
|
{
|
|
//On most platforms, you probably want to use std::random_device("/dev/urandom")()
|
|
random_selector(RandomGenerator g = RandomGenerator(std::random_device()()))
|
|
: gen(g) {}
|
|
|
|
template <typename Iter>
|
|
Iter select(Iter start, Iter end) {
|
|
std::uniform_int_distribution<> dis(0, std::distance(start, end) - 1);
|
|
std::advance(start, dis(gen));
|
|
return start;
|
|
}
|
|
|
|
//convenience function
|
|
template <typename Iter>
|
|
Iter operator()(Iter start, Iter end) {
|
|
return select(start, end);
|
|
}
|
|
|
|
//convenience function that works on anything with a sensible begin() and end(), and returns with a ref to the value type
|
|
template <typename Container>
|
|
auto operator()(const Container& c) -> decltype(*begin(c))& {
|
|
return *select(begin(c), end(c));
|
|
}
|
|
|
|
private:
|
|
RandomGenerator gen;
|
|
};
|
|
|
|
|
|
/** particle-filter init randomly distributed within the building*/
|
|
struct PFInit : public K::ParticleFilterInitializer<MyState> {
|
|
|
|
Grid<MyNode>& grid;
|
|
int mode;
|
|
|
|
PFInit(Grid<MyNode>& grid, int mode) : grid(grid), mode(mode) {;}
|
|
|
|
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
|
for (K::Particle<MyState>& p : particles) {
|
|
|
|
int idx = rand() % grid.getNumNodes();
|
|
p.state.position = grid[idx]; // random position
|
|
p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading
|
|
p.state.heading.error = 0;
|
|
p.state.relativePressure = 0; // start with a relative pressure of 0
|
|
p.weight = 1.0 / particles.size(); // equal weight
|
|
|
|
//for debugging
|
|
p.state.curMode = mode;
|
|
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
/** particle-filter init with fixed position*/
|
|
struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
|
|
|
|
Grid<MyNode>& grid;
|
|
GridPoint startPos;
|
|
float headingDeg;
|
|
|
|
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
|
|
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
|
|
|
|
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
|
|
|
Distribution::Normal<float> norm(0.0f, 1.5f);
|
|
|
|
for (K::Particle<MyState>& p : particles) {
|
|
|
|
GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f);
|
|
|
|
GridPoint startPos = grid.getNodeFor(pos);
|
|
p.state.position = startPos; // scatter arround the start position
|
|
p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed heading
|
|
p.state.heading.error = 0;
|
|
p.state.relativePressure = 0; // start with a relative pressure of 0
|
|
p.weight = 1.0 / particles.size(); // equal weight
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
/** very simple transition model, just scatter normal distributed */
|
|
struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
|
|
|
|
Grid<MyNode>& grid;
|
|
|
|
// define the noise
|
|
Distribution::Normal<float> noise_cm = Distribution::Normal<float>(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
|
|
Distribution::Normal<float> height = Distribution::Normal<float>(0.0, 600.0);
|
|
|
|
// draw randomly from a vector
|
|
random_selector<> rand;
|
|
|
|
// draw from 0 - 1
|
|
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);
|
|
|
|
/** ctor */
|
|
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
|
|
|
|
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
|
|
|
int noNewPositionCounter = 0;
|
|
|
|
#pragma omp parallel for num_threads(6)
|
|
for (int i = 0; i < Settings::numParticles; ++i) {
|
|
K::Particle<MyState>& p = particles[i];
|
|
|
|
// // if neighboring node is a staircase, we have a 0.8 chance to walk them.
|
|
// GridPoint tmp = grid.getNodeFor(p.state.position);
|
|
// MyNode tmpNode(tmp);
|
|
// int numNeigbors = grid.getNumNeighbors(tmpNode);
|
|
|
|
// std::vector<MyNode> zNodes;
|
|
// for(int i = 0; i < numNeigbors; ++i){
|
|
|
|
// //if neighbor is stair (1) or elevator (2)
|
|
// MyNode curNode = grid.getNeighbor(tmpNode, i);
|
|
// if(curNode.getType() == 1 || curNode.getType() == 2){
|
|
// zNodes.push_back(curNode);
|
|
// }
|
|
// }
|
|
|
|
// float height = 0.0;
|
|
// if(!zNodes.empty()){
|
|
|
|
// if(uniRand.draw() > 0.3){
|
|
// //get a random height from all the neighbors on stairs or elevators
|
|
// height = rand(zNodes).z_cm - p.state.position.z_cm;
|
|
// }else{
|
|
// //do nothin
|
|
// }
|
|
|
|
// }
|
|
|
|
|
|
double diffHeight = p.state.position.z_cm + height.draw();
|
|
|
|
if()
|
|
|
|
|
|
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), height.draw());
|
|
GridPoint newPosition = p.state.position + noisePt;
|
|
|
|
p.state.position = grid.getNearestNode(newPosition);
|
|
|
|
// if(grid.hasNodeFor(newPosition)){
|
|
// p.state.position = newPosition;
|
|
// }else{
|
|
// //no new position!
|
|
// #pragma omp atomic
|
|
// noNewPositionCounter++;
|
|
// }
|
|
|
|
}
|
|
|
|
// std::cout << noNewPositionCounter << std::endl;
|
|
}
|
|
};
|
|
|
|
/** particle-filter transition */
|
|
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
|
|
|
|
Grid<MyNode>& grid;
|
|
|
|
GridWalker<MyNode, MyState> walker;
|
|
|
|
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
|
|
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
|
|
WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
|
|
WalkModuleNodeImportance<MyNode, MyState> modImportance;
|
|
WalkModuleSpread<MyNode, MyState> modSpread;
|
|
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
|
|
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
|
|
|
|
//WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
|
|
|
|
std::minstd_rand gen;
|
|
|
|
|
|
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
|
|
|
|
walker.addModule(&modHead);
|
|
//walker.addModule(&modHeadMises);
|
|
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
|
|
//walker.addModule(&modActivity);
|
|
//walker.addModule(&modHeadUgly);
|
|
//walker.addModule(&modImportance);
|
|
//walker.addModule(&modFavorZ);
|
|
//walker.addModule(&modButterAct);
|
|
//walker.addModule(&modWifi);
|
|
//walker.addModule(&modPreventVisited);
|
|
}
|
|
|
|
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
|
|
|
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
|
|
|
|
#pragma omp parallel for num_threads(6)
|
|
for (int i = 0; i < Settings::numParticles; ++i) {
|
|
K::Particle<MyState>& p = particles[i];
|
|
|
|
// save old position
|
|
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
|
|
|
// update steps
|
|
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
|
|
|
|
// update the particle in-place
|
|
p.state = walker.getDestination(grid, p.state, dist_m);
|
|
|
|
// update the baromter
|
|
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
|
|
p.state.relativePressure += deltaZ_cm * 0.105f;
|
|
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
|
|
struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
|
|
|
|
WiFiModelLogDistCeiling& wifiModel;
|
|
WiFiObserverFree wiFiProbability; // free-calculation
|
|
//WiFiObserverGrid<MyNode> wiFiProbability; // grid-calculation
|
|
|
|
BeaconModelLogDistCeiling& beaconModel;
|
|
BeaconObserverFree beaconProbability;
|
|
|
|
Grid<MyNode>& grid;
|
|
|
|
PFEval(WiFiModelLogDistCeiling& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid<MyNode>& grid) :
|
|
wifiModel(wifiModel),
|
|
beaconModel(beaconModel),
|
|
grid(grid),
|
|
wiFiProbability(Settings::WiFiModel::sigma, wifiModel),
|
|
beaconProbability(Settings::BeaconModel::sigma, beaconModel){
|
|
|
|
}
|
|
|
|
/** probability for WIFI */
|
|
inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {
|
|
|
|
//const MyNode& node = grid.getNodeFor(point);
|
|
return wiFiProbability.getProbability(point.inMeter() + Point3(0,0,1.3), observation.currentTime, vapWifi);
|
|
}
|
|
|
|
/** probability for BEACONS */
|
|
inline double getBEACON(const MyObs& observation, const GridPoint& point){
|
|
|
|
//consider adding the persons height
|
|
Point3 p = point.inMeter() + Point3(0,0,1.3);
|
|
return beaconProbability.getProbability(p, observation.currentTime, observation.beacons);
|
|
}
|
|
|
|
/** probability for Barometer */
|
|
inline double getBaroPressure(const MyObs& observation, const float hPa) const{
|
|
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
|
|
}
|
|
|
|
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
|
|
|
|
const float kappa = 0.75;
|
|
|
|
const MyNode& gn = grid.getNodeFor(p.state.position);
|
|
switch (act) {
|
|
|
|
case ActivityButterPressure::Activity::STAY:
|
|
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
|
|
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
|
|
{return 1-kappa;}
|
|
|
|
case ActivityButterPressure::Activity::UP:
|
|
case ActivityButterPressure::Activity::DOWN:
|
|
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
|
|
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
|
|
{return 1-kappa;}
|
|
|
|
}
|
|
|
|
return 1.0;
|
|
|
|
}
|
|
|
|
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
|
|
|
|
double sum = 0;
|
|
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
|
|
|
|
#pragma omp parallel for num_threads(6)
|
|
for (int i = 0; i < Settings::numParticles; ++i) {
|
|
K::Particle<MyState>& p = particles[i];
|
|
|
|
// Point3 pos_m = p.state.position.inMeter();
|
|
// Point3 posOld_m = p.state.positionOld.inMeter();
|
|
|
|
const double pWifi = getWIFI(observation, wifiObs, p.state.position);
|
|
//const double pBaroPressure = getStairProb(p, observation.activity);
|
|
//const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
|
|
//const double pBeacon = getBEACON(observation, p.state.position);
|
|
|
|
//small checks
|
|
_assertNotNAN(pWifi, "Wifi prob is nan");
|
|
//_assertNot0(pBaroPressure,"pBaroPressure is null");
|
|
|
|
const double prob = pWifi;
|
|
|
|
p.weight = prob;
|
|
|
|
#pragma omp atomic
|
|
sum += (prob);
|
|
|
|
}
|
|
|
|
if(sum == 0.0){
|
|
return 1.0;
|
|
}
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // FLOGIC_H
|