137 lines
3.7 KiB
C++
137 lines
3.7 KiB
C++
|
|
#ifndef FSTRUCTS_H
|
|
#define FSTRUCTS_H
|
|
|
|
#include <Indoor/grid/Grid.h>
|
|
#include <Indoor/sensors/radio/WiFiGridNode.h>
|
|
#include <Indoor/math/Distributions.h>
|
|
#include <Indoor/sensors/radio/WiFiMeasurements.h>
|
|
#include <Indoor/sensors/beacon/BeaconMeasurements.h>
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include <Indoor/floorplan/v2/FloorplanHelper.h>
|
|
#include <Indoor/grid/factory/v2/GridNodeImportance.h>
|
|
#include <Indoor/math/distribution/KernelDensity.h>
|
|
|
|
|
|
#include <Indoor/sensors/activity/Activity.h>
|
|
#include <Indoor/grid/walk/v2/GridWalker.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.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>
|
|
|
|
struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ {
|
|
|
|
static Floorplan::IndoorMap* map;
|
|
|
|
float relativePressure = 0.0f;
|
|
GridPoint positionOld;
|
|
|
|
float headingChangeMeasured_rad;
|
|
|
|
MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
|
|
|
|
MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
|
|
|
|
MyState& operator += (const MyState& o) {
|
|
this->position += o.position;
|
|
return *this;
|
|
}
|
|
MyState& operator /= (const double d) {
|
|
this->position /= d;
|
|
return *this;
|
|
}
|
|
MyState operator * (const double d) const {
|
|
return MyState(this->position*d);
|
|
}
|
|
bool belongsToRegion(const MyState& o) const {
|
|
return position.inMeter().getDistance(o.position.inMeter()) < 3.0;
|
|
}
|
|
|
|
float getBinValue(const int dim) const {
|
|
switch (dim) {
|
|
case 0: return this->position.x_cm / 100.0;
|
|
case 1: return this->position.y_cm / 100.0;
|
|
case 2: return this->position.z_cm / 100.0;
|
|
case 3: return this->heading.direction.getRAD();
|
|
}
|
|
throw "cant find this value within the bin";
|
|
}
|
|
};
|
|
|
|
struct MyControl {
|
|
|
|
/** turn angle (in radians) since the last transition */
|
|
float turnSinceLastTransition_rad = 0;
|
|
float sumTurn_rad = 0;
|
|
|
|
/** number of steps since the last transition */
|
|
int numStepsSinceLastTransition = 0;
|
|
|
|
/** current motion delta angle*/
|
|
float motionDeltaAngle_rad = 0;
|
|
float sumMotion_rad = 0;
|
|
|
|
/** reset the control-data after each transition */
|
|
void resetAfterTransition() {
|
|
turnSinceLastTransition_rad = 0;
|
|
numStepsSinceLastTransition = 0;
|
|
motionDeltaAngle_rad = 0;
|
|
}
|
|
|
|
};
|
|
|
|
struct MyObs {
|
|
|
|
/** relative pressure since t_0 */
|
|
float relativePressure = 0;
|
|
|
|
/** current estimated sigma for pressure sensor */
|
|
float sigmaPressure = 0.10f;
|
|
|
|
/** wifi measurements */
|
|
WiFiMeasurements wifi;
|
|
|
|
/** detected activity */
|
|
Activity activity = Activity::STANDING;
|
|
|
|
/** beacon measurements */
|
|
BeaconMeasurements beacons;
|
|
|
|
/** gps measurements */
|
|
//GPSData gps;
|
|
|
|
/** time of evaluation */
|
|
Timestamp currentTime;
|
|
|
|
};
|
|
|
|
struct MyNode : public GridPoint, public GridNode, public GridNodeImportance, public WiFiGridNode<20> {
|
|
|
|
float navImportance;
|
|
float getNavImportance() const { return navImportance; }
|
|
|
|
float walkImportance;
|
|
float getWalkImportance() const { return walkImportance; }
|
|
|
|
|
|
/** empty ctor */
|
|
MyNode() : GridPoint(-1, -1, -1) {;}
|
|
|
|
/** ctor */
|
|
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
|
|
|
|
static void staticDeserialize(std::istream& inp) {
|
|
WiFiGridNode::staticDeserialize(inp);
|
|
}
|
|
|
|
static void staticSerialize(std::ostream& out) {
|
|
WiFiGridNode::staticSerialize(out);
|
|
}
|
|
};
|
|
|
|
|
|
#endif // FSTRUCTS_H
|