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

added code

Browse Source
This commit is contained in:
toni
2017-03-10 14:25:48 +01:00
commit 48566b79fc
8 changed files with 3768 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
code/CMakeLists.txt Executable file
View File

@@ -0,0 +1,94 @@
# Usage:
# Create build folder, like RC-build next to RobotControl and WifiScan folder
# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl'
# make
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
# select build type
SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" )
PROJECT(IPIN2017)
IF(NOT CMAKE_BUILD_TYPE)
MESSAGE(STATUS "No build type selected. Default to Debug")
SET(CMAKE_BUILD_TYPE "Debug")
ENDIF()
INCLUDE_DIRECTORIES(
../
../../
../../../
../../../../
)
FILE(GLOB HEADERS
./notes.txt
./*.h
./*/*.h
./*/*/*.h
./*/*/*/*.h
./*/*/*/*/*.h
./*/*/*/*/*/*.h
)
FILE(GLOB SOURCES
./*.cpp
./*/*.cpp
./*/*/*.cpp
./*/*/*/*.cpp
../../Indoor/lib/tinyxml/tinyxml2.cpp
)
# system specific compiler flags
ADD_DEFINITIONS(
-std=gnu++11
-Wall
-Werror=return-type
-Wextra
-Wpedantic
-fstack-protector-all
-g3
#-O2
-march=native
-DWITH_TESTS
-DWITH_ASSERTIONS
-DWITH_DEBUG_LOG
)
# allow OMP
find_package(OpenMP)
if (OPENMP_FOUND)
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()
# build a binary file
ADD_EXECUTABLE(
${PROJECT_NAME}
${HEADERS}
${SOURCES}
)
# needed external libraries
TARGET_LINK_LIBRARIES(
${PROJECT_NAME}
gtest
pthread
)
SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER})

275
code/Plotti.h Normal file
View File

@@ -0,0 +1,275 @@
#ifndef PLOTTI_H
#define PLOTTI_H
#include "filter/Structs.h"
#include "Settings.h"
#include <functional>
#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
struct Plotti {
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementPoints pGrid;
K::GnuplotSplotElementLines pFloor;
K::GnuplotSplotElementLines pOutline;
K::GnuplotSplotElementLines pStairs;
K::GnuplotSplotElementPoints pAPs;
K::GnuplotSplotElementPoints pInterest;
K::GnuplotSplotElementPoints pParticles;
K::GnuplotSplotElementColorPoints pColorPoints;
K::GnuplotSplotElementLines gtPath;
K::GnuplotSplotElementLines estPath;
K::GnuplotSplotElementLines estPathSmoothed;
Plotti() {
gp << "set xrange[0-50:70+50]\nset yrange[0-50:50+50]\nset ticslevel 0\n";
splot.add(&pGrid); pGrid.setPointSize(0.25); pGrid.setColorHex("#888888");
splot.add(&pAPs); pAPs.setPointSize(0.7);
splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
splot.add(&pParticles); pParticles.setColorHex("#0000ff"); pParticles.setPointSize(0.4f);
splot.add(&pFloor);
splot.add(&pOutline); pOutline.setColorHex("#999999");
splot.add(&pStairs); pStairs.setColorHex("#000000");
splot.add(&pInterest); pInterest.setPointSize(2); pInterest.setColorHex("#ff0000");
splot.add(&gtPath); gtPath.setLineWidth(2); gtPath.setColorHex("#000000");
splot.add(&estPath); estPath.setLineWidth(2); estPath.setColorHex("#00ff00");
splot.add(&estPathSmoothed); estPathSmoothed.setLineWidth(2); estPathSmoothed.setColorHex("#0000ff");
}
void addLabel(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "'\n";
}
void addLabelV(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "' rotate by 90\n";
}
void showAngle(const int idx, const float rad, const Point2 cen, const std::string& str) {
Point2 rot(0, 1);
Point2 pos = cen + rot.rotated(rad) * 0.05;
gp << "set label "<<idx<<" at screen " << cen.x << "," << cen.y << " '" << str << "'"<< "\n";
gp << "set arrow "<<idx<<" from screen " << cen.x << "," << cen.y << " to screen " << pos.x << "," << pos.y << "\n";
}
void setEst(const Point3 pos) {
gp << "set arrow 991 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+1 << " nohead lw 1 front \n";
}
void setGT(const Point3 pos) {
gp << "set arrow 995 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+0.3 << " nohead lw 3 front \n";
gp << "set arrow 996 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x+0.3 << "," << pos.y << "," << pos.z << " nohead lw 3 front \n";
}
void addGroundTruthNode(const Point3 pos) {
K::GnuplotPoint3 gp(pos.x, pos.y, pos.z);
gtPath.add(gp);
}
// estimated path
void addEstimationNode(const Point3 pos){
K::GnuplotPoint3 est(pos.x, pos.y, pos.z);
estPath.add(est);
}
// estimated path
void addEstimationNodeSmoothed(const Point3 pos){
K::GnuplotPoint3 est(pos.x, pos.y, pos.z);
estPathSmoothed.add(est);
}
void debugWiFi(WiFiModelLogDistCeiling& model, const WiFiMeasurements& scan, const Timestamp curTS, const float z) {
WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model);
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(scan);
float min = +9999;
float max = -9999;
const float step = 2.0f;
for (float x = 0; x < 80; x += step) {
for (float y = 0; y < 55; y += step) {
Point3 pt(x,y,z);
double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), curTS, wifiObs);
if (prob < min) {min = prob;}
if (prob > max) {max = prob;}
pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
}
}
if (min == max) {min -= 1;}
gp << "set cbrange [" << min << ":" << max << "]\n";
}
template <typename Node> void debugProb(Grid<Node>& grid, std::function<double(const MyObs&, const Point3& pos)> func, const MyObs& obs) {
pColorPoints.clear();
// const float step = 2.0;
// float z = 0;
// for (float x = -20; x < 90; x += step) {
// for (float y = -10; y < 60; y += step) {
// const Point3 pos_m(x,y,z);
// const double prob = func(obs, pos_m);
// pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
// }
// }
std::minstd_rand gen;
std::uniform_int_distribution<int> dist(0, grid.getNumNodes()-1);
float min = +9999;
float max = -9999;
for (int i = 0; i < 10000; ++i) {
int idx = dist(gen);
Node& n = grid[idx];
const Point3 pos_cm(n.x_cm, n.y_cm, n.z_cm);
const Point3 pos_m = pos_cm / 100.0f;
const double prob = func(obs, pos_m);
if (prob < min) {min = prob;}
if (prob > max) {max = prob;}
pColorPoints.add(K::GnuplotPoint3(pos_m.x, pos_m.y, pos_m.z), prob);
}
if (min == max) {min -= 1;}
gp << "set cbrange [" << min << ":" << max << "]\n";
}
void addStairs(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::Stair* stair : f->stairs) {
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), f);
for (const Floorplan::Quad3& quad : quads) {
for (int i = 0; i < 4; ++i) {
int idx1 = i;
int idx2 = (i+1) % 4;
pStairs.addSegment(
K::GnuplotPoint3(quad[idx1].x,quad[idx1].y, quad[idx1].z),
K::GnuplotPoint3(quad[idx2].x,quad[idx2].y, quad[idx2].z)
);
}
}
}
}
}
void addFloors(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::FloorObstacle* obs : f->obstacles) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line) {
K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight);
K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight);
pFloor.addSegment(p1, p2);
}
}
}
}
void addOutline(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::FloorOutlinePolygon* poly : f->outline) {
const int cnt = poly->poly.points.size();
for (int i = 0; i < cnt; ++i) {
Point2 p1 = poly->poly.points[(i+0)];
Point2 p2 = poly->poly.points[(i+1)%cnt];
K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
pOutline.addSegment(gp1, gp2);
}
}
}
}
template <typename Node> void addGrid(Grid<Node>& grid) {
pGrid.clear();
for (const Node& n : grid) {
K::GnuplotPoint3 p(n.x_cm, n.y_cm, n.z_cm);
pGrid.add(p/100.0f);
}
}
template <typename State> void addParticles(const std::vector<K::Particle<State>>& particles) {
pParticles.clear();
int i = 0;
for (const K::Particle<State>& p : particles) {
if (++i % 25 != 0) {continue;}
K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
pParticles.add(pos / 100.0f);
}
}
void show() {
gp.draw(splot);
gp.flush();
}
void saveToFile(std::ofstream& stream){
gp.draw(splot);
stream << "set terminal x11 size 2000,1500\n";
stream << gp.getBuffer();
stream << "pause -1\n";
gp.flush();
}
void printSingleFloor(const std::string& path, const int floorNum) {
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_" << floorNum <<".png'\n";
gp << "set view 0,0\n";
gp << "set zrange [" << (floorNum * 4) - 2 << " : " << (floorNum * 4) + 2 << "]\n";
gp << "set autoscale xy\n";
}
void printSideView(const std::string& path, const int degree) {
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_deg" << degree <<".png'\n";
gp << "set view 90,"<< degree << "\n";
gp << "set autoscale xy\n";
gp << "set autoscale z\n";
}
void printOverview(const std::string& path) {
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_overview" << ".png'\n";
gp << "set view 75,60\n";
gp << "set autoscale xy\n";
gp << "set autoscale z\n";
}
};
#endif // PLOTTI_H

69
code/Settings.h Normal file
View File

@@ -0,0 +1,69 @@
#ifndef SETTINGS_H
#define SETTINGS_H
#include <Indoor/grid/GridPoint.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/VAPGrouper.h>
namespace Settings {
const int numParticles = 10000;
namespace IMU {
const float turnSigma = 2.5; // 3.5
const float stepLength = 1.00;
const float stepSigma = 0.15; //toni changed
}
const float smartphoneAboveGround = 1.3;
const float offlineSensorSpeedup = 2;
namespace Grid {
constexpr int gridSize_cm = 20;
}
//const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination
const GridPoint destination = GridPoint(0,0,0); // do not use destination
namespace SensorDebug {
const Timestamp updateEvery = Timestamp::fromMS(200);
}
namespace WiFiModel {
constexpr float sigma = 8.0;
/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
constexpr float TXP = -46;
constexpr float EXP = 2.7;
constexpr float WAF = -5.0;
// how to perform VAP grouping. see
// - calibration in Controller.cpp
// - eval in Filter.h
// NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without!
const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
}
namespace BeaconModel {
constexpr float sigma = 8.0;
constexpr float TXP = -71;
constexpr float EXP = 1.5;
constexpr float WAF = -20.0; //-5 //20??
}
namespace MapView3D {
const int maxColorPoints = 10000;
constexpr int fps = 15;
const Timestamp msPerFrame = Timestamp::fromMS(1000/fps);
}
namespace Filter {
const Timestamp updateEvery = Timestamp::fromMS(500);
constexpr bool useMainThread = false; // perform filtering in the main thread
}
}
#endif // SETTINGS_H

216
code/filter/Logic.h Normal file
View File

@@ -0,0 +1,216 @@
#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/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 <KLib/math/filter/particles/ParticleFilter.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"
/** particle-filter init randomly distributed within the building*/
struct PFInit : public K::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
PFInit(Grid<MyNode>& grid) : grid(grid) {;}
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
}
}
};
/** 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
}
}
};
/** 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;
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(&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);
for (K::Particle<MyState>& p : particles) {
// 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(node, 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));
}
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);
for (K::Particle<MyState>& p : particles) {
// 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 = 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 = pBaroPressure * pBeacon * pWifi;
p.weight = prob;
sum += (prob);
}
if(sum == 0.0){
return 1.0;
}
return sum;
}
};
#endif // FLOGIC_H

116
code/filter/Structs.h Normal file
View File

@@ -0,0 +1,116 @@
#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/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>
struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ {
static Floorplan::IndoorMap* map;
float relativePressure = 0.0f;
GridPoint positionOld;
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;
}
};
struct MyControl {
/** turn angle (in radians) since the last transition */
float turnSinceLastTransition_rad = 0;
/** number of steps since the last transition */
int numStepsSinceLastTransition = 0;
/** current motion delta angle*/
float motionDeltaAngle_rad = 0;
/** reset the control-data after each transition */
void resetAfterTransition() {
turnSinceLastTransition_rad = 0;
numStepsSinceLastTransition = 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;
/** 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

357
code/main.cpp Executable file
View File

@@ -0,0 +1,357 @@
#include <iostream>
#include "filter/Structs.h"
#include "Plotti.h"
#include <chrono>
#include "filter/Logic.h"
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <KLib/math/statistics/Statistics.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/FixedFrequencyInterpolator.h>
#include <Indoor/data/Timestamp.h>
#include "Settings.h"
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationKernelDensity.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/geo/Heading.h>
//frank
//const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
//const std::string dataDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/src/data/";
//toni
const std::string mapDir = "/home/toni/Documents/programme/localization/russenJournal/russen/map/";
const std::string dataDir = "/home/toni/Documents/programme/localization/russenJournal/russen/data/";
const std::string errorDir = dataDir + "results/";
/** describes one dataset (map, training, parameter-estimation, ...) */
struct DataSetup {
std::string map;
std::vector<std::string> training;
std::string wifiParams;
int minWifiOccurences;
VAPGrouper::Mode vapMode;
std::string grid;
};
/** all configured datasets */
struct Data {
DataSetup BERKWERK = {
mapDir + "SHL/SHL25.xml",
{
dataDir + "bergwerk/path1/nexus/vor/1454775984079.csv",
dataDir + "bergwerk/path1/galaxy/vor/1454776168794.csv",
dataDir + "bergwerk/path2/nexus/vor/1454779863041.csv",
dataDir + "bergwerk/path2/galaxy/vor/1454780113404.csv",
dataDir + "bergwerk/path3/nexus/vor/1454782562231.csv",
dataDir + "bergwerk/path3/galaxy/vor/1454782896548.csv",
dataDir + "bergwerk/path4/nexus/vor/1454776525797.csv",
dataDir + "bergwerk/path4/galaxy/vor/1454779020844.csv"
},
dataDir + "bergwerk/wifiParams.txt",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
mapDir + "SHL/grid25.dat"
};
DataSetup IPIN2015 = {
mapDir + "SHL/SHL_IPIN2015_gt.xml",
{
dataDir + "ipin2015/galaxy/Path0/1433581471902.csv",
dataDir + "ipin2015/nexus/Path0/1433606195078.csv",
dataDir + "ipin2015/galaxy/Path1/1433587749492.csv",
dataDir + "ipin2015/nexus/Path1/1433606670723.csv",
dataDir + "ipin2015/galaxy/Path2/1433581471902.csv",
dataDir + "ipin2015/nexus/Path2/1433607251262.csv",
dataDir + "eiszeit/path2/1479986737368.csv"
},
dataDir + "bergwerk/wifiParams.txt",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
mapDir + "SHL/grid_IPIN2015_gt.dat"
};
} data;
Floorplan::IndoorMap* MyState::map;
void run(DataSetup setup, int numFile, std::string folder) {
// load the floorplan
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
MyState::map = map;
WiFiModelLogDistCeiling WiFiModel(map);
WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
BeaconModelLogDistCeiling beaconModel(map);
beaconModel.loadBeaconsFromMap(map, Settings::BeaconModel::TXP, Settings::BeaconModel::EXP, Settings::BeaconModel::WAF);
Assert::isFalse(beaconModel.getAllBeacons().empty(), "no Beacons stored within the map.xml");
// build the grid
std::ifstream inp(setup.grid, std::ifstream::binary);
Grid<MyNode> grid(20);
// grid.dat empty? -> build one and save it
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
std::ofstream onp;
onp.open(setup.grid);
GridFactory<MyNode> factory(grid);
factory.build(map);
grid.write(onp);
} else {
grid.read(inp);
}
// add node-importance
Importance::addImportance(grid);
// stamp WiFi signal-strengths onto the grid
WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround);
// reading file
FileReader fr(setup.training[numFile]);
// doing ground truth stuff
std::vector<int> path_0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> path_1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> path_2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, path_1);
Plotti plot;
plot.addFloors(map);
plot.addOutline(map);
plot.addStairs(map);
plot.gp << "set autoscale xy\n";
//plot.addGrid(grid);
// init ctrl and observation
MyControl ctrl;
ctrl.resetAfterTransition();
MyObs obs;
//random start position
//std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid)); std::move(init);
//filter init
//std::unique_ptr<PFInit> init =
//K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(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, 1080.0f), 90.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
pf.setEvaluation(std::unique_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
//resampling
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.4)));
pf.setNEffThreshold(0.95);
//estimation
//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.95)));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
Timestamp lastTimestamp = Timestamp::fromMS(0);
StepDetection sd;
TurnDetection td;
MotionDetection md;
RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
K::Statistics<float> errorStats;
//file writing for error data
long int t = static_cast<long int>(time(NULL));
std::ofstream errorFile;
errorFile.open (errorDir + folder + "/error_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
// parse each sensor-value within the offline data
for (const FileReader::Entry& e : fr.getEntries()) {
const Timestamp ts = Timestamp::fromMS(e.ts);
if (e.type == FileReader::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == FileReader::Sensor::BEACON){
obs.beacons.entries.push_back(fr.getBeacons()[e.idx].data);
// remove to old beacon measurements
obs.beacons.removeOld(ts);
} else if (e.type == FileReader::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
++ctrl.numStepsSinceLastTransition;
}
const FileReader::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
td.addAccelerometer(ts, _acc.data);
} else if (e.type == FileReader::Sensor::GYRO) {
const FileReader::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
const float delta_gyro = td.addGyroscope(ts, _gyr.data);
ctrl.turnSinceLastTransition_rad += delta_gyro;
} else if (e.type == FileReader::Sensor::BARO) {
relBaro.add(ts, fr.getBarometer()[e.idx].data);
obs.relativePressure = relBaro.getPressureRealtiveToStart();
obs.sigmaPressure = relBaro.getSigma();
} else if (e.type == FileReader::Sensor::LIN_ACC) {
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
} else if (e.type == FileReader::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data);
Eigen::Vector2f curVec = md.getCurrentMotionAxis();
ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad();
}
if (ts.ms() - lastTimestamp.ms() > 500) {
obs.currentTime = ts;
MyState est = pf.update(&ctrl, obs);
Point3 estPos = est.position.inMeter();
//current ground truth position
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms()));
/** plotting stuff */
plot.pInterest.clear();
//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: ");
//plot.debugWiFi(eval->model, obs.wifis, obs.curTS);
//plot.debugProb(grid, std::bind(&PFEval::getGPS, eval, std::placeholders::_1, std::placeholders::_2), obs);
//plot.debugProb(grid, std::bind(&PFEval::getWIFI, eval, std::placeholders::_1, std::placeholders::_2), obs);
//plot.debugProb(grid, std::bind(&PFEval::getALL, eval, std::placeholders::_1, std::placeholders::_2), obs);
plot.setEst(estPos);
plot.setGT(gtPos);
plot.addEstimationNode(estPos);
plot.addParticles(pf.getParticles());
plot.pColorPoints.clear();
plot.debugWiFi(WiFiModel, obs.wifi, ts, 10.8f); //3 floor
plot.debugWiFi(WiFiModel, obs.wifi, ts, 7.4f); //2 floor
plot.debugWiFi(WiFiModel, obs.wifi, ts, 4.0f); //1 floor
plot.debugWiFi(WiFiModel, obs.wifi, ts, 0.0f); //0 floor
//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";
//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";
//plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << ctrl.barometer.act << "'\n";
// error between GT and estimation
float err_m = gtPos.getDistance(estPos);
errorStats.add(err_m);
errorFile << err_m << "\n";
plot.show();
usleep(10*10);
lastTimestamp = ts;
// reset control
ctrl.resetAfterTransition();
}
}
errorFile.close();
std::cout << "Statistical Analysis: " << std::endl;
std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << std::endl;
//Write the current plotti buffer into file
std::ofstream plotFile;
plotFile.open(errorDir + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
// for(int i = 0; i < map->floors.size(); ++i){
// plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i);
// plot.show();
// usleep(1000*10);
// }
// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90);
// plot.show();
// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0);
// plot.show();
// plot.printOverview("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t));
// plot.show();
sleep(1);
}
int main(int argc, char** argv) {
//Testing files
//run(data.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor
//for(int i = 0; i < 5; ++i){
run(data.IPIN2015, 3, "EVALIPIN2015"); // Nexus Path2
//}
}

1332
code/map/SHL/SHL25.xml Normal file
View File

File diff suppressed because it is too large Load Diff

1309
code/map/SHL/SHL_IPIN2015_gt.xml Normal file
View File

File diff suppressed because it is too large Load Diff