first commit init project
This commit is contained in:
toni
94
CMakeLists.txt
Executable file
94
CMakeLists.txt
Executable 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(Museum)
|
||||
|
||||
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})
|
||||
|
||||
352
Plotti.h
Normal file
352
Plotti.h
Normal file
@@ -0,0 +1,352 @@
|
||||
#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::GnuplotSplotElementPoints pNormal1;
|
||||
K::GnuplotSplotElementPoints pNormal2;
|
||||
K::GnuplotSplotElementColorPoints pDistributation1;
|
||||
K::GnuplotSplotElementColorPoints pDistributation2;
|
||||
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.getColor().setHexStr("#888888");
|
||||
splot.add(&pAPs); pAPs.setPointSize(0.7);
|
||||
splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
|
||||
splot.add(&pDistributation1); pDistributation1.setPointSize(0.6);
|
||||
splot.add(&pDistributation2); pDistributation2.setPointSize(0.6);
|
||||
splot.add(&pParticles); pParticles.getColor().setHexStr("#0000ff"); pParticles.setPointSize(0.4f);
|
||||
splot.add(&pNormal1); pNormal1.getColor().setHexStr("#ff00ff"); pNormal1.setPointSize(0.4f);
|
||||
splot.add(&pNormal2); pNormal2.getColor().setHexStr("#00aaff"); pNormal2.setPointSize(0.4f);
|
||||
splot.add(&pFloor);
|
||||
splot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999");
|
||||
splot.add(&pStairs); pStairs.getStroke().getColor().setHexStr("#000000");
|
||||
splot.add(&pInterest); pInterest.setPointSize(2); pInterest.getColor().setHexStr("#ff0000");
|
||||
splot.add(>Path); gtPath.getStroke().setWidth(2); gtPath.getStroke().getColor().setHexStr("#000000");
|
||||
splot.add(&estPath); estPath.getStroke().setWidth(2); estPath.getStroke().getColor().setHexStr("#00ff00");
|
||||
splot.add(&estPathSmoothed); estPathSmoothed.getStroke().setWidth(2); estPathSmoothed.getStroke().getColor().setHexStr("#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 << "," << std::round(pos.z * 10) / 10 << " to " << pos.x << "," << pos.y << "," << (std::round(pos.z * 10) / 10)+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 setTimeInMinute(const int minutes, const int seconds) {
|
||||
gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << seconds << "'\n";
|
||||
}
|
||||
|
||||
void addGroundTruthNode(const Point3 pos) {
|
||||
K::GnuplotPoint3 gp(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
gtPath.add(gp);
|
||||
}
|
||||
|
||||
// estimated path
|
||||
void addEstimationNode(const Point3 pos){
|
||||
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
estPath.add(est);
|
||||
}
|
||||
|
||||
// estimated path
|
||||
void addEstimationNodeSmoothed(const Point3 pos){
|
||||
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
||||
estPathSmoothed.add(est);
|
||||
}
|
||||
|
||||
void debugDistribution1(std::vector<K::Particle<MyState>> samples){
|
||||
|
||||
float min = +9999;
|
||||
float max = -9999;
|
||||
|
||||
pDistributation1.clear();
|
||||
for (int i = 0; i < samples.size(); ++i) {
|
||||
//if (i % 10 != 0) {continue;}
|
||||
|
||||
double prob = samples[i].weight;
|
||||
if (prob < min) {min = prob;}
|
||||
if (prob > max) {max = prob;}
|
||||
|
||||
K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
|
||||
pDistributation1.add(pos, prob);
|
||||
}
|
||||
|
||||
if (min == max) {min -= 1;}
|
||||
gp << "set cbrange [" << min << ":" << max << "]\n";
|
||||
}
|
||||
|
||||
void debugDistribution2(std::vector<K::Particle<MyState>> samples){
|
||||
|
||||
float min = +9999;
|
||||
float max = -9999;
|
||||
|
||||
pDistributation2.clear();
|
||||
for (int i = 0; i < samples.size(); ++i) {
|
||||
if (i % 25 != 0) {continue;}
|
||||
|
||||
double prob = samples[i].weight;
|
||||
if (prob < min) {min = prob;}
|
||||
if (prob > max) {max = prob;}
|
||||
|
||||
K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
|
||||
pDistributation2.add(pos, prob);
|
||||
}
|
||||
|
||||
if (min == max) {min -= 1;}
|
||||
gp << "set cbrange [" << min << ":" << max << "]\n";
|
||||
}
|
||||
|
||||
void drawNormalN1(Distribution::NormalDistributionN normParticle){
|
||||
|
||||
pNormal1.clear();
|
||||
for (int i = 0; i < 100000; ++i) {
|
||||
if (++i % 25 != 0) {continue;}
|
||||
Eigen::VectorXd vec = normParticle.draw();
|
||||
K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
|
||||
pNormal1.add(pos);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void drawNormalN2(Distribution::NormalDistributionN normParticle){
|
||||
|
||||
pNormal2.clear();
|
||||
for (int i = 0; i < 100000; ++i) {
|
||||
if (++i % 25 != 0) {continue;}
|
||||
Eigen::VectorXd vec = normParticle.draw();
|
||||
K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
|
||||
pNormal2.add(pos);
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
88
Settings.h
Normal file
88
Settings.h
Normal file
@@ -0,0 +1,88 @@
|
||||
#ifndef SETTINGS_H
|
||||
#define SETTINGS_H
|
||||
|
||||
#include <Indoor/grid/GridPoint.h>
|
||||
#include <Indoor/data/Timestamp.h>
|
||||
#include <Indoor/sensors/radio/VAPGrouper.h>
|
||||
|
||||
namespace Settings {
|
||||
|
||||
bool useKLB = false;
|
||||
|
||||
const int numParticles = 6000;
|
||||
const int numBSParticles = 50;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
namespace Smoothing {
|
||||
const bool activated = false;
|
||||
const double stepLength = 0.7;
|
||||
const double stepSigma = 0.2;
|
||||
const double headingSigma = 25.0;
|
||||
const double zChange = 0.0; // mu change in height between two time steps
|
||||
const double zSigma = 0.1;
|
||||
const int lag = 5;
|
||||
}
|
||||
|
||||
|
||||
//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 = -40;
|
||||
constexpr float EXP = 2.3;
|
||||
constexpr float WAF = 0.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::MAXIMUM, 1);
|
||||
const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, 1);
|
||||
}
|
||||
|
||||
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 = 1000;
|
||||
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
|
||||
}
|
||||
|
||||
namespace Path_DongleTest {
|
||||
const std::vector<int> path1 = {0, 1, 2, 3, 4, 5, 6};
|
||||
const std::vector<int> path2 = {6, 5, 4, 7, 8, 9, 8, 10};
|
||||
const std::vector<int> path3 = {10, 8, 7, 4, 11, 12, 13, 14, 6};
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // SETTINGS_H
|
||||
180
filter/KLB.h
Normal file
180
filter/KLB.h
Normal file
@@ -0,0 +1,180 @@
|
||||
#ifndef KLB_H
|
||||
#define KLB_H
|
||||
|
||||
#include <chrono>
|
||||
|
||||
#include <Indoor/math/divergence/KullbackLeibler.h>
|
||||
#include <Indoor/grid/factory/v2/GridFactory.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/Heading.h>
|
||||
#include <Indoor/geo/Point2.h>
|
||||
#include <Indoor/sensors/offline/FileReader.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/math/divergence/KullbackLeibler.h>
|
||||
#include <Indoor/math/divergence/JensenShannon.h>
|
||||
#include <Indoor/data/Timestamp.h>
|
||||
|
||||
#include <KLib/math/statistics/Statistics.h>
|
||||
|
||||
#include <KLib/math/filter/particles/Particle.h>
|
||||
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
|
||||
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
|
||||
#include <KLib/math/filter/particles/ParticleFilterHistory.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 <KLib/math/filter/particles/resampling/ParticleFilterResamplingDivergence.h>
|
||||
|
||||
#include <KLib/math/filter/merging/MarkovTransitionProbability.h>
|
||||
#include <KLib/math/filter/merging/mixing/MixingSamplerDivergency.h>
|
||||
#include <KLib/math/filter/merging/estimation/JointEstimationPosteriorOnly.h>
|
||||
|
||||
#include <KLib/math/filter/smoothing/BackwardSimulation.h>
|
||||
#include <KLib/math/filter/smoothing/CondensationBackwardFilter.h>
|
||||
#include <KLib/math/filter/smoothing/sampling/ParticleTrajectorieSampler.h>
|
||||
#include <KLib/math/filter/smoothing/sampling/CumulativeSampler.h>
|
||||
#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
|
||||
|
||||
#include "Structs.h"
|
||||
|
||||
#include "../Plotti.h"
|
||||
#include "Logic.h"
|
||||
#include "../Settings.h"
|
||||
|
||||
static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& samplesWifi){
|
||||
|
||||
Distribution::KernelDensity<double, MyState> parzen([&](MyState state){
|
||||
int size = particles.size();
|
||||
double prob = 0;
|
||||
|
||||
#pragma omp parallel for reduction(+:prob) num_threads(6)
|
||||
for(int i = 0; i < size; ++i){
|
||||
double distance = particles[i].state.position.getDistanceInCM(state.position);
|
||||
prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
|
||||
}
|
||||
|
||||
return prob;
|
||||
;});
|
||||
|
||||
std::vector<double> probsWifiV;
|
||||
std::vector<double> probsParticleV;
|
||||
|
||||
//just for plottingstuff
|
||||
std::vector<K::Particle<MyState>> samplesParticles;
|
||||
|
||||
const int step = 4;
|
||||
int i = 0;
|
||||
for(K::Particle<MyState> particle : samplesWifi){
|
||||
if(++i % step != 0){continue;}
|
||||
MyState state(GridPoint(particle.state.position.x_cm, particle.state.position.y_cm, particle.state.position.z_cm));
|
||||
|
||||
double probiParticle = parzen.getProbability(state);
|
||||
probsParticleV.push_back(probiParticle);
|
||||
|
||||
double probiwifi = particle.weight;
|
||||
probsWifiV.push_back(probiwifi);
|
||||
|
||||
//samplesParticles.push_back(K::Particle<MyState>(state, probiParticle));
|
||||
}
|
||||
|
||||
//make vectors
|
||||
Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
|
||||
Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
|
||||
|
||||
//get divergence
|
||||
double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
|
||||
//double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
|
||||
|
||||
//plotti
|
||||
//plot.debugDistribution1(samplesWifi);
|
||||
//plot.debugDistribution1(samplesParticles);
|
||||
|
||||
|
||||
//estimate the mean
|
||||
// K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
|
||||
// const MyState estWifi = estimateWifi.estimate(samplesWifi);
|
||||
// plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
|
||||
|
||||
return kld;
|
||||
}
|
||||
|
||||
|
||||
static double kldFromMultivariatNormal(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& particleWifi){
|
||||
//kld: particle die resampling hatten nehmen und nv daraus schätzen. vergleiche mit wi-fi
|
||||
//todo put this in depletionhelper.h
|
||||
|
||||
Point3 estPos = state.position.inMeter();
|
||||
|
||||
//this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and
|
||||
//no inverse matrix is possible
|
||||
std::mt19937_64 rng;
|
||||
// initialize the random number generator with time-dependent seed
|
||||
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
|
||||
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)};
|
||||
rng.seed(ss);
|
||||
// initialize a uniform distribution between -0.0001 and 0.0001
|
||||
std::uniform_real_distribution<double> unif(-0.0001, 0.0001);
|
||||
|
||||
//create a gauss dist for the current particle approx.
|
||||
Eigen::MatrixXd m(particles.size(), 3);
|
||||
for(int i = 0; i < particles.size(); ++i){
|
||||
m(i,0) = (particles[i].state.position.x_cm / 100.0) + unif(rng);
|
||||
m(i,1) = (particles[i].state.position.y_cm / 100.0) + unif(rng);
|
||||
m(i,2) = (particles[i].state.position.z_cm / 100.0) + unif(rng);
|
||||
}
|
||||
|
||||
Eigen::VectorXd mean(3);
|
||||
mean << estPos.x, estPos.y, estPos.z;
|
||||
|
||||
Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(m, mean);
|
||||
|
||||
//create a gauss dist for wifi
|
||||
Eigen::MatrixXd covWifi(3,3);
|
||||
covWifi << Settings::WiFiModel::sigma, 0, 0,
|
||||
0, Settings::WiFiModel::sigma, 0,
|
||||
0, 0, 0.01;
|
||||
|
||||
//estimate the mean
|
||||
K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
|
||||
const MyState estWifi = estimateWifi.estimate(particleWifi);
|
||||
|
||||
Eigen::VectorXd meanWifi(3);
|
||||
meanWifi << estWifi.position.x_cm / 100.0, estWifi.position.y_cm / 100.0, estWifi.position.z_cm / 100.0;
|
||||
Distribution::NormalDistributionN normWifi(meanWifi, covWifi);
|
||||
|
||||
//get the kld distance
|
||||
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
|
||||
|
||||
//plot.debugDistribution1(particleWifi);
|
||||
|
||||
//plot.drawNormalN1(normParticle);
|
||||
//plot.drawNormalN2(normWifi);
|
||||
|
||||
//plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
|
||||
|
||||
return kld;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif // KLB_H
|
||||
554
filter/Logic.h
Normal file
554
filter/Logic.h
Normal file
@@ -0,0 +1,554 @@
|
||||
#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/WiFiQualityAnalyzer.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/ParticleFilterMixing.h>
|
||||
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
||||
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
||||
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingKLD.h>
|
||||
|
||||
#include <KLib/math/filter/smoothing/BackwardFilterTransition.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
|
||||
p.weight = 1.0 / particles.size(); // equal weight
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** 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_m = Distribution::Normal<float>(0.0, 6.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 < particles.size(); ++i) {
|
||||
K::Particle<MyState>& p = particles[i];
|
||||
|
||||
// update the baromter
|
||||
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
|
||||
p.state.relativePressure += deltaZ_cm * 0.105f;
|
||||
|
||||
double diffHeight = p.state.position.inMeter().z + height_m.draw();
|
||||
double newHeight_cm = p.state.position.z_cm;
|
||||
if(diffHeight > 9.1){
|
||||
newHeight_cm = 10.8 * 100.0;
|
||||
} else if (diffHeight < 9.1 && diffHeight > 5.7){
|
||||
newHeight_cm = 7.4 * 100.0;
|
||||
} else if (diffHeight < 5.7 && diffHeight > 2.0) {
|
||||
newHeight_cm = 4.0 * 100.0;
|
||||
} else {
|
||||
newHeight_cm = 0.0;
|
||||
}
|
||||
|
||||
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), 0.0);
|
||||
GridPoint newPosition = p.state.position + noisePt;
|
||||
newPosition.z_cm = newHeight_cm;
|
||||
|
||||
// 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);
|
||||
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
//this is just for the smoothing transition... quick and dirty
|
||||
p.state.headingChangeMeasured_rad = control->turnSinceLastTransition_rad;
|
||||
|
||||
// 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;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* particle-filter transition
|
||||
* Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox
|
||||
*/
|
||||
struct PFTransKLDSampling : 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;
|
||||
|
||||
/** upper bound epsilon of the kld distance - the particle size is not allowed to exceed epsilon*/
|
||||
double epsilon;
|
||||
|
||||
/** the upper 1 - delta quantil of the normal distribution. something like 0.01 */
|
||||
double delta;
|
||||
|
||||
/** the bins */
|
||||
Binning<MyState> bins;
|
||||
|
||||
/** max particle size */
|
||||
uint32_t N_max;
|
||||
|
||||
|
||||
PFTransKLDSampling(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);
|
||||
|
||||
|
||||
epsilon = 0.15;
|
||||
delta = 0.01;
|
||||
N_max = 5000;
|
||||
bins.setBinSizes({0.01, 0.01, 0.2, 0.3});
|
||||
bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)});
|
||||
}
|
||||
|
||||
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
|
||||
Distribution::Uniform<int> getParticle(0, particles.size()-1);
|
||||
|
||||
//init stuff
|
||||
uint32_t n = 0;
|
||||
uint32_t k = 1;
|
||||
double N = 0;
|
||||
|
||||
//clear the bins
|
||||
bins.clearUsed();
|
||||
|
||||
//create new particle set
|
||||
std::vector<K::Particle<MyState>> particlesNew;
|
||||
|
||||
do{
|
||||
|
||||
//draw equally from the particle set
|
||||
int particleIdx = getParticle.draw();
|
||||
K::Particle<MyState>& p = particles[particleIdx];
|
||||
|
||||
//sample new particles based on the transition step
|
||||
// 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;
|
||||
|
||||
//if it falls into an empty bin then draw another particle
|
||||
//is bin free?
|
||||
if(bins.isFree(p.state)){
|
||||
k++;
|
||||
bins.markUsed(p.state);
|
||||
|
||||
//calculate the new N
|
||||
double z_delta = K::NormalDistributionCDF::getProbit(1 - delta);
|
||||
double front = (k - 1) / (2 * epsilon);
|
||||
double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta );
|
||||
|
||||
N = front * std::pow(back, 3.0);
|
||||
}
|
||||
++n;
|
||||
|
||||
//add particle to new particleset
|
||||
particlesNew.push_back(p);
|
||||
|
||||
|
||||
} while (n < N && n < N_max);
|
||||
|
||||
|
||||
//write new particleset
|
||||
particles.clear();
|
||||
particles.swap(particlesNew);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
struct BFTrans : public K::BackwardFilterTransition<MyState>{
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* ctor
|
||||
* @param choice the choice to use for randomly drawing nodes
|
||||
* @param fp the underlying floorplan
|
||||
*/
|
||||
BFTrans()
|
||||
{
|
||||
//nothin
|
||||
}
|
||||
|
||||
uint64_t ts = 0;
|
||||
uint64_t deltaMS = 0;
|
||||
|
||||
/** set the current time in millisconds */
|
||||
void setCurrentTime(const uint64_t ts) {
|
||||
if (this->ts == 0) {
|
||||
this->ts = ts;
|
||||
deltaMS = 0;
|
||||
} else {
|
||||
deltaMS = this->ts - ts;
|
||||
this->ts = ts;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* smoothing transition starting at T with t, t-1,...0
|
||||
* @param particles_new p_t (Forward Filter) p2
|
||||
* @param particles_old p_t+1 (Smoothed Particles from Step before) p1
|
||||
* q(p1 | p2) is calculated
|
||||
*/
|
||||
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
|
||||
std::vector<K::Particle<MyState>>const& particles_old ) override {
|
||||
|
||||
|
||||
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
|
||||
// this means, predict all possible states q_t+1 with all passible states q_t
|
||||
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
|
||||
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
|
||||
std::vector<std::vector<double>> predictionProbabilities;
|
||||
|
||||
omp_set_dynamic(0); // Explicitly disable dynamic teams
|
||||
omp_set_num_threads(7);
|
||||
#pragma omp parallel for shared(predictionProbabilities)
|
||||
for (int i = 0; i < particles_old.size(); ++i) {
|
||||
std::vector<double> innerVector;
|
||||
auto p1 = &particles_old[i];
|
||||
|
||||
for(int j = 0; j < particles_new.size(); ++j){
|
||||
|
||||
auto p2 = &particles_new[j];
|
||||
|
||||
const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0;
|
||||
|
||||
//TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental
|
||||
|
||||
const double distProb = K::NormalDistribution::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);
|
||||
|
||||
// TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement?
|
||||
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD());
|
||||
double diffDeg = Angle::radToDeg(diffRad);
|
||||
double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad);
|
||||
double measurementDeg = Angle::radToDeg(measurementRad);
|
||||
const double headingProb = K::NormalDistribution::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);
|
||||
|
||||
// does the angle between two particles positions is similiar to the measurement
|
||||
//double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y);
|
||||
|
||||
//check how near we are to the measurement
|
||||
double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0;
|
||||
const double floorProb = K::NormalDistribution::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
|
||||
|
||||
//combine the probabilities
|
||||
double prob = distProb;// * floorProb * headingProb;
|
||||
innerVector.push_back(prob);
|
||||
|
||||
}
|
||||
#pragma omp critical
|
||||
predictionProbabilities.push_back(innerVector);
|
||||
}
|
||||
|
||||
return predictionProbabilities;
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
|
||||
|
||||
WiFiModel& wifiModel;
|
||||
WiFiObserverFree wiFiProbability; // free-calculation
|
||||
//WiFiObserverGrid<MyNode> wiFiProbability; // grid-calculation
|
||||
WiFiQualityAnalyzer wqa;
|
||||
|
||||
BeaconModelLogDistCeiling& beaconModel;
|
||||
BeaconObserverFree beaconProbability;
|
||||
|
||||
Grid<MyNode>& grid;
|
||||
|
||||
PFEval(WiFiModel& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid<MyNode>& grid) :
|
||||
wifiModel(wifiModel),
|
||||
beaconModel(beaconModel),
|
||||
grid(grid),
|
||||
wiFiProbability(Settings::WiFiModel::sigma, wifiModel),
|
||||
beaconProbability(Settings::BeaconModel::sigma, beaconModel){
|
||||
|
||||
}
|
||||
|
||||
/** probability step-distance */
|
||||
//TODO: add number of recognized steps
|
||||
inline double getStepDistanceProb(const Point3 particle1, const Point3 particle2){
|
||||
double distance = particle1.getDistance(particle2);
|
||||
return Distribution::Normal<double>::getProbability(Settings::IMU::stepLength, Settings::IMU::stepSigma + 0.4, distance);
|
||||
}
|
||||
|
||||
//TODO: combinied evaluation heading and distance
|
||||
|
||||
/** 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));
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
wqa.add(wifiObs);
|
||||
float quality = wqa.getQuality();
|
||||
|
||||
#pragma omp parallel for num_threads(3)
|
||||
for (int i = 0; i < particles.size(); ++i) {
|
||||
K::Particle<MyState>& p = particles[i];
|
||||
|
||||
Point3 pos_m = p.state.position.inMeter();
|
||||
Point3 posOld_m = p.state.positionOld.inMeter();
|
||||
|
||||
double pWifi = getWIFI(observation, wifiObs, p.state.position);
|
||||
const double pBaroPressure = getStairProb(p, observation.activity);
|
||||
const double pStepDistance = getStepDistanceProb(pos_m, posOld_m);
|
||||
//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 bool volatile init = observation.currentTime.sec() < 25;
|
||||
//double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));
|
||||
//double pWiFiMod = (init) ? (std::pow(pWifi, 0.5)) : (std::pow(pWifi, 0.9));
|
||||
|
||||
// bad wifi? -> we have no idea where we are!
|
||||
if (quality < 0.25 && !init) {
|
||||
//pWifi = 1;
|
||||
//p.weight = std::pow(p.weight, 0.5);
|
||||
}
|
||||
|
||||
const double prob = pWifi * pStepDistance;// * pBaroPressure;
|
||||
|
||||
p.weight = prob;
|
||||
|
||||
#pragma omp atomic
|
||||
sum += (prob);
|
||||
|
||||
}
|
||||
|
||||
if(sum == 0.0){
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
return sum;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // FLOGIC_H
|
||||
132
filter/Structs.h
Normal file
132
filter/Structs.h
Normal file
@@ -0,0 +1,132 @@
|
||||
|
||||
#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/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;
|
||||
|
||||
/** 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;
|
||||
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 */
|
||||
ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
|
||||
|
||||
/** 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
|
||||
424
main.cpp
Executable file
424
main.cpp
Executable file
@@ -0,0 +1,424 @@
|
||||
#include <iostream>
|
||||
|
||||
#include "filter/Structs.h"
|
||||
#include "filter/KLB.h"
|
||||
#include "Plotti.h"
|
||||
#include "filter/Logic.h"
|
||||
#include "Settings.h"
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include <Indoor/sensors/radio/model/WiFiModelFactory.h>
|
||||
#include <Indoor/sensors/radio/model/WiFiModelFactoryImpl.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/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 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 SecondFloorOnly = {
|
||||
|
||||
mapDir + "SHL_Stock_2_01.xml",
|
||||
|
||||
{
|
||||
dataDir + "Path1_1.csv",
|
||||
dataDir + "Path2_1.csv",
|
||||
dataDir + "Path3_1.csv",
|
||||
},
|
||||
|
||||
mapDir + "wifi_fp_all.dat",
|
||||
40,
|
||||
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
|
||||
mapDir + "grid_Stock_2_01.dat"
|
||||
};
|
||||
|
||||
} data;
|
||||
|
||||
|
||||
Floorplan::IndoorMap* MyState::map;
|
||||
|
||||
K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
|
||||
|
||||
std::vector<double> kld_data;
|
||||
|
||||
// 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");
|
||||
|
||||
//Wi-Fi model new
|
||||
// WiFiModelFactory factory(map);
|
||||
// WiFiModel* wifimodel= factory.loadXML("/home/toni/Documents/programme/localization/data/wifi/model/eachOptParPos_multimodel.xml");
|
||||
// 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);
|
||||
|
||||
// add node-importance
|
||||
Importance::addImportance(grid);
|
||||
|
||||
grid.write(onp);
|
||||
} else {
|
||||
grid.read(inp);
|
||||
}
|
||||
|
||||
// stamp WiFi signal-strengths onto the grid
|
||||
WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround);
|
||||
|
||||
// reading file
|
||||
Offline::FileReader fr(setup.training[numFile]);
|
||||
|
||||
//interpolator for ground truth
|
||||
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
|
||||
|
||||
//gnuplot plot
|
||||
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;
|
||||
|
||||
//History of all estimated particles. Used for smoothing
|
||||
std::vector<std::vector<K::Particle<MyState>>> pfHistory;
|
||||
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)));
|
||||
|
||||
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
|
||||
//pf.setTransition(std::unique_ptr<PFTransKLDSampling>(new PFTransKLDSampling(grid, &ctrl)));
|
||||
//pf.setTransition(std::unique_ptr<PFTransSimple>(new PFTransSimple(grid)));
|
||||
pf.setEvaluation(std::unique_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
|
||||
|
||||
//resampling
|
||||
if(Settings::useKLB){
|
||||
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<MyState>()));
|
||||
} else {
|
||||
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.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
|
||||
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingKLD<MyState>>(new K::ParticleFilterResamplingKLD<MyState>()));
|
||||
}
|
||||
|
||||
pf.setNEffThreshold(0.85);
|
||||
|
||||
//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.5)));
|
||||
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
|
||||
|
||||
|
||||
/** Smoothing Init */
|
||||
K::BackwardSimulation<MyState> bf(Settings::numBSParticles);
|
||||
if(Settings::Smoothing::activated){
|
||||
|
||||
//create the backward smoothing filter
|
||||
bf.setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
|
||||
|
||||
bool smoothing_resample = false;
|
||||
//bf->setNEffThreshold(1.0);
|
||||
if(smoothing_resample)
|
||||
bf.setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
|
||||
bf.setTransition(std::unique_ptr<BFTrans>( new BFTrans) );
|
||||
|
||||
//Smoothing estimation
|
||||
bf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
|
||||
//bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
|
||||
//bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
|
||||
}
|
||||
|
||||
|
||||
|
||||
Timestamp lastTimestamp = Timestamp::fromMS(0);
|
||||
|
||||
StepDetection sd;
|
||||
TurnDetection td;
|
||||
MotionDetection md;
|
||||
ActivityButterPressure act;
|
||||
|
||||
RelativePressure relBaro;
|
||||
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
|
||||
|
||||
K::Statistics<float> errorStats;
|
||||
K::Statistics<float> errorStatsSmoothing;
|
||||
|
||||
//file writing for error data
|
||||
const long int t = static_cast<long int>(time(NULL));
|
||||
const std::string evalDir = errorDir + folder + std::to_string(t);
|
||||
if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){
|
||||
Assert::doThrow("Eval folder couldn't be created!");
|
||||
}
|
||||
|
||||
std::ofstream errorFile;
|
||||
errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
|
||||
|
||||
std::ofstream errorFileSmoothing;
|
||||
errorFileSmoothing.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_Smoothing.csv");
|
||||
|
||||
// parse each sensor-value within the offline data
|
||||
for (const Offline::Entry& e : fr.getEntries()) {
|
||||
|
||||
const Timestamp ts = Timestamp::fromMS(e.ts);
|
||||
|
||||
if (e.type == Offline::Sensor::WIFI) {
|
||||
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
|
||||
|
||||
} else if (e.type == Offline::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 == Offline::Sensor::ACC) {
|
||||
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
|
||||
++ctrl.numStepsSinceLastTransition;
|
||||
}
|
||||
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
|
||||
td.addAccelerometer(ts, _acc.data);
|
||||
|
||||
} else if (e.type == Offline::Sensor::GYRO) {
|
||||
const Offline::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 == Offline::Sensor::BARO) {
|
||||
relBaro.add(ts, fr.getBarometer()[e.idx].data);
|
||||
obs.relativePressure = relBaro.getPressureRealtiveToStart();
|
||||
obs.sigmaPressure = relBaro.getSigma();
|
||||
|
||||
//activity recognition
|
||||
obs.activity = act.add(ts, fr.getBarometer()[e.idx].data);
|
||||
//activity for transition
|
||||
|
||||
} else if (e.type == Offline::Sensor::LIN_ACC) {
|
||||
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
|
||||
|
||||
} else if (e.type == Offline::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) {
|
||||
|
||||
|
||||
/** filtering stuff */
|
||||
obs.currentTime = ts;
|
||||
MyState est = pf.update(&ctrl, obs);
|
||||
|
||||
Point3 estPos = est.position.inMeter();
|
||||
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms()));
|
||||
|
||||
/** plotting stuff */
|
||||
plot.pInterest.clear();
|
||||
|
||||
plot.setEst(estPos);
|
||||
plot.setGT(gtPos);
|
||||
plot.addEstimationNode(estPos);
|
||||
plot.addParticles(pf.getParticles());
|
||||
|
||||
/** error calculation stuff */
|
||||
float err_m = gtPos.getDistance(estPos);
|
||||
errorStats.add(err_m);
|
||||
errorFile << err_m << "\n";
|
||||
|
||||
|
||||
/** smoothing stuff */
|
||||
if(Settings::Smoothing::activated){
|
||||
//save the current estimation for later smoothing.
|
||||
pfHistory.push_back(pf.getNonResamplingParticles());
|
||||
tsHistory.push_back(ts.ms());
|
||||
|
||||
//backward filtering
|
||||
MyState estBF = est;
|
||||
if(pfHistory.size() > Settings::Smoothing::lag){
|
||||
|
||||
bf.reset();
|
||||
|
||||
//use every n-th (std = 1) particle set of the history within a given lag (std = 5)
|
||||
for(int i = 0; i <= Settings::Smoothing::lag; ++i){
|
||||
|
||||
((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
|
||||
estBF = bf.update(pfHistory[(pfHistory.size() - 1) - i]);
|
||||
}
|
||||
}
|
||||
|
||||
Point3 estPosSmoothing = estBF.position.inMeter();
|
||||
Point3 gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(tsHistory[(tsHistory.size() - 1) - Settings::Smoothing::lag]));
|
||||
|
||||
//plot
|
||||
plot.addEstimationNodeSmoothed(estPosSmoothing);
|
||||
|
||||
// error between GT and smoothing
|
||||
float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing);
|
||||
errorStatsSmoothing.add(errSmoothing_m);
|
||||
errorFileSmoothing << errSmoothing_m << "\n";
|
||||
}
|
||||
|
||||
//plot misc
|
||||
plot.setTimeInMinute(static_cast<int>(ts.sec()) / 60, static_cast<int>(static_cast<int>(ts.sec())%60));
|
||||
|
||||
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";
|
||||
|
||||
/** Draw everything */
|
||||
plot.show();
|
||||
usleep(10*10);
|
||||
|
||||
lastTimestamp = ts;
|
||||
|
||||
// reset control
|
||||
ctrl.resetAfterTransition();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
errorFile.close();
|
||||
|
||||
std::cout << "Statistical Analysis Filtering: " << std::endl;
|
||||
std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl;
|
||||
|
||||
std::cout << "Statistical Analysis Smoothing: " << std::endl;
|
||||
std::cout << "Median: " << errorStatsSmoothing.getMedian() << " Average: " << errorStatsSmoothing.getAvg() << " Std: " << errorStatsSmoothing.getStdDev() << std::endl;
|
||||
|
||||
//Write the current plotti buffer into file
|
||||
std::ofstream plotFile;
|
||||
plotFile.open(evalDir + "/plot_" + 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(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i);
|
||||
plot.show();
|
||||
usleep(10*10);
|
||||
}
|
||||
|
||||
plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90);
|
||||
plot.show();
|
||||
|
||||
plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0);
|
||||
plot.show();
|
||||
|
||||
plot.printOverview(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t));
|
||||
plot.show();
|
||||
|
||||
|
||||
/** Draw KLB */
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plotkld;
|
||||
K::GnuplotPlotElementLines lines;
|
||||
if(Settings::useKLB){
|
||||
|
||||
std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
|
||||
gp << "set terminal png size 1280,720\n";
|
||||
gp << "set output '" << path << "_shennendistance.png'\n";
|
||||
|
||||
for(int i=0; i < kld_data.size()-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 p1(i, kld_data[i]);
|
||||
K::GnuplotPoint2 p2(i+1, kld_data[i+1]);
|
||||
|
||||
lines.addSegment(p1, p2);
|
||||
}
|
||||
|
||||
plotkld.add(&lines);
|
||||
gp.draw(plotkld);
|
||||
gp.flush();
|
||||
}
|
||||
|
||||
std::cout << "finished" << std::endl;
|
||||
sleep(1);
|
||||
|
||||
return errorStats;
|
||||
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
K::Statistics<float> statsAVG;
|
||||
K::Statistics<float> statsMedian;
|
||||
K::Statistics<float> statsSTD;
|
||||
K::Statistics<float> statsQuantil;
|
||||
K::Statistics<float> tmp;
|
||||
|
||||
for(int i = 0; i < 1; ++i){
|
||||
|
||||
tmp = run(data.SecondFloorOnly, 2, "Wifi-Dongle-Test", Settings::Path_DongleTest::path3);
|
||||
statsMedian.add(tmp.getMedian());
|
||||
statsAVG.add(tmp.getAvg());
|
||||
statsSTD.add(tmp.getStdDev());
|
||||
statsQuantil.add(tmp.getQuantile(0.75));
|
||||
|
||||
std::cout << "Iteration " << i << " completed" << std::endl;;
|
||||
}
|
||||
|
||||
std::cout << "==========================================================" << std::endl;
|
||||
std::cout << "Average of all statistical data: " << std::endl;
|
||||
std::cout << "Median: " << statsMedian.getAvg() << std::endl;
|
||||
std::cout << "Average: " << statsAVG.getAvg() << std::endl;
|
||||
std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl;
|
||||
std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl;
|
||||
std::cout << "==========================================================" << std::endl;
|
||||
|
||||
//EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS
|
||||
std::ofstream finalStatisticFile;
|
||||
finalStatisticFile.open (errorDir + "/tmp.csv");
|
||||
|
||||
finalStatisticFile << "Average of all statistical data: \n";
|
||||
finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n";
|
||||
finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n";
|
||||
finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n";
|
||||
finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n";
|
||||
|
||||
finalStatisticFile.close();
|
||||
//EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user