toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added code from fusion2016

Browse Source
This commit is contained in:
Toni
2016-03-01 15:04:46 +01:00
commit 8d2be0f8a0
97 changed files with 19831 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
code/CMakeLists.txt Executable file
View File

@@ -0,0 +1,90 @@
# 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(Fusion2016)
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
./*.h
./*/*.h
./*/*/*.h
./*/*/*/*.h
./*/*/*/*/*.h
./*/*/*/*/*/*.h
)
FILE(GLOB SOURCES
./*.cpp
./*/*.cpp
./*/*/*.cpp
./*/*/*/*.cpp
../../KLib/inc/tinyxml/*.cpp
)
if(${CMAKE_GENERATOR} MATCHES "Visual Studio")
SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /D_X86_ /D_USE_MATH_DEFINES")
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox /D_X86_ /D_USE_MATH_DEFINES")
SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG")
SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO")
set(CMAKE_CONFIGURATION_TYPES Release Debug)
else()
# system specific compiler flags
ADD_DEFINITIONS(
-std=gnu++11
-Wall
-Werror=return-type
-Wextra
-Wpedantic
-fstack-protector-all
-g
-O0
-DWITH_TESTS
-DWITH_ASSERTIONS
)
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})

190
code/CMakeLists.txt.user Normal file
View File

@@ -0,0 +1,190 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 3.4.2, 2016-02-09T02:12:04. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{019cc681-80c8-4d51-8bae-40a1fa38def1}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
<value type="int">0</value>
</data>
<data>
<variable>ProjectExplorer.Project.EditorSettings</variable>
<valuemap type="QVariantMap">
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
</valuemap>
</valuemap>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
<value type="QString" key="language">QmlJS</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
</valuemap>
</valuemap>
<value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
<value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
<value type="int" key="EditorConfiguration.IndentSize">4</value>
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
<value type="int" key="EditorConfiguration.TabSize">8</value>
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.PluginSettings</variable>
<valuemap type="QVariantMap"/>
</data>
<data>
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{30c8d991-8fa7-4988-88f5-4b7190981f4a}</value>
<value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
<value type="bool" key="CMakeProjectManager.CMakeBuildConfiguration.UseNinja">false</value>
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/toni/Documents/programme/localization/Fusion2016/code-build</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
<valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets"/>
<value type="bool" key="CMakeProjectManager.MakeStep.Clean">false</value>
<value type="QString" key="CMakeProjectManager.MakeStep.MakeCommand"></value>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments">clean</value>
<valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets"/>
<value type="bool" key="CMakeProjectManager.MakeStep.Clean">true</value>
<value type="QString" key="CMakeProjectManager.MakeStep.MakeCommand"></value>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">all</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>
</valuemap>
<value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
<value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
<value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
<value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
<value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
<value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
<valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
<value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
<value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
<value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
<valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
<value type="int">0</value>
<value type="int">1</value>
<value type="int">2</value>
<value type="int">3</value>
<value type="int">4</value>
<value type="int">5</value>
<value type="int">6</value>
<value type="int">7</value>
<value type="int">8</value>
<value type="int">9</value>
<value type="int">10</value>
<value type="int">11</value>
<value type="int">12</value>
<value type="int">13</value>
<value type="int">14</value>
</valuelist>
<value type="QString" key="CMakeProjectManager.CMakeRunConfiguation.Title">Fusion2016</value>
<value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.Arguments"></value>
<value type="bool" key="CMakeProjectManager.CMakeRunConfiguration.UseTerminal">false</value>
<value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.UserWorkingDirectory"></value>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Fusion2016</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeRunConfiguration.Fusion2016</value>
<value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
<value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
<value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.TargetCount</variable>
<value type="int">1</value>
</data>
<data>
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
<value type="int">18</value>
</data>
<data>
<variable>Version</variable>
<value type="int">18</value>
</data>
</qtcreator>

51
code/DijkstraMapper.h Normal file
View File

@@ -0,0 +1,51 @@
#ifndef DIJKSTRAMAPPER_H
#define DIJKSTRAMAPPER_H
#include "MyGridNode.h"
/**
* allows BETTER dijkstra calculation on top of our data-structure
*/
class DijkstraMapper {
Grid<MyGridNode>& grid;
public:
DijkstraMapper(Grid<MyGridNode>& grid) : grid(grid) {;}
int getNumNeighbors(const MyGridNode& node) const {return node.getNumNeighbors();}
const MyGridNode* getNeighbor(const MyGridNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const MyGridNode& n1, const MyGridNode& n2) const {
float d = ((Point3)n1 - (Point3)n2).length(2) ;
//if (d > 20) {d*= 1.30;}
d /= std::pow(n2.imp, 3);
return d;
}
};
/**
* allows NORMAL dijkstra calculation on top of our data-structure
*/
class DijkstraMapperNormal {
Grid<MyGridNode>& grid;
public:
DijkstraMapperNormal(Grid<MyGridNode>& grid) : grid(grid) {;}
int getNumNeighbors(const MyGridNode& node) const {return node.getNumNeighbors();}
const MyGridNode* getNeighbor(const MyGridNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const MyGridNode& n1, const MyGridNode& n2) const {
return ((Point3)n1 - (Point3)n2).length();
}
};
#endif // DIJKSTRAMAPPER_H

167
code/Helper.h Normal file
View File

@@ -0,0 +1,167 @@
#ifndef HELPER_H
#define HELPER_H
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/grid/factory/GridImportance.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>
#include "Settings.h"
#include "MyGridNode.h"
#include "OldGroundTruth.h"
class Helper {
private:
public:
/** convert height (in cm) to floor-numbers */
static int getFloorNr(float z_cm) {
// if (z_cm < 360) {return 0;}
// if (z_cm < 360+340) {return 1;}
// if (z_cm < 360+340+340) {return 2;}
// return 3;
if (z_cm < 380) {return 0;}
if (z_cm < 380+340) {return 1;}
if (z_cm < 380+340+340) {return 2;}
return 3;
}
/** convert height (in cm) to floor-numbers */
static int getFloorNrFloat(const float z_cm) {
return z_cm / 350.0f;
}
static int getHeight(const int floorNr) {
switch(floorNr) {
case 0: return 0;
case 1: return 380;
case 2: return 380+340;
case 3: return 380+340+340;
default: throw "error";
}
}
/** align the given value onto the grid */
static int align(const int val) {
return val / MiscSettings::gridSize_cm * MiscSettings::gridSize_cm;
}
/** all floors within the building */
struct FHWSFloors {
Floor f0, f1, f2, f3;
Stairs s01, s12, s23;
const LengthF h0 = LengthF::cm(align(getHeight(0)));
const LengthF h1 = LengthF::cm(align(getHeight(1)));
const LengthF h2 = LengthF::cm(align(getHeight(2)));
const LengthF h3 = LengthF::cm(align(getHeight(3)));
// all ground-truth points
std::unordered_map<int, Point3> gtwp;
FHWSFloors() {;}
};
/** load the entire floorplan */
static FHWSFloors getFloors() {
FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222);
FHWSFloors f;
f.f0 = fpFac.getFloor("floor_0");
f.f1 = fpFac.getFloor("floor_1");
f.f2 = fpFac.getFloor("floor_2");
f.f3 = fpFac.getFloor("floor_3");
f.s01 = fpFac.getStairs("staircase_0_1");
f.s12 = fpFac.getStairs("staircase_1_2");
f.s23 = fpFac.getStairs("staircase_2_3");
OldGroundTruth gtwp0(MiscSettings::floorplan, "ground_truth_0", 2.822222);
OldGroundTruth gtwp05(MiscSettings::floorplan, "ground_truth_0_5", 2.822222);
OldGroundTruth gtwp1(MiscSettings::floorplan, "ground_truth_1", 2.822222);
OldGroundTruth gtwp15(MiscSettings::floorplan, "ground_truth_1_5", 2.822222);
OldGroundTruth gtwp2(MiscSettings::floorplan, "ground_truth_2", 2.822222);
OldGroundTruth gtwp25(MiscSettings::floorplan, "ground_truth_2_5", 2.822222);
OldGroundTruth gtwp3(MiscSettings::floorplan, "ground_truth_3", 2.822222);
for (auto it : gtwp0.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(0));}
for (auto it : gtwp1.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(1));}
for (auto it : gtwp2.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(2));}
for (auto it : gtwp3.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(3));}
for (auto it : gtwp05.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(0)+getHeight(1))/2);}
for (auto it : gtwp15.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(1)+getHeight(2))/2);}
for (auto it : gtwp25.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(2)+getHeight(3))/2);}
return f;
}
template <typename T> static void buildTheGrid(Grid<T>& grid, FHWSFloors floors) {
GridFactory<MyGridNode> gridFac(grid);
gridFac.addFloor(floors.f0, floors.h0.cm());
gridFac.addFloor(floors.f1, floors.h1.cm());
gridFac.addFloor(floors.f2, floors.h2.cm());
gridFac.addFloor(floors.f3, floors.h3.cm());
gridFac.addStairs(floors.s01, floors.h0.cm(), floors.h1.cm());
gridFac.addStairs(floors.s12, floors.h1.cm(), floors.h2.cm());
gridFac.addStairs(floors.s23, floors.h2.cm(), floors.h3.cm());
PlatformStair psUpperLeft;
psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
psUpperLeft.s1 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
psUpperLeft.s2 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
gridFac.buildPlatformStair(psUpperLeft, floors.h0.cm(), floors.h1.cm());
gridFac.buildPlatformStair(psUpperLeft, floors.h1.cm(), floors.h2.cm());
gridFac.buildPlatformStair(psUpperLeft, floors.h2.cm(), floors.h3.cm());
// vis.gp << "set xrange [1100:1800]\n";
// vis.gp << "set yrange [4500:5200]\n";
PlatformStair psUpperRight;
psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
gridFac.buildPlatformStair(psUpperRight, floors.h0.cm(), floors.h1.cm());
gridFac.buildPlatformStair(psUpperRight, floors.h1.cm(), floors.h2.cm());
gridFac.buildPlatformStair(psUpperRight, floors.h2.cm(), floors.h3.cm());
// vis.gp << "set xrange [6100:6900]\n";
// vis.gp << "set yrange [4500:5200]\n";
PlatformStair psLowerLeft;
psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
gridFac.buildPlatformStair(psLowerLeft, floors.h0.cm(), floors.h1.cm());
gridFac.buildPlatformStair(psLowerLeft, floors.h1.cm(), floors.h2.cm());
gridFac.buildPlatformStair(psLowerLeft, floors.h2.cm(), floors.h3.cm());
// vis.gp << "set xrange [1300:2100]\n";
// vis.gp << "set yrange [400:1400]\n";
// remove all isolated nodes not attached to 300,300,floor0
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor(GridPoint(300,300,floors.h0.cm())) );
// stamp importance information onto the grid-nodes
GridImportance gridImp;
gridImp.addImportance(grid, floors.h0.cm());
gridImp.addImportance(grid, floors.h1.cm());
gridImp.addImportance(grid, floors.h2.cm());
gridImp.addImportance(grid, floors.h3.cm());
}
};
#endif // HELPER_H

29
code/MyGridNode.h Normal file
View File

@@ -0,0 +1,29 @@
#ifndef MYGRIDNODE_H
#define MYGRIDNODE_H
#include <Indoor/grid/GridNode.h>
#include <Indoor/grid/GridPoint.h>
/**
* the nodes we add to our grid
*/
struct MyGridNode : public GridNode, public GridPoint {
/** distance to the desired target */
float distToTarget = 1.0;
/** node importance based on surroundings */
float imp = 1.0;
/** used for eval */
int cnt = 0;
public:
/** needed ctor */
MyGridNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
};
#endif // MYGRIDNODE_H

138
code/OldGroundTruth.h Normal file
View File

@@ -0,0 +1,138 @@
#ifndef OLDGROUNDTRUTH_H
#define OLDGROUNDTRUTH_H
#include <KLib/geo/Point.h>
#include <unordered_map>
#include <KLib/gfx/svg/SVGLoader.h>
#include <Indoor/geo/Point2.h>
/**
* TODO: REMOVE
*/
class OldGroundTruth {
/**
* helper class for SVG floorplans.
*
* converts between the SVG's scale and real-world scale
*/
class SVGScaler {
private:
/** the scaling factor to apply to the svg data */
double scalingFactor;
public:
/** ctor */
SVGScaler(const double scalingFactor) : scalingFactor(scalingFactor) {
;
}
/** scale (x, y) into (_x, _y) */
void scale(const double x, const double y, float& _x, float& _y) const {
_x = x * scalingFactor;
_y = y * scalingFactor;
}
/** scale the given point into a new output point */
Point2 scale(const K::Point p) const {
Point2 ret;
scale (p.x, p.y, ret.x, ret.y);
return ret;
}
/** scale the given line into a new output line */
Line2 scale(const K::Line l) const {
Line2 ret;
scale (l.p1.x, l.p1.y, ret.p1.x, ret.p1.y);
scale (l.p2.x, l.p2.y, ret.p2.x, ret.p2.y);
return ret;
}
};
private:
/** helper to scale the SVG into real-world-scale */
SVGScaler scaler;
/** all ground-truth waypoints within the floorplan */
std::unordered_map<int, Point2> points;
public:
OldGroundTruth() :scaler(0) {;}
/**
* ctor
* @param file the svg's filename
* @param layerName the name of the layer (within the SVG) to load
* @param scalingFactor the scaling to apply to convert between SVG and real-world scale
*/
OldGroundTruth(const std::string& file, const std::string& layerName, const double scalingFactor) : scaler(scalingFactor) {
K::SVGFile svg;
K::SVGLoader::load(K::File(file), &svg);
K::SVGComposite* sc = svg.getLayers();
K::SVGLayer* layer = sc->getContainedLayerNamed(layerName);
if (!layer) {throw "svg has no layer named '" + layerName + "'";}
load(layer);
}
/** get all waypoints */
const std::unordered_map<int, Point2>& getWaypoints() const {
return points;
}
private:
/** recursive loading/parsing of nested SVG elements */
void load(K::SVGElement* el) {
switch (el->getType()) {
case SVGElementType::COMPOSITE: {
for (K::SVGElement* sub : ((K::SVGComposite*)el)->getChilds()) {
load(sub);
}
break;
}
case SVGElementType::LAYER: {
K::SVGLayer* layer = (K::SVGLayer*) el;
for (K::SVGElement* sub : layer->getChilds()) {
load(sub);
}
break;
}
case SVGElementType::TEXT: {
const K::SVGText* text = (K::SVGText*) el;
if (text->getText().empty()) {break;}
const int id = std::stoi(text->getText());
points[id] = scaler.scale(K::Point(text->getPosition().x, text->getPosition().y));
break;
}
case SVGElementType::PATH: {
break;
}
default:
throw "should not happen!";
}
}
};
#endif // OLDGROUNDTRUTH_H

7
code/README.txt Normal file
View File

@@ -0,0 +1,7 @@
indoor Framework (https://git.frank-ebner.de/kazu/Indoor.git) must be relative to the "Fusion2016" folder:
/path/xyz/Fusion2016/code
/path/xyz/Indoor
KLib (https://github.com/k-a-z-u/KLib.git) must be relative to the "Fusion2016" folder:
/path/xyz/Fusion2016/code
/path/xyz/KLib

27
code/Settings.h Normal file
View File

@@ -0,0 +1,27 @@
#ifndef OTHER_SETTINGS_H
#define OTHER_SETTINGS_H
#define USE_STATIC_CIRCULAR_BUFFERING false
#define USE_BAROMETER_SMOOTHING_RC_LOWPASS false
#define USE_BAROMETER_SMOOTHING_HEAD_TAIL false
#define USE_BAROMETRIC_FORMULAR false
#include <string>
namespace MiscSettings {
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan_new.svg";
const std::string floorplanPlot = "/mnt/data/workspaces/Fusion2016/code/plan_plots.svg";
const int gridSize_cm = 20;
const int timeSteps = 500;
const int numParticles = 7500;
}
#endif // OTHER_SETTINGS_H

181
code/Vis.h Normal file
View File

@@ -0,0 +1,181 @@
#ifndef VIS_H
#define VIS_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 <Indoor/geo/Length.h>
#include <Indoor/floorplan/Floor.h>
#include <Indoor/geo/Angle.h>
#include <Indoor/grid/walk/GridWalkState.h>
#include "eval/GroundTruthWay.h"
class Vis {
public:
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementLines floors;
K::GnuplotSplotElementColorPoints gridNodes;
K::GnuplotSplotElementLines gridEdges;
K::GnuplotSplotElementPoints particles;
K::GnuplotSplotElementLines particleDir;
K::GnuplotSplotElementLines estPath;
K::GnuplotSplotElementLines groundTruth;
public:
Vis() {
gp << "set hidden3d front\n";
//gp << "set view equal xy\n";
gp << "set ticslevel 0\n";
gp << "set cbrange[0.8:2.0]\n";
gp << "unset xtics\n";
gp << "unset ytics\n";
gp << "unset ztics\n";
gp << "unset border\n";
groundTruth.setLineWidth(2);
groundTruth.setColorHex("#666666");
particles.setColorHex("#0000ff");
particles.setPointSize(0.3);
particleDir.setColorHex("#444444");
estPath.setLineWidth(2);
// attach all layers
splot.add(&floors);
splot.add(&gridNodes);
splot.add(&gridEdges);
splot.add(&particleDir);
splot.add(&particles);
splot.add(&groundTruth);
splot.add(&estPath);
}
/** add all obstacles of the given floor to the provided height */
Vis& addFloor(const Floor& f, const LengthF height) {
// add each wall
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
floors.addSegment(p1, p2);
}
return *this;
}
/** add the grid to the plot */
template <typename T> Vis& addGrid(Grid<T>& grid) {
std::set<uint64_t> used;
float max = 0;
for (const T& n1 : grid) {
if (n1.distToTarget > max) {max = n1.distToTarget;}
}
gp << "set cbrange[0.0:1.0]\n";
//gp << "set cbrange[0.8:1.3]\n";
for (const T& n1 : grid) {
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
//const float color = n1.imp;
//const float color = n1.distToTarget/max;
const float color = 0;
gridNodes.add(p1, color);
for (const T& n2 : grid.neighbors(n1)) {
const uint64_t idx = n1.getIdx() * n2.getIdx();
if (used.find(idx) == used.end()) {
const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
gridEdges.addSegment(p1, p2);
used.insert(idx);
}
}
}
return *this;
}
void addGroundTruth(GroundTruthWay& gtw) {
groundTruth.clear();
for (auto it : gtw.getWay()) {
K::GnuplotPoint3 gp(it.value.x, it.value.y, it.value.z);
groundTruth.add(gp);
}
}
void addEstPath(std::vector<Point3>& est) {
estPath.clear();;
for (const Point3& p : est) {
K::GnuplotPoint3 gp(p.x, p.y, p.z);
estPath.add(gp);
}
}
void setTimestamp(uint64_t ts) {
static uint64_t firstTs = ts;
gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n";
}
void removeGrid() {
gridNodes.clear();;
}
void clearStates() {
particles.clear();
particleDir.clear();
}
void addObject(const int idx, const Point3& p) {
gp << "set object " << idx << " polygon ";
gp << "from " << p.x << "," << p.y << "," << p.z;
gp << " to " << p.x << "," << p.y << "," << p.z + 200;
gp << " to " << p.x << "," << p.y << "," << p.z; // close
gp << " lw 2 ";
gp << "\n";
}
void setEstAndShould(const Point3& est, const Point3& should) {
addObject(2,est);
addObject(3,should);
}
template <typename T> void addState(const GridWalkState<T>& n) {
Point2 dir = Angle::getPointer(n.heading.getRAD());
K::GnuplotPoint3 p1(n.node->x_cm, n.node->y_cm, n.node->z_cm);
K::GnuplotPoint3 p2 = p1 + K::GnuplotPoint3(dir.x, dir.y, 0) * 85;
particles.add(p1);
particleDir.addSegment(p1, p2);
}
template <typename T> Vis& showStates(std::vector<GridWalkState<T>>& states) {
particles.clear();;
for (const GridWalkState<T>& n : states) {
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
}
return *this;
}
/** show (plot) the current setup */
void show() {
gp.draw(splot);
gp.flush();
}
};
#endif // VIS_H

62
code/eval/DebugShortestPath.h Normal file
View File

@@ -0,0 +1,62 @@
#ifndef DEBUGSHORTESTPATH_H
#define DEBUGSHORTESTPATH_H
#include <Indoor/grid/walk/GridWalkShortestPathControl.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 "../Helper.h"
#include "../Vis.h"
template <typename T> class DebugShortestPath : public GridWalkShortestPathControl<T> {
private:
Vis vis;
public:
/** ctor */
template <typename Access> DebugShortestPath(Grid<T>& grid, const Access& acc, const T& target, Helper::FHWSFloors& floors) : GridWalkShortestPathControl<T>(grid, acc, target) {
vis.particles.setColorHex("#0000ff");
vis.particles.setPointSize(1.5);
vis.addFloor(floors.f0, floors.h0);
vis.addFloor(floors.f1, floors.h1);
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
GridWalkState<T> s = GridWalkShortestPathControl<T>::getDestination(grid, start, distance_m, headChange_rad);
if (this->recalc == 0){
vis.estPath.clear();
vis.particles.clear();
vis.particles.add(K::GnuplotPoint3(this->centerOfMass.x, this->centerOfMass.y, this->centerOfMass.z));
for (int i = 0; i < (int)this->path->size()-1; ++i) {
const DijkstraNode<T>& dn1 = (*this->path)[i+0];
const DijkstraNode<T>& dn2 = (*this->path)[i+1];
K::GnuplotPoint3 p1 (dn1.element->x_cm, dn1.element->y_cm, dn1.element->z_cm);
K::GnuplotPoint3 p2 (dn2.element->x_cm, dn2.element->y_cm, dn2.element->z_cm);
vis.estPath.addSegment(p1, p2);
}
vis.show();
}
return s;
}
};
#endif // DEBUGSHORTESTPATH_H

73
code/eval/Eval.h Normal file
View File

@@ -0,0 +1,73 @@
#ifndef EVAL_H
#define EVAL_H
#include "EvalBase.h"
#include "../DijkstraMapper.h"
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.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>
class Eval : public EvalBase {
public:
// Eval() {
// pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
// MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
// MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
// //GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
// GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
// //GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
// //GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
// pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
// sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
// srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
// srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
// gtw = getGroundTruthWay(*sr, floors.gtwp, way2);
// }
// //wifi also uniform dist 0/1 fuer bereiche die OK sind?
// //steps hochzaehlen weil mehr als einer in einer transition??
// //increase regional average region
// void setEval1() {
// runName = "TODO";
// // the particle filter's evaluation method
// std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
// eval.get()->setUsage(true, false, false, true, true);
// pf->setEvaluation( std::move(eval) );
// // resampling step?
// pf->setNEffThreshold(1.0);
// pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
// // state estimation step
// //pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
// //pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
// pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.33f)));
// }
};
#endif // EVAL_H

264
code/eval/Eval1.h Normal file
View File

@@ -0,0 +1,264 @@
#ifndef EVAL1_H
#define EVAL1_H
#include "EvalBase.h"
#include "../DijkstraMapper.h"
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/grid/walk/GridWalkSimpleControl.h>
#include <Indoor/grid/walk/GridWalkPathControl.h>
#include <Indoor/grid/walk/GridWalkShortestPathControl.h>
#include "DebugShortestPath.h"
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.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>
class Eval1 : public EvalBase {
public:
Eval1() {
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
eval.get()->setUsage(true, true, true, true, true);
pf->setEvaluation( std::move(eval) );
// resampling step?
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
// state estimation step
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
// std::vector<int> wp = path2;// std::reverse(wp.begin(), wp.end());
// MyGridNode& start = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.front()]) );
// MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.back()]) );
// //GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
// //GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
// //GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
// //GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
// //GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
// GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
// pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
// // path1
//// sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward
//// srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt");
//// srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt");
//// sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward
//// srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt");
//// srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt");
// // path2
// sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward
// srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
// srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
// //sr = new SensorReader("./measurements/path2/2/1454346071347.csv"); // backward
// //srt = new SensorReaderTurn("./measurements/path2/2/Turns.txt");
// //srs = new SensorReaderStep("./measurements/path2/2/Steps2.txt");
// // path3
//// sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
//// srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
//// srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
//// sr = new SensorReader("./measurements/path3/2/1454345622819.csv"); // backward
//// srt = new SensorReaderTurn("./measurements/path3/2/Turns.txt");
//// srs = new SensorReaderStep("./measurements/path3/2/Steps2.txt");
// // path4
//// sr = new SensorReader("./measurements/path4/1454595382218.csv"); // forward
//// srt = new SensorReaderTurn("./measurements/path4/Turns.txt");
//// srs = new SensorReaderStep("./measurements/path4/Steps2.txt");
// gtw = getGroundTruthWay(*sr, floors.gtwp, wp);
}
void setEvalFails() {
}
/**
* starting with bad barometer readings. takes some time to move upwards
* will be fixed by using the path
*/
void path2_forward_simple() {
// forward
runName = "path2_forward_simple";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path2/1/1454345775306.csv");
srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path2);
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void path2_forward_path() {
// forward
runName = "path2_forward_path";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path2/1/1454345775306.csv");
srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path2);
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2.back()]) );
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
// TODO: plot grid-nodes for stairs for the paper (also look at z-transitions. some have NO x/y change even though they should have!)
void path3_forward_simple() {
// forward
runName = "path3_forward_simple";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path3);
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void path3_forward_path() {
// looks like the left stairs here are NOT working as expected?!
// plot them
// look at the probability-draw, maybe there is an issue there
// forward
runName = "path3_forward_path";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path3);
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3.back()]) );
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void path4_nexus_simple() {
runName = "path4_nexus_simple";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
// remove importance
for (auto& n : grid) {n.imp = 1;}
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void path4_nexus_imp() {
runName = "path4_nexus_importance";
BarometerEvaluation::barometerSigma = 0.05;
sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void path4_nexus_path() {
runName = "path4_nexus_path";
BarometerEvaluation::barometerSigma = 0.05;
sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void path4_nexus_path_b() {
runName = "path4_nexus_path";
BarometerEvaluation::barometerSigma = 0.05;
sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void bergwerk_path1_nexus_simple() {
runName = "bergwerk_path1_nexus_simple";
BarometerEvaluation::barometerSigma = 0.10;
sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward
srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt");
srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl);
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
};
#endif // EVAL1_H

308
code/eval/EvalBase.h Normal file
View File

@@ -0,0 +1,308 @@
#ifndef EVALBASE_H
#define EVALBASE_H
#include "../Settings.h"
#include "../Helper.h"
#include "../Vis.h"
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/statistics/Statistics.h>
#include "GroundTruthWay.h"
#include "../particles/MyState.h"
#include "../particles/MyObservation.h"
#include "../particles/MyEvaluation.h"
#include "../particles/MyTransition.h"
#include "../particles/MyInitializer.h"
#include "../reader/SensorReader.h"
#include "../reader/SensorReaderStep.h"
#include "../reader/SensorReaderTurn.h"
#include "../lukas/TurnObservation.h"
#include "../lukas/StepObservation.h"
#include "../toni/BarometerSensorReader.h"
#include "../frank/WiFiSensorReader.h"
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"
class EvalBase {
protected:
Grid<MyGridNode> grid;
Helper::FHWSFloors floors;
Vis vis;
K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
SensorReader* sr;
SensorReaderTurn* srt;
SensorReaderStep* srs;
std::string runName;
GroundTruthWay gtw;
// OLD
//std::vector<int> way0 = {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> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way2 = {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};
// NEW
std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path1dbl = {29, 29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path2dbl = {19, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint!
public:
EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) {
// build the grid
Helper::buildTheGrid(grid, floors);
// setup the visualisation
vis.addFloor(floors.f0, floors.h0);
vis.addFloor(floors.f1, floors.h1);
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
vis.floors.setColorHex("#666666");
vis.groundTruth.setCustomAttr("dashtype 3");
vis.groundTruth.setColorHex("#009900");
vis.gp << "unset cbrange\n";
}
static GridPoint conv(const Point3& p) {
return GridPoint(p.x, p.y, p.z);
}
GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
std::vector<Point3> path;
for (int id : ids) {
auto it = waypoints.find(id);
if(it == waypoints.end()) {throw "not found";}
path.push_back(it->second);
}
// new created the timed path
GroundTruthWay gtw;
int i = 0;
while (sr.hasNext()) {
const SensorEntry se = sr.getNext();
if (se.data.empty()) {continue;} // why necessary??
if (se.idx == 99) {
gtw.add(se.ts, path[i]);
++i;
}
}
// ensure the sensor-data contained usable timestamps for the ground-truth mapping
assert(i>0);
sr.rewind();
return gtw;
}
void run() {
// sensor numbers
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
//const int s_linearAcceleration = 2;
std::list<TurnObservation> turn_observations;
std::list<StepObservation> step_observations;
//Create an BarometerSensorReader
BarometerSensorReader baroSensorReader;
//Read all turn Observations
while(srt->hasNext()) {
SensorEntryTurn set = srt->getNext();
TurnObservation to;
to.ts = set.ts;
to.delta_heading = set.delta_heading;
to.delta_motion = set.delta_motion;
turn_observations.push_back(to);
}
//Step Observations
while(srs->hasNext()) {
SensorEntryStep ses = srs->getNext();
StepObservation so;
so.ts = ses.ts;
step_observations.push_back(so);
}
// the to-be-evaluated observation
MyObservation obs;
obs.step = new StepObservation(); obs.step->steps = 0;
obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0;
// control data
MyControl ctrl;
std::vector<Point3> pathEst;
uint64_t lastTransitionTS = 0;
int64_t start_time = -1;
K::Statistics<double> stats;
int cnt = 0;
// process each single sensor reading
while(sr->hasNext()) {
// get the next sensor reading from the CSV
const SensorEntry se = sr->getNext();
//start_time needed for time calculation of steps and turns
obs.latestSensorDataTS = se.ts;
if (start_time == -1) {start_time = se.ts;}
int64_t current_time = se.ts - start_time;
switch(se.idx) {
case s_wifi: {
obs.wifi = WiFiSensorReader::readWifi(se);
break;
}
case s_beacons: {
BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
if (!boe.mac.empty()) {
obs.beacons.entries.push_back(boe);
} // add the observed beacon
obs.beacons.removeOld(obs.latestSensorDataTS);
break;
}
case s_barometer: {
obs.barometer = baroSensorReader.readBarometer(se);
break;
}
// case s_linearAcceleration:{
// baroSensorReader.readVerticalAcceleration(se);
// break;
// }
case s_orientation: {
obs.orientation = OrientationSensorReader::read(se);
break;
}
}
// process all occurred turns
while (!step_observations.empty() && current_time > step_observations.front().ts) {
const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
obs.step->steps++;
ctrl.walked_m = obs.step->steps * 0.71;
}
// process all occurred steps
while (!turn_observations.empty() && current_time > turn_observations.front().ts) {
const TurnObservation _to = turn_observations.front(); turn_observations.pop_front();
obs.turn->delta_heading += _to.delta_heading;
obs.turn->delta_motion += _to.delta_motion;
ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);
}
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
lastTransitionTS = se.ts;
// timed updates
((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
const MyState est = pf->update(&ctrl, obs);
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const int offset = 750;
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
// skip the first 10 scans due to uniform distribution start
if (++cnt > 10) {
pathEst.push_back(curEst);
const float err = diff.length();
stats.add(err);
std::cout << stats.asString() << std::endl;
}
// plot
vis.clearStates();
for (int i = 0; i < (int) pf->getParticles().size(); i+=15) {
const K::Particle<MyState>& p = pf->getParticles()[i];
vis.addState(p.state.walkState);
}
vis.setTimestamp(se.ts);
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
// prevent gnuplot errors
usleep(1000*333);
}
}
sleep(1000);
}
};
#endif // EVALBASE_H

24
code/eval/GroundTruthWay.h Normal file
View File

@@ -0,0 +1,24 @@
#ifndef GROUNDTRUTHWAY_H
#define GROUNDTRUTHWAY_H
#include <Indoor/math/Interpolator.h>
#include <Indoor/geo/Point3.h>
/**
* interpolated ground-trouth based on timed check-points
*/
class GroundTruthWay : public Interpolator<uint64_t, Point3> {
public:
Point3 getPosAtTime(const uint64_t ts) const {
return get(ts);
}
/** get the ground truth way */
const std::vector<InterpolatorEntry>& getWay() const {return entries;}
};
#endif // GROUNDTRUTHWAY_H

134
code/eval/PaperPlot.h Normal file
View File

@@ -0,0 +1,134 @@
#ifndef PAPERPLOT_H
#define PAPERPLOT_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 <Indoor/floorplan/Floor.h>
#include <Indoor/geo/Length.h>
class PaperPlot {
public:
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementLines floors;
K::GnuplotSplotElementColorPoints nodes;
K::GnuplotSplotElementLines edges;
public:
PaperPlot() {
floors.setLineWidth(2);
plot.add(&edges);
plot.add(&nodes);
plot.add(&floors);
nodes.setPointSize(0.7);
edges.setColorHex("#555555");
gp << "set ticslevel 0\n";
//gp << "set zrange [0:0]\n";
}
void show() {
gp.draw(plot);
gp.flush();;
}
/** add all obstacles of the given floor to the provided height */
void addFloor(const Floor& f, const LengthF height) {
// add each wall
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
floors.addSegment(p1, p2);
}
}
/** add the grid to the plot */
template <typename T> void addGrid(Grid<T>& grid) {
// std::set<uint64_t> used;
// get the min/max value
float max = -999999;
float min = +999999;
for (const T& n1 : grid) {
const float val = n1.imp;
//const float val = n1.distToTarget;
if (val > max) {max = val;}
if (val < min) {min = val;}
}
gp << "set cbrange["<<min<<":"<<max<<"]\n";
for (const T& n1 : grid) {
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
const float color = n1.imp;
//const float color = n1.distToTarget/max;
//const float color = 0;
nodes.add(p1, color);
// for (const T& n2 : grid.neighbors(n1)) {
// const uint64_t idx = n1.getIdx() * n2.getIdx();
// if (used.find(idx) == used.end()) {
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
// gridEdges.addSegment(p1, p2);
// used.insert(idx);
// }
// }
}
}
/** show all nodes (and edges?) within the given region */
template <typename T> void debugGrid(Grid<T>& grid, const BBox3& bbox, const bool addNodes, const bool addEdges) {
std::set<uint64_t> used;
for (T& n1 : grid) {
if (bbox.contains(n1)) {
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
if (addNodes) {
nodes.add(p1, 0);
}
if (addEdges) {
for (const T& n2 : grid.neighbors(n1)) {
if (n1.z_cm == n2.z_cm) {continue;} // speedup
if (used.find(n2.getIdx()) == used.end()) {
const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
edges.addSegment(p1, p2);
}
}
used.insert(n1.getIdx());
// for (const T& n2 : grid.neighbors(n1)) {
// const uint64_t idx = n1.getIdx() * n2.getIdx();
// if (used.find(idx) == used.end()) {
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
// edges.addSegment(p1, p2);
// used.insert(idx);
// }
// }
}
}
}
}
};
#endif // PAPERPLOT_H

141
code/eval/PaperPlot2D.h Normal file
View File

@@ -0,0 +1,141 @@
#ifndef PAPERPLOT2D_H
#define PAPERPLOT2D_H
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
#include <Indoor/floorplan/Floor.h>
#include <Indoor/geo/Length.h>
class PaperPlot2D {
public:
struct Size {
float w;
float h;
Size(const float w, const float h) : w(w), h(h) {;}
};
public:
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines floors;
K::GnuplotPlotElementColorPoints nodes;
std::string file;
public:
PaperPlot2D(const std::string& file, Size s) : file(file) {
toFile(file, s);
setup();
}
PaperPlot2D() {
setup();
}
private:
void setup() {
floors.setLineWidth(1.5);
nodes.setPointType(7);
plot.add(&nodes);
plot.add(&floors);
gp << "unset border\n";
gp << "unset colorbox\n";
gp << "set tics scale 0,0\n"; // HACK! "unset tics\n" segfaults current gnuplot version...
gp << "set format x ' '\n";
gp << "set format y ' '\n";
gp << "set size ratio -1\n";
}
void toFile(const std::string& file, const Size s) {
gp << "set output '" << file << "'\n";
gp << "set terminal eps size " << s.w << "," << s.h << "\n";
}
public:
void setRanges(const float x1, const float x2, const float y1, const float y2) {
gp << "set xrange [" << x1 << ":" << x2 << "]\n";
gp << "set yrange [" << y1 << ":" << y2 << "]\n";
}
void show() {
gp.draw(plot);
if (file.length() != 0) {
std::string dataFile = file + ".dat";
std::ofstream os(dataFile.c_str());
os << gp.getBuffer();
os.close();
}
gp.flush();
}
/** add all obstacles of the given floor to the provided height */
void addFloor(const Floor& f) {
// add each wall
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint2 p1(l.p1.x, l.p1.y);
const K::GnuplotPoint2 p2(l.p2.x, l.p2.y);
floors.addSegment(p1, p2);
}
}
// void removeGrid() {
// gp << "unset object\n";
// }
/** add the grid to the plot */
template <typename T, typename Colorizer> void addGrid(Grid<T>& grid, const Colorizer col) {
// get the min/max value
float max = -999999;
float min = +999999;
for (const T& n1 : grid) {
const float val = col.get(n1);
//const float val = n1.distToTarget;
if (val > max) {max = val;}
if (val < min) {min = val;}
}
gp << "set cbrange["<<min<<":"<<max<<"]\n";
// for (const T& n1 : grid) {
// const K::GnuplotPoint2 p1(n1.x_cm, n1.y_cm);
// const float color = n1.imp;
// //const float color = n1.distToTarget/max;
// //const float color = 0;
// nodes.add(p1, color);
// }
int i = 0;
for (const T& n1 : grid) {
if (col.skip(n1)) {continue;}
gp << "set object " << (++i) << " rectangle center " << n1.x_cm << "," << n1.y_cm << " size 20,20 fs solid noborder fc palette cb " << col.get(n1) << "\n";
}
}
};
#endif // PAPERPLOT2D_H

4
code/eval/PaperVisDijkstra.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef PAPERVISDIJKSTRA_H
#define PAPERVISDIJKSTRA_H
#endif // PAPERVISDIJKSTRA_H

111
code/eval/PaperVisGrid.h Normal file
View File

@@ -0,0 +1,111 @@
#ifndef PAPERVISGRID_H
#define PAPERVISGRID_H
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/grid/factory/GridImportance.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/nav/dijkstra/Dijkstra.h>
#include <Indoor/nav/dijkstra/DijkstraPath.h>
#include "PaperPlot.h"
#include "PaperPlot2D.h"
#include "../MyGridNode.h"
#include "../Settings.h"
#include "../Helper.h"
class PaperVisGrid {
public:
static void showStairs() {
// the grid
Grid<MyGridNode> grid(20);
// floors
Helper::FHWSFloors floors = Helper::getFloors();
Helper::buildTheGrid(grid, floors);
// // load the floorplan
// FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222);
// Floor f0 = fpFac.getFloor("floor_0");
// Floor f0 = fpFac.getFloor("floor_1");
// const LengthF h0 = LengthF::cm(0);
// const LengthF h1 = LengthF::cm(400);
// const LengthF h2 = LengthF::cm(400+340);
// const LengthF h3 = LengthF::cm(400+340+340);
// add the floorplan to the grid
// GridFactory<MyGridNode> gridFac(grid);
// gridFac.addFloor(f0, h0.cm());
// gridFac.addFloor(f1, h1.cm());
// gridFac.addFloor(f2, h2.cm());
// gridFac.addFloor(f3, h3.cm());
// remove all isolated nodes not attached to 300,300,floor0
// gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );
PaperPlot plot;
// stairwell low left
{BBox3 bbox;
bbox.add(Point3(1300, 650,000));
bbox.add(Point3(2000,1300,1400));
plot.debugGrid(grid, bbox, true, true);}
// stairwell upper left
{BBox3 bbox;
bbox.add(Point3(1200,4758,000));
bbox.add(Point3(1800,5158,1400));
plot.debugGrid(grid, bbox, true, true);}
// stairwell upper right
{BBox3 bbox;
bbox.add(Point3(6240,4718,000));
bbox.add(Point3(6830,5158,1400));
plot.debugGrid(grid, bbox, true, true);}
// stair left
{BBox3 bbox;
bbox.add(Point3(1200,3200,000));
bbox.add(Point3(1440,4078,1400));
plot.debugGrid(grid, bbox, true, true);}
// stair center
{BBox3 bbox;
bbox.add(Point3(4200,4118,100));
bbox.add(Point3(6120,4438,1400));
plot.debugGrid(grid, bbox, true, true);}
//stair lower right
{BBox3 bbox;
bbox.add(Point3(7360,3358,000));
bbox.add(Point3(7880,4300,500));
plot.debugGrid(grid, bbox, true, true);}
plot.show();
sleep(1000);
}
};
#endif // PAPERVISGRID_H

190
code/eval/PaperVisImportance.h Normal file
View File

@@ -0,0 +1,190 @@
#ifndef PAPERVISIMPORTANCE_H
#define PAPERVISIMPORTANCE_H
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/grid/factory/GridImportance.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/nav/dijkstra/Dijkstra.h>
#include <Indoor/nav/dijkstra/DijkstraPath.h>
#include "PaperPlot.h"
#include "PaperPlot2D.h"
#include "../MyGridNode.h"
#include "../Settings.h"
#include "../DijkstraMapper.h"
PaperPlot2D::Size s1 = PaperPlot2D::Size(2,4);
class PaperVisImportance {
public:
static float clamp(const float in, const float min, const float max) {
if (in < min) {return min;}
if (in > max) {return max;}
return in;
}
// use node-importance as grid-color
struct ColorizerImp {
float get(const MyGridNode& n) const {return n.imp;}
bool skip(const MyGridNode& n) const {(void) n; return false;}
};
// use node-distance as grid-color
struct ColorizerDist {
float get(const MyGridNode& n) const {return n.distToTarget;}
bool skip(const MyGridNode& n) const {(void) n; return false;}
};
// use num-visited as grid-color
struct ColorizeHeat {
int maxCnt; int cutoff;
ColorizeHeat(const int maxCnt, const int cutoff) : maxCnt(maxCnt), cutoff(cutoff) {;}
float get(const MyGridNode& n) const {return (n.cnt > maxCnt) ? (maxCnt) : (n.cnt);}
bool skip(const MyGridNode& n) const {return n.cnt < cutoff;} // skip to reduce plot size
};
static void createImportance() {
// load the floorplan
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
Floor f0 = fpFac.getFloor("test1");
const LengthF h0 = LengthF::cm(0);
// add the floorplan to the grid
Grid<MyGridNode> grid(20);
GridFactory<MyGridNode> gridFac(grid);
gridFac.addFloor(f0, h0.cm());
// remove all isolated nodes not attached to 300,300,floor0
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(400,400,h0.cm()) ) );
// stamp importance information onto the grid-nodes
GridImportance gridImp;
gridImp.addImportance(grid, h0.cm());
{
PaperPlot2D plot("floorplan_importance.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.addFloor(f0);
plot.addGrid(grid, ColorizerImp());
plot.show();
}
}
static void createPath() {
// load the floorplan
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
Floor f0 = fpFac.getFloor("test1");
const LengthF h0 = LengthF::cm(0);
// add the floorplan to the grid
Grid<MyGridNode> grid(20);
GridFactory<MyGridNode> gridFac(grid);
gridFac.addFloor(f0, h0.cm());
// remove all isolated nodes not attached to 300,300,floor0
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );
// start and end
const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0));
const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0));
// build all shortest path to reach th target
Dijkstra<MyGridNode> dijkstra;
DijkstraMapper accImp(grid);
DijkstraMapperNormal accNormal(grid);
// path without importance
dijkstra.build(gnStart, gnStart, accNormal);
DijkstraPath<MyGridNode> pathNormal(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// stamp importance information onto the grid-nodes
GridImportance gridImp;
gridImp.addImportance(grid, h0.cm());
// path WITH importance
dijkstra.build(gnStart, gnStart, accImp);
DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// build plot
K::GnuplotPlotElementLines gpPath1; gpPath1.setLineWidth(2); gpPath1.setColorHex("#444444");
K::GnuplotPlotElementLines gpPath2; gpPath2.setLineWidth(2); gpPath2.setColorHex("#000000");
for (DijkstraNode<MyGridNode>* dn : pathNormal) {
gpPath1.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
}
for (DijkstraNode<MyGridNode>* dn : pathImp) {
gpPath2.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
}
// plot the 2 paths
{
PaperPlot2D plot("floorplan_paths.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.addFloor(f0);
plot.plot.add(&gpPath1); gpPath1.setCustomAttr("dashtype 3");
plot.plot.add(&gpPath2);
plot.show();
}
// stamp distance information onto the grid
// attach a corresponding weight-information to each user-grid-node
for (MyGridNode& node : grid) {
const DijkstraNode<MyGridNode>* dn = dijkstra.getNode(node);
node.distToTarget = dn->cumWeight;
}
// walk
GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk (grid, accImp, gnStart);
for (int i = 0; i < 30000; ++i) {
if (i % 250 == 0) {std::cout << i << std::endl;}
const MyGridNode& nStart = gnEnd;
GridWalkState<MyGridNode> sStart(&nStart, Heading::rnd());
//GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
}
// plot the heat-map
{
PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.gp << "set palette gray negative\n";
plot.addFloor(f0);
plot.addGrid(grid, ColorizeHeat(7000, 50));
plot.show();
}
}
};
#endif // PAPERVISIMPORTANCE_H

110
code/frank/BeaconEvaluation.h Executable file
View File

@@ -0,0 +1,110 @@
#ifndef BEACONEVALUATION_H
#define BEACONEVALUATION_H
#include <KLib/math/distribution/Normal.h>
#include "BeaconObservation.h"
#include "Settings.h"
#include "../particles/MyState.h"
#include "../particles/MyObservation.h"
#include "PositionedBeacon.h"
class BeaconEvaluation {
private:
Settings settings;
//BeaconObservation obs;
public:
double getProbability(const MyState& state, const MyObservation& observation) const {
//if (obs.entries.empty()) {return 1.0;}
double prob = 1.0;
// const double tx = -74;
const double waf = 8.0;
// // get the ap the client had the strongest measurement for
// const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
// const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
// const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
// const double mdlStrongestRSSI = distanceToRssi(tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);
// process each detected beacon
for (const BeaconObservationEntry& entry : observation.beacons.entries) {
// get the AP data from the settings
const PositionedBeacon* beacon = settings.getBeacon(entry.mac);
if (!beacon) {continue;}
// distance (in meter) between particle and AP
//const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
const double distToBeacon_m = state.pCur.getDistance(*beacon) / 100.0;
// floor difference?
//const double floorDist = std::abs(beacon->zNr - state.getFloorNr());
const float floorDist = std::ceil(std::abs(Helper::getFloorNrFloat(beacon->z) - Helper::getFloorNrFloat(state.pCur.z)));
// estimate the rssi depending on above distance
const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf);
// the measured rssi
const double realRSSI = entry.rssi;
// // the measured relative rssi
// const double realRelRSSI = strongest.rssi - realRSSI;
// const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
// probability? (sigma grows with measurement's age)
const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0;
const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI);
//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI);
//prob *= p;
prob += std::log(p);
}
const double lambda = 0.15;
const double res = lambda * exp(- lambda * (-prob));
return res;
//return prob;
}
// WiFiObservation filter(const WiFiObservation* obs) const {
// WiFiObservation out;
// out.ts = obs->ts;
// for (const WiFiObservationEntry& entry : obs->entries) {
// // alter the mac
// WiFiObservationEntry ne = entry;
// ne.mac[ne.mac.length()-1] = '0';
// if (settings.getAP(ne.mac)) {out.entries.push_back(ne);}
// }
// return out;
// }
// /** get the strongest AP within all measurements */
// WiFiObservationEntry getStrongest(const WiFiObservation* obs) const {
// WiFiObservationEntry max = obs->entries.front();
// for (const WiFiObservationEntry& entry : obs->entries) {
// if (entry.rssi > max.rssi) {max = entry;}
// }
// return max;
// }
static double rssiToDistance(double txPower, double rssi, double pathLoss) {
return pow(10, (txPower - rssi) / (10 * pathLoss));
}
static double distanceToRssi(double txPower, double distance, double pathLoss) {
if (distance <= 1) {return txPower;}
return (txPower - (10 * pathLoss * log10(distance)));
}
};
#endif // BEACONEVALUATION_H

41
code/frank/BeaconObservation.h Executable file
View File

@@ -0,0 +1,41 @@
#ifndef BEACONOBSERVATION_H
#define BEACONOBSERVATION_H
#include "MACAddress.h"
#include <vector>
/** one observed AP and its signal strength */
struct BeaconObservationEntry {
/** the timestamp this beacon was discovered at */
uint64_t ts;
/** the beacon's mac address */
std::string mac;
/** the beacon's rssi */
int rssi;
BeaconObservationEntry() : ts(0), mac(), rssi(0) {;}
BeaconObservationEntry(const uint64_t ts, const std::string& mac, const int rssi) : ts(ts), mac(mac), rssi(rssi) {;}
};
/** all APs observed during one scan */
struct BeaconObservation {
std::vector<BeaconObservationEntry> entries;
void removeOld(uint64_t latestTS) {
auto lambda = [latestTS] (const BeaconObservationEntry& e) {
uint64_t age = latestTS - e.ts;
return age > 1000*3;
};
entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end());
}
};
#endif // BEACONOBSERVATION_H

60
code/frank/BeaconSensorReader.h Executable file
View File

@@ -0,0 +1,60 @@
#ifndef BEACONSENSORREADER_H
#define BEACONSENSORREADER_H
#include "../reader/SensorReader.h"
#include "BeaconObservation.h"
#include "Settings.h"
#include <cassert>
class BeaconSensorReader {
public:
// /** get wifi observation data from one CSV entry */
// static BeaconObservation* readBeacons(const SensorEntry& se) {
// std::string tmp = se.data;
// BeaconObservation* obs = new BeaconObservation();
// obs->ts = se.ts;
// std::string mac = tmp.substr(0, 17);
// tmp = tmp.substr(17);
// assert(tmp[0] == ';'); tmp = tmp.substr(1);
// std::string rssi = tmp;
// BeaconObservationEntry e(mac, std::stoi(rssi));
// obs->entries.push_back(e);
// /** skip unknown beacons */
// if (settings.getBeacon(mac) == nullptr) {return nullptr;}
// return obs;
// }
/** get wifi observation data from one CSV entry */
static BeaconObservationEntry getBeacon(const SensorEntry& se) {
BeaconObservationEntry boe;
std::string tmp = se.data;
std::string mac = tmp.substr(0, 17);
tmp = tmp.substr(17);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
std::string rssi = tmp;
BeaconObservationEntry e(se.ts, mac, std::stoi(rssi));
/** skip unknown beacons */
if (settings.getBeacon(mac) == nullptr) {return BeaconObservationEntry();}
return e;
}
};
#endif // BEACONSENSORREADER_H

131
code/frank/MACAddress.h Executable file
View File

@@ -0,0 +1,131 @@
#ifndef MACADDRESS_H
#define MACADDRESS_H
#include <cstdint>
#include <string>
/**
* describe a MAC-Address as 64-bit integer
* or 8-bit access to all fields
*/
union MACAddressValue {
struct {
uint8_t h5;
uint8_t h4;
uint8_t h3;
uint8_t h2;
uint8_t h1;
uint8_t h0;
};
uint64_t mac;
/** initialize everything with zeros */
MACAddressValue() : mac(0) {;}
};
class MACAddress {
private:
/** the address as integer value */
MACAddressValue value;
public:
/** empty ctor */
MACAddress() {
;
}
/** copy ctor */
MACAddress(const MACAddress& o) : value(o.value) {
;
}
/** ctor form string (e.g. "xx:xx:xx:xx:xx:xx") */
MACAddress(const std::string& str) {
// sanity check
if (str.size() != 17) {throw "invalid hex string length. must be 17";}
value.mac = 0; // all zeros
value.h5 = hexWordToInt(str[ 0], str[ 1]);
value.h4 = hexWordToInt(str[ 3], str[ 4]);
value.h3 = hexWordToInt(str[ 6], str[ 7]);
value.h2 = hexWordToInt(str[ 9], str[10]);
value.h1 = hexWordToInt(str[12], str[13]);
value.h0 = hexWordToInt(str[15], str[16]);
}
/** convert to hex-string ("xx:xx:xx:xx:xx:xx") */
std::string asString() {
std::string str = ":::::::::::::::::";
intToHexStr(value.h5, &str[ 0]);
intToHexStr(value.h4, &str[ 3]);
intToHexStr(value.h3, &str[ 6]);
intToHexStr(value.h2, &str[ 9]);
intToHexStr(value.h1, &str[12]);
intToHexStr(value.h0, &str[15]);
return str;
}
/** get the mac address as a long-int value */
uint64_t asLong() const {
return value.mac;
}
/** equal? */
bool operator == (const MACAddress& o) const {
return o.asLong() == asLong();
}
private:
/** convert the given hex char [0-F] to an integer [0-15] */
static uint8_t hexCharToInt(char hex) {
// to upper case
if (hex >= 'a') {hex -= 'a' - 'A';}
// convert
return (hex - '0' < 10) ? (hex - '0') : (hex - 'A' + 10);
}
/** convert the given hex-word to an integer */
static uint8_t hexWordToInt(char hi, char lo) {
return hexCharToInt(hi) << 4 | hexCharToInt(lo);
}
/** conver the given integer [0-15] to a hex char [0-F] */
static char intToHexChar(const uint8_t val) {
return (val < 10) ? ('0' + val) : ('A' - 10 + val);
}
/** insert two hex chars into the provided string buffer */
static void intToHexStr(const uint8_t val, char* dst) {
dst[0] = intToHexChar((val >> 4) & 0xF);
dst[1] = intToHexChar((val >> 0) & 0xF);
}
};
/** hash-method for MAC-Addresses */
namespace std {
template <> struct hash<MACAddress> {
std::size_t operator() (const MACAddress& mac) const {
return std::hash<uint64_t>()(mac.asLong());
}
};
}
#endif // MACADDRESS_H

11
code/frank/OrientationObservation.h Normal file
View File

@@ -0,0 +1,11 @@
#ifndef ORIENTATIONOBSERVATION_H
#define ORIENTATIONOBSERVATION_H
/** android device orientation */
struct OrientationObservation {
float values[3];
};
#endif // ORIENTATIONOBSERVATION_H

42
code/frank/OrientationSensorReader.h Normal file
View File

@@ -0,0 +1,42 @@
#ifndef ORIENTATIONSENSORREADER_H
#define ORIENTATIONSENSORREADER_H
#include "../reader/SensorReader.h"
#include "OrientationObservation.h"
#include <cassert>
class OrientationSensorReader {
public:
/** get wifi observation data from one CSV entry */
static OrientationObservation read(const SensorEntry& se) {
std::string tmp = se.data;
OrientationObservation obs;
size_t pos1 = tmp.find(';');
size_t pos2 = tmp.find(';', pos1+1);
size_t pos3 = tmp.find(';', pos2+1);
assert(pos1 != std::string::npos);
assert(pos2 != std::string::npos);
assert(pos3 != std::string::npos);
const std::string s1 = tmp.substr(0, pos1);
const std::string s2 = tmp.substr(pos1+1, pos2-pos1-1);
const std::string s3 = tmp.substr(pos2+1, pos3-pos2-1);
obs.values[0] = std::stof(s1);
obs.values[1] = std::stof(s2);
obs.values[2] = std::stof(s3);
return obs;
}
};
#endif // ORIENTATIONSENSORREADER_H

36
code/frank/Position3D.h Executable file
View File

@@ -0,0 +1,36 @@
#ifndef POSITION3D_H
#define POSITION3D_H
#include <cmath>
/**
* represents a 3D position (x,y,z)
*/
struct Position3D {
/** x-position (in centimeter) */
double xCM;
/** y-position (in centimeter) */
double yCM;
/** floor number */
int zNr;
/** ctor */
Position3D() : xCM(0), yCM(0), zNr(0) {;}
/** ctor. x,y in centimeter, z = floor-number */
Position3D(const double xCM, const double yCM, const int zNr) : xCM(xCM), yCM(yCM), zNr(zNr) {;}
/** get the distance to the given position (in centimeter) */
double getDistanceCM(const Position3D& p) const {
const double dx = xCM - p.xCM;
const double dy = yCM - p.yCM;
const double dz = (zNr - p.zNr) * 300; // 300 = average floor height (centimeter)
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
};
#endif // POSITION3D_H

30
code/frank/PositionedBeacon.h Executable file
View File

@@ -0,0 +1,30 @@
#ifndef POSITIONEDBEACON_H
#define POSITIONEDBEACON_H
#include "WiFiAP.h"
//#include "Position3D.h"
#include <Indoor/geo/Point3.h>
class PositionedBeacon : public Point3 {
public:
MACAddress mac;
float tx;
float pl;
// /** ctor */
// PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) :
// mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) {
// ;
// }
/** ctor */
PositionedBeacon(const MACAddress& mac, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
Point3(x_cm, y_cm, z_cm), mac(mac), tx(tx), pl(pl) {
;
}
};
#endif // POSITIONEDBEACON_H

26
code/frank/PositionedWiFiAP.h Executable file
View File

@@ -0,0 +1,26 @@
#ifndef POSITIONEDWIFIAP_H
#define POSITIONEDWIFIAP_H
#include "WiFiAP.h"
//#include "Position3D.h"
#include <Indoor/geo/Point3.h>
class PositionedWifiAP : public WiFiAP, public Point3 {
public:
// /** ctor */
// PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) :
// WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) {
// ;
// }
/** ctor */
PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
WiFiAP(mac, ssid, tx, pl), Point3(x_cm, y_cm, z_cm) {
;
}
};
#endif // POSITIONEDWIFIAP_H

178
code/frank/Settings.h Executable file
View File

@@ -0,0 +1,178 @@
#ifndef SETTINGS_H
#define SETTINGS_H
#include "PositionedWiFiAP.h"
#include "PositionedBeacon.h"
#include "MACAddress.h"
#include <unordered_map>
#include "../Helper.h"
class Settings {
private:
std::unordered_map<MACAddress, PositionedWifiAP*> aps;
std::unordered_map<MACAddress, PositionedBeacon*> beacons;
public:
Settings() {
const double pl = 2.7; // 2.7
const double tx = -46;
const float ibPLE = 1.5;
addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl);
addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl);
addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl);
addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl);
addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, Helper::getHeight(3), tx, pl);
// 2nd floor (vague AP position)
addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, Helper::getHeight(2), tx, pl);
addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, Helper::getHeight(2), tx, pl);
addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, Helper::getHeight(2), tx, pl);
addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, Helper::getHeight(2), tx, pl);
addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl);
// 1st floor (vague AP position)
addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, Helper::getHeight(1), tx, pl);
addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, Helper::getHeight(1), tx, pl);
addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, Helper::getHeight(1), tx, pl);
addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, Helper::getHeight(1), tx, pl);
addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, Helper::getHeight(1), tx, pl);
addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, Helper::getHeight(1), tx, pl);
// 0th floor (exact AP position)
addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, Helper::getHeight(0), tx, pl);
addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, Helper::getHeight(0), tx, pl);
addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, Helper::getHeight(0), tx, pl);
addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, Helper::getHeight(0), tx, pl);
addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, Helper::getHeight(0), tx, pl);
addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl); // vague!!
addBeacon("48:EF:8D:77:66:DF", -81, ibPLE, 6984, 4526, Helper::getHeight(2));
addBeacon("6F:5F:39:0C:51:E4", -81, ibPLE, 7829, 3916, 200);
addBeacon("49:23:D8:7F:E8:D2", -81, ibPLE, 6946, 4536, Helper::getHeight(1));
// OLD
// const double pl = 2.7; // 2.7
// const double tx = -46;
// addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl-1);
// addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl);
// addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-1);
// addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl);
// addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, Helper::getHeight(3), tx, pl);
// // 2nd floor (vague AP position)
// addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, Helper::getHeight(2), tx, pl);
// addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, Helper::getHeight(2), tx, pl);
// addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, Helper::getHeight(2), tx, pl);
// addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, Helper::getHeight(2), tx, pl);
// addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl);
// // 1st floor (vague AP position)
// addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, Helper::getHeight(1), tx, pl);
// // 0th floor (exact AP position)
// addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl); // vague!!
// const int ibOff = +2;
// const float ibPLE = 1.9;
// addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(3)); // id:16
// addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, Helper::getHeight(2)); // id:18
// addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(1)); // id:11
// addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(0)); // id:20
// addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, Helper::getHeight(2)); // idis switchboard
// addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, Helper::getHeight(2)); // idis outside
// addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, Helper::getHeight(2)); // idis toni
// addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(1)); // i.1.47
// addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, Helper::getHeight(1)); // fachschaft
// addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, Helper::getHeight(1)); // neben fachschaft
//// addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3);
//// addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3);
// addBeacon("78:A5:04:1F:91:08", -88, ibPLE, 1448, 4538, 3);
// addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 2028, 4528, 3);
}
/** get the AP behind the given MAC (if any) */
const PositionedWifiAP* getAP(const MACAddress& mac) const {
auto it = aps.find(mac);
if (it == aps.end()) {return nullptr;}
return (it->second);
}
/** get the Beacon behind the given MAC (if any) */
const PositionedBeacon* getBeacon(const MACAddress& mac) const {
auto it = beacons.find(mac);
if (it == beacons.end()) {return nullptr;}
return (it->second);
}
private:
/** add a new known AP */
void addAP(const std::string& mac, const std::string& room, const double x_cm, const double y_cm, const int floor, const double tx, const double pl) {
std::string mac2 = mac;
//mac2[mac2.length()-1] = '9';
PositionedWifiAP* pap = new PositionedWifiAP(MACAddress(mac2), room, tx, pl, x_cm, y_cm, floor);
aps[mac2] = pap;
}
/** add a new known Beacon */
void addBeacon(const std::string& mac, const double tx, const double pl, const double x_cm, const double y_cm, const int floor) {
PositionedBeacon* pap = new PositionedBeacon(MACAddress(mac), tx, pl, x_cm, y_cm, floor);
beacons[mac] = pap;
}
// // access points
// PositionedWifiAP aps[] = {
//// // 3rd floor (excat AP position)
//// PositionedWifiAP(MACAddress("00:04:96:6b:64:90"), "i.3.20", 290, 1300, 3),
//// PositionedWifiAP(MACAddress("00:04:96:6b:70:c0"), "i.3.25", 290, 3930, 3),
//// PositionedWifiAP(MACAddress("00:04:96:6b:82:70"), "i.3.16", 1860, 3400, 3),
//// PositionedWifiAP(MACAddress("00:04:96:77:ed:f0"), "i.3.39", 4700, 4850, 3),
//// PositionedWifiAP(MACAddress("00:04:96:77:ed:60"), "i.3.3", 6460, 3400, 3),
//// // 2nd floor (vague AP position)
//// PositionedWifiAP(MACAddress("00:04:96:6c:3a:a9"), "I.2.1", 6300, 3600, 2),
//// PositionedWifiAP(MACAddress("00:04:96:6b:bf:89"), "I.2.8", 3300, 3500, 2),
//// PositionedWifiAP(MACAddress("00:04:96:77:ec:a9"), "I.2.15", 300, 1300, 2),
//// PositionedWifiAP(MACAddress("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, 2),
//// PositionedWifiAP(MACAddress("00:04:96:6b:db:69"), "I.2.34", 4400, 4800, 2),
//// // 1st floor (vague AP position)
//// PositionedWifiAP(MACAddress("00:04:96:6c:cf:19"), "I.1.2", 5700, 3500, 1),
//// PositionedWifiAP(MACAddress("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, 1),
//// PositionedWifiAP(MACAddress("00:04:96:69:48:89"), "I.1.17", 1500, 300, 1),
//// PositionedWifiAP(MACAddress("00:04:96:77:eb:99"), "I.1.21", 500, 1700, 1),
//// PositionedWifiAP(MACAddress("00:04:96:6b:45:59"), "I.1.30", 800, 4800, 1),
//// PositionedWifiAP(MACAddress("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1),
// };
};
extern Settings settings;
#endif // SETTINGS_H

45
code/frank/WiFiAP.h Executable file
View File

@@ -0,0 +1,45 @@
#ifndef WIFIAP_H
#define WIFIAP_H
#include "MACAddress.h"
#include <ostream>
/**
* represents a WiFi-AccessPoint
* an AP is represented by its MAC-Address and
* may provide a readably SSID
*/
class WiFiAP {
public:
/** the AP's MAC-Address */
const MACAddress mac;
/** the AP's readable SSID */
const std::string ssid;
/** AP tx-power */
const float tx;
/** path loss for this ap. for testing */
const float pl;
public:
/** ctor */
WiFiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl) : mac(mac), ssid(ssid), tx(tx), pl(pl) {
;
}
/** ctor */
WiFiAP(const std::string& mac, const std::string& ssid, const double tx, const double pl) : mac(mac), ssid(ssid), tx(tx), pl(pl) {
;
}
};
#endif // WIFIAP_H

128
code/frank/WiFiEvaluation.h Executable file
View File

@@ -0,0 +1,128 @@
#ifndef WIFIEVALUATION_H
#define WIFIEVALUATION_H
#include "../particles/MyState.h"
#include "WiFiObservation.h"
#include "PositionedWiFiAP.h"
#include "Settings.h"
#include "../particles/MyObservation.h"
#include <KLib/math/distribution/Normal.h>
class WiFiEvaluation {
private:
Settings settings;
WiFiObservation obs;
WiFiObservationEntry strongest;
public:
void nextObservation(const WiFiObservation& _obs) {
if (_obs.entries.empty()) {return;}
obs = filter(_obs);
strongest = getStrongest(&obs);
}
double getProbability(const MyState& state, const MyObservation& observation) const {
if (obs.entries.empty()) {return 1.0;}
double prob = 0;//1.0;
//const double tx = -48; // tablet
//const double pl = 3.15;
const float waf = 8;//10.0; // was 7 before?! has something todo with the floor heights / levels
// get the ap the client had the strongest measurement for
//const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
//const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
//const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
//const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);
// process each detected AP
for (const WiFiObservationEntry& entry : obs.entries) {
// get the AP data from the settings
const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap);
// distance (in meter) between particle and AP
const float distToAP_m = state.pCur.getDistance(*ap) / 100.0;
// floor difference?
const float floorDiff = std::abs(Helper::getFloorNrFloat(ap->z) - Helper::getFloorNrFloat(state.pCur.z));
//const float floorDiff = std::round(std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z)));
//const float floorDiff = std::abs(ap->z - state.pCur.z) / 340;
// estimate the rssi depending on above distance
const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf);
// the measured rssi
const double realRSSI = entry.rssi;
// the measured relative rssi
//const double realRelRSSI = strongest.rssi - realRSSI;
//const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
// probability? (sigma grows with measurement's age)
const double sigma = (8) + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.5;
const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI); // absolute
//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI); // relative
//prob *= p;
prob += std::log(p);
}
//const double lambda = 0.05;//0.15; //0.12;
//return lambda * exp(- lambda * (-prob));
return std::pow(std::exp(prob), 0.1);
// if (prob < -30) {return 0.01;}
// if (prob < -15) {return 0.50;}
// else {return 1.00;}
//return prob;
}
WiFiObservation filter(const WiFiObservation& obs) const {
WiFiObservation out;
for (const WiFiObservationEntry& entry : obs.entries) {
// alter the mac
WiFiObservationEntry ne = entry;
//ne.mac[ne.mac.length()-1] = '0'; // enabled = VAP grouping. also adjust settings to use ending "0"
if (settings.getAP(ne.mac)) {out.entries.push_back(ne);}
}
return out;
}
/** get the strongest AP within all measurements */
WiFiObservationEntry getStrongest(const WiFiObservation* obs) const {
WiFiObservationEntry max = obs->entries.front();
for (const WiFiObservationEntry& entry : obs->entries) {
if (entry.rssi > max.rssi) {max = entry;}
}
return max;
}
static double rssiToDistance(double txPower, double rssi, double pathLoss) {
return pow(10, (txPower - rssi) / (10 * pathLoss));
}
static double distanceToRssi(double txPower, double distance, double pathLoss) {
if (distance <= 1) {return txPower;}
return (txPower - (10 * pathLoss * log10(distance)));
}
};
#endif // WIFIEVALUATION_H

4
code/frank/WiFiHelper.h Executable file
View File

@@ -0,0 +1,4 @@
#ifndef WIFIHELPER_H
#define WIFIHELPER_H
#endif // WIFIHELPER_H

23
code/frank/WiFiObservation.h Executable file
View File

@@ -0,0 +1,23 @@
#ifndef WIFIOBSERVATION_H
#define WIFIOBSERVATION_H
#include "MACAddress.h"
#include <vector>
/** one observed AP and its signal strength */
struct WiFiObservationEntry {
uint64_t ts;
std::string mac;
int freq;
int rssi;
WiFiObservationEntry() {;}
WiFiObservationEntry(const uint64_t ts, const std::string& mac, const int freq, const int rssi) : ts(ts), mac(mac), freq(freq), rssi(rssi) {;}
};
/** all APs observed during one scan */
struct WiFiObservation {
std::vector<WiFiObservationEntry> entries;
};
#endif // WIFIOBSERVATION_H

46
code/frank/WiFiSensorReader.h Executable file
View File

@@ -0,0 +1,46 @@
#ifndef WIFISENSORREADER_H
#define WIFISENSORREADER_H
#include "../reader/SensorReader.h"
#include "WiFiObservation.h"
#include <cassert>
class WiFiSensorReader {
public:
/** get wifi observation data from one CSV entry */
static WiFiObservation readWifi(const SensorEntry& se) {
std::string tmp = se.data;
WiFiObservation obs;
// process all APs
while(!tmp.empty()) {
std::string mac = tmp.substr(0, 17);
tmp = tmp.substr(17);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
std::string freq = tmp.substr(0, 4);
tmp = tmp.substr(4);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
int pos = tmp.find(';');
std::string rssi = tmp.substr(0, pos);
tmp = tmp.substr(pos);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
WiFiObservationEntry e(se.ts, mac, std::stoi(freq), std::stoi(rssi));
obs.entries.push_back(e);
}
return obs;
}
};
#endif // WIFISENSORREADER_H

14
code/lukas/ReadMe.txt Executable file
View File

@@ -0,0 +1,14 @@
Python Skripte:
StepDetector.py TurnDetector.py
Benötigt wird Python2.7, scipy und numpy, sowie zum plotten matplotlib
Benötigte Parameter:
1. Input-Datei
2. Output-Datei
Beispiel:
python StepDetector.py ./FH_Sensor.csv Steps.txt
python TurnDetector.py ./FH_Sensor.csv Turns.txt
Weitere optimale Parameter mit -h aufrufbar

278
code/lukas/StepDetector.py Executable file
View File

@@ -0,0 +1,278 @@
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import argrelmax
import sys
import math
import argparse
def rotate_data_fhws(data, data_t, rotation, rotation_t):
#Invert rotationmatrix
np.linalg.inv(rotation)
#Align rotation time according to data time
tmp = []
for t in data_t:
# Find indices of roation matrix that are earlier
#than the current time of the sensor value
ind = np.where(rotation_t <= t)[0]
#Use the last index
if len(ind) != 0:
tmp.append(ind[-1])
else:
tmp.append(0)
#Only use the values of the rotation matrix that are aligned with the sensor data
rotation = rotation[tmp]
# Multiply data with rotationmatrix
rot_data = []
for i, row in enumerate(data):
row = np.append(row, 1)
rot_data.append(np.dot(rotation[i], row))
return np.array(rot_data)
def rotate_data_lukas(data, rotation):
#Invert rotationmatrix
np.linalg.inv(rotation)
rot_data = []
for i, row in enumerate(data):
row = np.append(row, 1)
rot_data.append(np.dot(rotation[i], row))
return np.array(rot_data)
def magnitude(x, y, z):
ret = [math.sqrt(i) for i in (x**2 + y**2 + z**2)]
mean = np.mean(ret)
ret -= mean
return ret
def count_steps(time, signal, lt, ht, dead):
"""
Find steps in the accelerometer signal.
After a step was found, a "dead" period exists, where no step can be found again.
This is to avoid too many steps
Parameters
----------
time: array_like
Timestaps of accelerometer signal
Must have same length as signal
signal: array_like
Accelerometer signal of all three axis.
Must have same length as time
lt: float
Low threshold, which must be exceeded by the accelerometer signal to be counted as step
ht: float
High treshold, which must not be exceeded by the accelerometer signal to be counted as step
dead: float
After a step was detected, during the dead time no other step will be found.
Given in milliseconds
"""
time_signal = zip(time, signal)
dead_time = 0
steps = []
for i in time_signal:
if lt < i[1] < ht and i[0] > dead_time:
steps.append(i[0])
dead_time = i[0] + dead
return np.array(steps)
def write_steps_to_file(fname, steps):
f = open(fname, 'w')
print steps
for s in steps:
f.write(str(s) + "\n")
f.close()
def plot_steps(time, signal, steps):
plt.title("Step detection")
plt.xlabel("ms")
plt.ylabel("Accelerometer magnitude")
plt.plot(time, signal, label="Accelerometer")
s = []
for i,t in enumerate(time):
if t in steps:
s.append((t, signal[i]))
s = np.array(s)
plt.plot(s[:,0], s[:,1], 'ro', label = "Steps")
plt.legend(numpoints=1)
plt.show()
def read_data(fname):
time = np.loadtxt(fname,
delimiter=";",
usecols=[0],
unpack=True)
f = open(fname, 'r')
accls = []
accls_t = []
rotations = []
rotations_t = []
start = time[0]
for line in f:
line = line.split(";")
t = int(line[0]) - start
#Lin Accel
if line[1] == "2":
accls_t.append(t)
accls.append((line[2], line[3], line[4]))
#Rotation
elif line[1] == "7":
rotations_t.append(t)
rotations.append((line[2], line[3], line[4], line[5],
line[6], line[7], line[8], line[9],
line[10], line[11], line[12],line[13],
line[14], line[15], line[16], line[17]))
accls = np.array(accls, dtype=float)
accls_t = np.array(accls_t, dtype=int)
rotations = np.array(rotations, dtype=float)
rotations = [row.reshape((4,4)) for row in rotations]
rotations = np.array(rotations)
rotations_t = np.array(rotations_t, dtype=int)
return accls, accls_t, rotations, rotations_t
def main():
parser = argparse.ArgumentParser()
parser.add_argument("fname_sensor",
help = "Accelerometer file")
parser.add_argument("fname_output",
help = "Output file, where timestamps of steps will be saved")
parser.add_argument("--dead",
help = "Time span (in ms) after a detected step in which no additional step will be detected (default=600)",
type=int)
parser.add_argument("--lt",
help = "Low threshold, which must be exceeded by the accelerometer signal to be counted as step (default=1.5)",
type=float)
parser.add_argument("--ht",
help = "High treshold, which must not be exceeded by the accelerometer signal to be counted as step(default=6.5)",
type=float)
parser.add_argument("--plot",
help = "Plot step detection",
action="store_true")
parser.add_argument("--file_format",
help = "Sensor data file format [fhws|lukas] (default: fhws)",
type = str)
args = parser.parse_args()
file_format = "fhws"
if args.file_format:
file_format = args.file_format
#My own data format
if file_format == "lukas":
delimiter = ','
time_cols = [40]
accel_cols = [6,7,8]
time = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=time_cols,
skiprows=2,
unpack=True)
accelX, accelY, accelZ = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=accel_cols,
skiprows=2,
unpack=True)
rotation = np.loadtxt(args.fname_sensor,
delimiter = delimiter,
usecols=range(18,34),
skiprows=1,
unpack=True)
rotations = rotation.T
rotations = [row.reshape((4,4)) for row in rotations]
accl = np.array([accelX, accelY, accelZ]).T
world_accl = rotate_data_lukas(accl, rotations)
#FHWS file format
else:
accls, time, rotation, rotation_t = read_data(args.fname_sensor)
world_accl = rotate_data_fhws(accls, time, rotation, rotation_t)
accelX = world_accl[:,0]
accelY = world_accl[:,1]
accelZ = world_accl[:,2]
accel_mag = magnitude(accelX, accelY, accelZ)
lt = 1.5
ht = 6.5
dead = 600
if args.dead:
dead = args.dead
if args.lt:
lt = args.lt
if args.ht:
ht = args.ht
steps = count_steps(time, accel_mag, lt, ht, dead)
print("#Steps detected: ", len(steps))
write_steps_to_file(args.fname_output, steps)
if args.plot:
plot_steps(time, accel_mag, steps)
if __name__ == "__main__":
main()

55
code/lukas/StepEvaluation.h Executable file
View File

@@ -0,0 +1,55 @@
#ifndef STEPEVALUATION_H
#define STEPEVALUATION_H
#include "../particles/MyState.h"
#include "StepObservation.h"
#include <math.h>
static constexpr double mu_walk = 90;
static constexpr double sigma_walk = 30;
static constexpr double mu_stop = 0;
static constexpr double sigma_stop = 5;
class StepEvaluation {
public:
double getProbability(const MyState& state, const StepObservation* obs) const {
(void) state;
(void) obs;
return 1;
// see: particle-filter-control-data
// const float mdlWalked_m = state.walkState.distanceWalked_m;
// ((MyState&)state).walkState.distanceWalked_m = 0;
// const float stepSize_m = 0.71;
// const float sensSigma_m = 0.05 + (0.05 * obs->steps);
// const float sensWalked_m = obs->steps * stepSize_m;
// if (obs->steps > 1) {
// int i = 0;
// int j = i+1; ++j;
// }
// const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m);
// if (prob != prob) {
// throw 1;
// }
// return prob;
}
};
#endif // STEPEVALUATION_H

17
code/lukas/StepObservation.h Executable file
View File

@@ -0,0 +1,17 @@
#ifndef STEPOBSERVATION_H
#define STEPOBSERVATION_H
struct StepObservation {
float ts;
int steps = 0;
StepObservation() {;}
StepObservation(const float ts) : ts(ts), steps(0) {;}
};
#endif // STEPOBSERVATION_H

25
code/lukas/StepReader.h Executable file
View File

@@ -0,0 +1,25 @@
#ifndef STEPREADER_H
#define STEPREADER_H
#endif // STEPREADER_H
#include "../SensorReaderStep.h"
class StepReader {
public:
static StepObservation* readStep(const SensorEntryStep& se) {
std::string tmp = se.data;
StepObservation* obs = new TurnObservation();
while(!tmp.empty()) {
std::string angle = tmp;
StepObservation t(std::stof(angle));
}
return obs;
}
};

445
code/lukas/TurnDetector.py Executable file
View File

@@ -0,0 +1,445 @@
import numpy as np
import sys
import scipy.integrate
import math
import argparse
from sklearn.decomposition import PCA
import scipy.signal as signal
def project(v1, v2):
"""
Project vector v1 on v2
Return projected vector
"""
p = [np.dot(a, g) / np.dot(g,g) for a,g in zip(v1, v2)]
p = np.array(p)
p = [p*g for p,g in zip(p, v2)]
p = np.array(p)
return p
def motion_axis(time, lin_accel, gravity, interval = 500):
"""
Returns the motion axis, which is the axis with the biggest variance
lin_accel -- Linear acceleration
gravity -- Gravity
Lin_accel and gravity should have equal length
"""
p = project(lin_accel, gravity)
#add time to vector p
p = np.array([time, p[:,0], p[:,1], p[:,2]]).T
start = 0
end = start + interval
end_time = p[:,0][-1] #last timestep
pca = PCA(n_components=1)
result = []
while start < end_time:
indices = np.where((p[:,0] >= start) & (p[:,0] < end))
Z = p[indices, 1:3][0]
Z[:,0] = signal.medfilt(Z[:,0],31)
Z[:,1] = signal.medfilt(Z[:,1],31)
pca.fit(Z)
x1 = pca.components_[0][0]
y1 = pca.components_[0][1]
result.append((end, x1, y1))
start += interval
end += interval
return np.array(result)
def angle_between(v1, v2):
l_a = np.linalg.norm(v1)
l_b = np.linalg.norm(v2)
cos_ab = np.dot(v1, v2 / (l_a * l_b))
angle = np.arccos(cos_ab) * 180/math.pi
return min([180 - angle, angle])
def rotate_data_fhws(data, data_t, rotation, rotation_t):
#Invert rotationmatrix
np.linalg.inv(rotation)
#Align rotation time according to data time
tmp = []
for t in data_t:
# Find indices of roation matrix that are earlier
#than the current time of the sensor value
ind = np.where(rotation_t <= t)[0]
#Use the last index
if len(ind) != 0:
tmp.append(ind[-1])
else:
tmp.append(0)
#Only use the values of the rotation matrix that are aligned with the sensor data
rotation = rotation[tmp]
# Multiply data with rotation matrix
rot_data = []
for i, row in enumerate(data):
row = np.append(row, 1)
rot_data.append(np.dot(rotation[i], row))
return np.array(rot_data)
def rotate_data_lukas(data, rotation):
#Invert rotationmatrix
np.linalg.inv(rotation)
rot_data = []
for i, row in enumerate(data):
row = np.append(row, 1)
rot_data.append(np.dot(rotation[i], row))
return np.array(rot_data)
def read_data(fname):
"""
Read the data out of the file provided by FHWS sensor reader app
"""
time = np.loadtxt(fname,
delimiter=";",
usecols=[0],
unpack=True)
f = open(fname, 'r')
lin_accel = []
gyros = []
rotations = []
gravity = []
start = time[0]
time = []
gyro_tmp = [0, 0, 0]
lin_accel_tmp = [0, 0, 0]
gravity_tmp = [0, 0, 9.81]
rotations_tmp = 16*[-1]
s = 0
for line in f:
line = line.split(";")
t = int(line[0]) - start
#Gyro-Data
if line[1] == "3":
gyro_tmp[0] = line[2]
gyro_tmp[1] = line[3]
gyro_tmp[2] = line[4]
#Linear Acceleration-Data
elif line[1] == "2":
lin_accel_tmp[0] = line[2]
lin_accel_tmp[1] = line[3]
lin_accel_tmp[2] = line[4]
#Gravity data
elif line[1] == "1":
gravity_tmp[0] = line[2]
gravity_tmp[1] = line[3]
gravity_tmp[2] = line[4]
#Rotation-Data
elif line[1] == "7":
for i in range(0,16):
rotations_tmp[i] = line[i+2]
if s != t:
gyros.append(gyro_tmp[:])
lin_accel.append(lin_accel_tmp[:])
gravity.append(gravity_tmp[:])
rotations.append(rotations_tmp[:])
time.append(t)
s = t
gyros = np.array(gyros, dtype=float)
lin_accel = np.array(lin_accel, dtype=float)
gravity = np.array(gravity, dtype=float)
rotations = np.array(rotations, dtype=float)
time = np.array(time, dtype = int)
#HACK
#In the first timestamps the rotation matrix is all zero, because
#no measurements are available yet.
#Avoid this by replacing these lines with the first measured
#rotation matrix
ind = np.where(rotations[:,0] == -1)[0]
if len(ind) != 0:
index = ind[-1] + 1
rotations[ind] = rotations[index]
#Reshape matrix
rotations = [row.reshape((4,4)) for row in rotations]
rotations = np.array(rotations)
return time, gyros, lin_accel, gravity, rotations
def detect_turns(time, signal, interval):
n_intervals = int(time[-1] / interval) + 1
result = []
for i in range(n_intervals):
start = i * interval
end = start + interval
tmp = integrate(start, end, zip(time, signal)) * 180.0/math.pi
result.append((end, tmp))
return np.array(result)
def integrate(time_from, time_to, signal):
"""Integrate signal from time_from to time_to. Signal should be two dimensional.
First dimension is the timestamp, second dimension is the signal value.
dt is the interval between two recordings
"""
sum = 0
last_time = 0
#go through signal
for value in signal:
#check if time is in the given timespan
if time_from <= value[0] < time_to:
#multiply value with dt and add it to the sum = integral
# sum += value[1] * dt
sum += value[1] * ((value[0] - last_time)/1000.)
last_time = value[0]
#sum is the integral over rad/s
return sum
def write_to_file(fname, turns, motion):
f = open(fname, 'w')
for index, t in enumerate(turns):
f.write(str(t[0]) + "," + str(t[1]) + "," + str(motion[index][1]) + "\n")
f.close()
def deg_to_rad(deg):
return deg * math.pi / 180.0
def rad_to_deg(rad):
return rad * 180.0 / math.pi
def main():
parser = argparse.ArgumentParser()
parser.add_argument("fname_sensor",
help = "Gyroscope file")
parser.add_argument("fname_output",
help = "Output file, where timestamps and angle of heading will be saved")
parser.add_argument("--time",
help = "Time interval, over which gyroscope will be integrated (default=500ms)",
type=int)
parser.add_argument("--rad",
help = "Output angles in rad (default in degree)",
action = "store_true")
parser.add_argument("--file_format",
help = "Sensor data file format [fhws|lukas] (default: fhws)",
type = str)
parser.add_argument("--cosy",
help = "Coordinate system of the gyroscope data [world|device] (default: device). If given in device, the data will automatically be rotated in world coordinates.",
type = str)
args = parser.parse_args()
#Choose between file format of sensor data and coordinate system
file_format = "fhws"
cosy = "device"
if args.file_format:
file_format = args.file_format
if args.cosy:
cosy = args.cosy
#My own data format
if file_format == "lukas":
delimiter = ","
time_cols = [40]
time = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=time_cols,
skiprows = 1,
unpack=True)
if cosy == "device":
gyros_cols = [9, 10, 11]
lin_accel_cols = [6, 7, 8]
else:
gyros_cols = [34, 35,36]
lin_accel_cols = [37, 38, 39]
grav_cols = [3, 4, 5]
gyroX, gyroY, gyroZ = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=gyros_cols,
skiprows = 1,
unpack=True)
rotation = np.loadtxt(args.fname_sensor,
delimiter = delimiter,
usecols=range(18,34),
skiprows=1,
unpack=True)
lin_accel_X, lin_accel_Y, lin_accel_Z = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=lin_accel_cols,
skiprows=1,
unpack=True)
gravity_X, gravity_Y, gravity_Z = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=grav_cols,
skiprows=1,
unpack=True)
rotation = rotation.T
rotation = [row.reshape((4,4)) for row in rotation]
# rotation = np.array(rotation).T
print rotation
gyro = np.array([gyroX, gyroY, gyroZ]).T
lin_accel = np.array([lin_accel_X, lin_accel_Y, lin_accel_Z]).T
gravity = np.array([gravity_X, gravity_Y, gravity_Z]).T
if cosy == "device":
world_gyro = rotate_data_lukas(gyro, rotation)
world_lin_accel = rotate_data_lukas(lin_accel, rotation)
else:
world_gyro = gyro
world_lin_accel = lin_accel
#FHWS file format
else:
time, gyro, lin_accel, gravity, rotation = read_data(args.fname_sensor)
if cosy == "device":
world_gyro = rotate_data_lukas(gyro, rotation)
world_lin_accel = rotate_data_lukas(lin_accel, rotation)
else:
print "Option 'fhws' in combination with 'world' not available"
return
gyroX = world_gyro[:,0]
gyroY = world_gyro[:,1]
gyroZ = world_gyro[:,2]
lin_accel_X = world_lin_accel[:,0]
lin_accel_Y = world_lin_accel[:,1]
lin_accel_Z = world_lin_accel[:,2]
#Parameters
#---------
time_interval = 500
if args.time:
time_interval = args.time
turns = detect_turns(time, gyroZ, time_interval)
motion = motion_axis(time, lin_accel, gravity, 500)
angles = []
for index, axis in enumerate(motion):
if index == 0:
angle = 0
else:
x_1 = motion[index-1][1]
y_1 = motion[index-1][2]
x_2 = axis[1]
y_2 = axis[2]
a = np.array([x_1, y_1])
b = np.array([x_2, y_2])
angle = angle_between(a,b)
angles.append((axis[0], angle))
np.set_printoptions(suppress=True)
turns = np.array(turns)
angles = np.array(angles)
if args.rad:
turns[:,1] = deg_to_rad(turns[:,1])
print "Sum of all angles: ", np.sum(turns[:,1])
write_to_file(args.fname_output, turns, angles)
if __name__ == "__main__":
main()

49
code/lukas/TurnEvaluation.h Executable file
View File

@@ -0,0 +1,49 @@
#ifndef TURNEVALUATION_H
#define TURNEVALUATION_H
#include "../particles/MyState.h"
#include "TurnObservation.h"
//#include <boost/math/special_functions/bessel.hpp>
#include <math.h>
//static constexpr double sigma_heading = 5;
class TurnEvaluation {
//All calculations use degree not rad!!!
public:
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
(void) state;
(void) obs;
(void) simple;
return 1;
// see: particle-filter-control-data
// // get the current heading-change
// const float delta = Angle::radToDeg(state.walkState.headingChange_rad);
// // and reset it as it was evaluated
// ((MyState&) state).walkState.headingChange_rad = 0;
// // proability?
// const float sigma = 15.0;
// const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
// if (prob != prob) {
// throw 1;
// }
// return prob;
}
};
#endif // TURNEVALUATION_H

21
code/lukas/TurnObservation.h Executable file
View File

@@ -0,0 +1,21 @@
#ifndef TURNOBSERVATION_H
#define TURNOBSERVATION_H
#include <vector>
struct TurnObservation {
float ts;
float delta_heading; //measured change of heading direction (given by Gyroskop)
float delta_motion; //measured change of motion direction (given by PCA)
TurnObservation() {;}
TurnObservation(const float delta_heading, const float motion_angle) : delta_heading(delta_heading), delta_motion(motion_angle) {;}
};
#endif // TURNOBSERVATION_H

36
code/lukas/TurnReader.h Executable file
View File

@@ -0,0 +1,36 @@
#ifndef TURNREADER_H
#define TURNREADER_H
#include "../reader/SensorReaderTurn.h"
#include "TurnObservation.h"
class TurnReader {
public:
static TurnObservation* readTurn(const SensorEntryTurn& se) {
std::string tmp = se.data;
TurnObservation* obs = new TurnObservation();
while(!tmp.empty()) {
int pos = tmp.find(',');
std::string heading = tmp.substr(0, pos);
tmp = tmp.substr(pos);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
std::string motion = tmp;
TurnObservation t(std::stof(heading), std::stof(motion));
}
return obs;
}
};
#endif // TURNREADER_H

21
code/lukas/detection.sh Executable file
View File

@@ -0,0 +1,21 @@
#!/bin/bash
FILES=$(find ../measurements/18/{Galaxy,Nexus}/ -name "*.csv")
for f in $FILES
do
echo $f
filename=$(basename $f)
directory=$(dirname $f)
#echo $filename
#echo $directory
step=$directory/Steps2.txt
turn=$directory/Turns.txt
echo $step
echo $turn
python StepDetector.py $f $step --lt -1.2 --ht 1.2
python TurnDetector.py $f $turn
done

113
code/main.cpp Normal file
View File

@@ -0,0 +1,113 @@
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include "Vis.h"
#include "Helper.h"
#include "MyGridNode.h"
#include "Helper.h"
#include "DijkstraMapper.h"
#include "eval/Eval.h"
#include "eval/Eval1.h"
#include "eval/EvalBase.h"
#include "eval/PaperVisImportance.h"
#include "eval/PaperVisDijkstra.h"
#include "eval/PaperVisGrid.h"
float BarometerEvaluation::barometerSigma = NAN;
Settings settings;
void testModelWalk() {
Grid<MyGridNode> grid(MiscSettings::gridSize_cm);
Helper::FHWSFloors floors = Helper::getFloors();
Helper::buildTheGrid(grid, floors);
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
//MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
Vis vis;
vis.addFloor(floors.f0, floors.h0);
vis.addFloor(floors.f1, floors.h1);
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
vis.gp << "set xrange [1100:1800]\n";
vis.gp << "set yrange [4500:5200]\n";
// vis.gp << "set xrange [1000:4000]\n";
// vis.gp << "set yrange [1000:4000]\n";
// vis.gp << "set zrange [0:600]\n";
// switch between different grid-walkers
GridWalkRandomHeadingUpdate<MyGridNode> walk;
//GridWalkRandomHeadingUpdateAdv<MyGridNode> walk;
//GridWalkPushForward<MyGridNode> walk;
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk(grid, DijkstraMapper(grid), end);
std::vector<GridWalkState<MyGridNode>> states;
for (int i = 0; i < 1000; ++i) { states.push_back(GridWalkState<MyGridNode>(&start, Heading::rnd())); }
// track the number-of-visits for each node to draw something like a particle-heat-map?
// show the importance factors
vis.addGrid(grid);
vis.show();
sleep(100);
vis.removeGrid();
Distribution::Normal<float> wDist(0.3, 0.3);
Distribution::Normal<float> wHead(0.3, 0.3);
while(true) {
for (GridWalkState<MyGridNode>& state : states) {
state = walk.getDestination(grid, state, std::abs(wDist.draw()), wHead.draw());
}
usleep(1000*80);
vis.showStates(states);
vis.show();
}
sleep(1000);
}
int main(void) {
// testModelWalk();
Eval1 eval;
//eval.path2_forward_simple();
//eval.path2_forward_path();
//eval.path3_forward_simple();
//eval.path3_forward_path();
//eval.path4_nexus_simple();
//eval.path4_nexus_imp();
//eval.path4_nexus_path();
//eval.path4_nexus_path_b();
eval.bergwerk_path1_nexus_simple();
eval.run();
// PaperVisGrid::showStairs();
// PaperVisImportance::createImportance();
// PaperVisImportance::createPath();
return 0;
}

12
code/particles/MyControl.h Executable file
View File

@@ -0,0 +1,12 @@
#ifndef MYCONTROL_H
#define MYCONTROL_H
struct MyControl {
float walked_m = 0;
float headingChange_rad = 0;
};
#endif // MYCONTROL_H

89
code/particles/MyEvaluation.h Executable file
View File

@@ -0,0 +1,89 @@
#ifndef MYEVALUATION_H
#define MYEVALUATION_H
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
#include "MyObservation.h"
#include "MyState.h"
#include "../frank/WiFiEvaluation.h"
#include "../frank/BeaconEvaluation.h"
#include "../toni/BarometerEvaluation.h"
#include "../lukas/StepEvaluation.h"
#include "../lukas/TurnEvaluation.h"
class MyEvaluation : public K::ParticleFilterEvaluation<MyState, MyObservation> {
private:
WiFiEvaluation wifiEval;
BeaconEvaluation beaconEval;
BarometerEvaluation barometerEval;
StepEvaluation stepEval;
TurnEvaluation turnEval;
bool useWifi = true;
bool useStep = true;
bool useTurn = true;
bool useBaro = true;
bool useIB = true;
public:
void setUsage(bool useWifi, bool useStep, bool useTurn, bool useBaro, bool useIB) {
this->useWifi = useWifi;
this->useStep = useStep;
this->useTurn = useTurn;
this->useBaro = useBaro;
this->useIB = useIB;
}
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
//if (observation.wifi) {
wifiEval.nextObservation(observation.wifi);
//}
// evalulate each particle
double sum = 0;
for (K::Particle<MyState>& p : particles) {
double weight = 1.0;
if (useWifi) {
weight *= wifiEval.getProbability(p.state, observation);
}
if (useBaro && observation.barometer) {
weight *= barometerEval.getProbability(p.state, observation.barometer);
}
if (useIB) {
weight *= beaconEval.getProbability(p.state, observation);
}
if (useStep) {
weight *= stepEval.getProbability(p.state, observation.step);
}
if (useTurn) {
weight *= turnEval.getProbability(p.state, observation.turn, true);
}
// set and accumulate
p.weight = weight;
sum += p.weight;
}
// reset
observation.step->steps = 0;
observation.turn->delta_heading = 0;
observation.turn->delta_motion = 0;
return sum;
}
};
#endif // MYEVALUATION_H

62
code/particles/MyInitializer.h Executable file
View File

@@ -0,0 +1,62 @@
#ifndef MYINITIALIZER3_H
#define MYINITIALIZER3_H
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include "MyState.h"
#include <Indoor/grid/Grid.h>
class MyInitializer : public K::ParticleFilterInitializer<MyState> {
private:
Grid<MyGridNode>& grid;
int x_cm;
int y_cm;
int z_cm;
int heading;
public:
/** q0 = random */
MyInitializer(Grid<MyGridNode>& grid) : grid(grid), heading(0) {
}
/** q0 = given */
MyInitializer(Grid<MyGridNode>& grid, int x_cm, int y_cm, int z_cm, int heading) :
grid(grid), x_cm(x_cm), y_cm(y_cm), z_cm(z_cm), heading(heading) {
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
std::minstd_rand gen;
std::uniform_int_distribution<> dist(0, grid.getNumNodes());
for (K::Particle<MyState>& p : particles) {
MyGridNode& n = grid[dist(gen)];
//p.state.pCur = Point3(x_cm, y_cm, z_cm);
//GridPoint gp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
//p.state.walkState.node = &grid.getNodeFor(gp);
p.state.pCur = (Point3) n;
p.state.walkState.node = &n;
p.state.pOld = p.state.pCur;
p.state.walkState.heading = Heading::rnd();
p.state.hPa = 0;
}
}
};
#endif // MYINITIALIZER_H

53
code/particles/MyObservation.h Executable file
View File

@@ -0,0 +1,53 @@
#ifndef MYOBSERVATION_H
#define MYOBSERVATION_H
#include "../frank/WiFiObservation.h"
#include "../frank/BeaconObservation.h"
#include "../frank/OrientationObservation.h"
#include "../toni/BarometerObservation.h"
#include "../lukas/StepObservation.h"
#include "../lukas/TurnObservation.h"
/**
* all available sensor readings
*/
struct MyObservation {
/** wifi observation */
WiFiObservation wifi;
OrientationObservation orientation;
/** barometer observation data (if any) */
BarometerObservation* barometer = nullptr;
/** beacon observation data */
BeaconObservation beacons;
/** step observation data (if any) */
StepObservation* step = nullptr;
/** turn observation data (if any) */
TurnObservation* turn = nullptr;
/** timestamp of the youngest sensor data that resides within this observation. used to detect the age of all other observations! */
uint64_t latestSensorDataTS = 0;
/** ctor */
MyObservation() {
// reset();
}
// /** set all observations to null */
// void reset() {
// //delete wifi; wifi = nullptr;
// delete barometer; barometer = nullptr;
// delete beacons; beacons = nullptr;
// //delete step; step = nullptr;
// //delete turn; turn = nullptr;
// }
};
#endif // MYOBSERVATION_H

140
code/particles/MyState.h Executable file
View File

@@ -0,0 +1,140 @@
#ifndef MYSTATE_H
#define MYSTATE_H
#include <KLib/math/distribution/Normal.h>
#include <KLib/math/optimization/NumOptVector.h>
#include <Indoor/grid/walk/GridWalkState.h>
#include "../MyGridNode.h"
/**
* one possible state for the pedestrian
* 3D position (x, y, floor-nr)
*/
struct MyState {
// current position
Point3 pCur;
// previous position
Point3 pOld;
// the grid-walk state
GridWalkState<MyGridNode> walkState;
//int distanceWalkedCM;
// double heading_old;
// //double transHeading;
// float numZChanges;
// // cumulative distance (in cm) this particle has taken. to-be-reset by the step detector whenever needed!
// double distanceWalkedCM;
double hPa; //relative Pressure given by a history with size defined in BarometerSensorReader.h
// double vertical_acc; //vertical acceleration
// /** the pedestrian's current heading */
// double heading;
/** empty ctor */
MyState() : pCur(0,0,0), pOld(0,0,0), walkState(nullptr, Heading(0)) {
;
}
// /** get the 2D distance between this state and the given x,y (in centimter) */
// double getDistance2D(const double x_cm, const double y_cm) const {
// const double dx = (x_cm - this->x_cm);
// const double dy = (y_cm - this->y_cm);
// return std::sqrt( (dx*dx) + (dy*dy) );
// }
// /** get the 3D distance between this state and the given x,y,floor (in centimter) */
// double getDistance3D(const double x_cm, const double y_cm, const double floor_height_cm) const {
// const double dx = (x_cm - this->x_cm);
// const double dy = (y_cm - this->y_cm);
// const double dz = (z_nr - this->z_nr) * floor_height_cm;
// return std::sqrt( (dx*dx) + (dy*dy) + (dz*dz) );
// }
/** -------- METHODS FOR THE PARTICLE FILTER -------- */
MyState& operator += (const MyState& o) {
pCur += o.pCur;
hPa += o.hPa;
//distanceWalked += o.distanceWalked;
return *this;
}
MyState& operator /= (const double d) {
pCur /= d;
hPa /= d;
//distanceWalked /= d;
return *this;
}
MyState operator * (const double d) const {
MyState s = MyState(*this);
s.pCur *= d;
s.hPa *= d;
//distanceWalked *= d;
return s;
}
// use the default one
// MyState& operator = (const MyState& o) {
// x_cm = o.x_cm;
// y_cm = o.y_cm;
// z_nr = o.z_nr;
// x_cm_old = o.x_cm_old;
// y_cm_old = o.y_cm_old;
// z_nr_old = o.z_nr_old;
// hPa = o.hPa;
// heading_old = o.heading_old;
// heading = o.heading;
// distanceWalkedCM = o.distanceWalkedCM;
// return *this;
// }
bool belongsToRegion(const MyState& o) const {
return o.pCur.getDistance(pCur) < 700;
}
// /** rejection for the regional estimator. reject after 150cm distance */
// bool belongsToRegion(const MyState& o) const {
//// // do NOT group particles in distinct floors!
//// if (z_nr != o.z_nr) {return false;}
//// // get the 2D distance
//// double d = (x_cm - o.x_cm)*(x_cm - o.x_cm) +
//// (y_cm - o.y_cm)*(y_cm - o.y_cm);
//// d = std::sqrt(d);
//// // 2D distance below grouping threshold?
//// return d < 350.0;
// const double dx = (x_cm - o.x_cm);
// const double dy = (y_cm - o.y_cm);
// const double dz = (z_nr - o.z_nr) * 3000;
// // get the 2D distance
// double d = dx*dx + dy*dy + dz*dz;
// d = std::sqrt(d);
// return d < 350.0;
// }
// MyState(K::NumOptVector<3>& params) : x_cm(params[0]), y_cm(params[1]), z_cm(params[2]) {;}
};
#endif // MYSTATE_H

125
code/particles/MyTransition.h Executable file
View File

@@ -0,0 +1,125 @@
#ifndef MYTRANSITION_H
#define MYTRANSITION_H
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/distribution/Normal.h>
#include <KLib/math/distribution/Uniform.h>
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/walk/GridWalk.h>
#include "MyState.h"
#include "MyControl.h"
//#include "Helper.h"
#include "../toni/barometric.h"
#include "../MyGridNode.h"
inline double sgn(double x){
return ((x>0)?1 : ((x<0)?-1 : 1));
}
class MyTransition : public K::ParticleFilterTransition<MyState, MyControl> {
private:
Grid<MyGridNode>& grid;
GridWalk<MyGridNode>& walker;
/** a simple normal distribution */
K::UniformDistribution distWalkStop;
K::NormalDistribution distWalkPerSec;
K::NormalDistribution distStop;
/** normal distribution for barometer */
K::NormalDistribution distBaro;
public:
/**
* ctor
* @param choice the choice to use for randomly drawing nodes
* @param fp the underlying floorplan
*/
MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& walker) :
grid(grid), walker(walker),
distWalkStop(0.0, 1.0), distWalkPerSec(1.0, 0.3), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
distWalkStop.setSeed(1234);
distWalkPerSec.setSeed(1234);
distStop.setSeed(1234);
distBaro.setSeed(5678);
}
public:
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 = ts - this->ts;
this->ts = ts;
}
}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
for (K::Particle<MyState>& p : particles) {
// TODO: depending on the time since the last update
// random distance to move
// const double distance = (distWalkStop.draw() > 0.2) ? (distWalk.draw()) : (distStop.draw());
// double dist_m = distance * deltaMS / 1000.0;
// if (dist_m < 0) {dist_m = -dist_m; p.state.heading = rand() % 360;}
// update the old heading and the other old values
//p.state.walkState.heading = p.state.heading;
p.state.pOld = p.state.pCur;
// // 10% stand still, 90% walk
// double dist_m;
// if (distWalkStop.draw() > 0.9) {
// dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
// } else {
// dist_m = std::abs(distWalkPerSec.draw() * deltaMS / 1000.0);
// }
// get new destination
//const Node3* dst = choice->getTarget(src, p.state, dist_m);
p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad );
// randomly move the particle within its target grid (box)
// (z remains unchanged!)
//const int grid_size_cm = grid.getGridSize_cm();
// new position (x,y) is randomly distributed within the target node
Point3 noise = Point3(0,0,0); // TODO
p.state.pCur = (Point3) *p.state.walkState.node + noise;
// update the baromter
p.state.hPa += (p.state.pOld.z - p.state.pCur.z) / 100.0f * 0.105f;
}
}
};
#endif // MYTRANSITION_H

83
code/particles/MyTransitionSimple.h Executable file
View File

@@ -0,0 +1,83 @@
#ifndef MYTRANSITIONSIMPLE_H
#define MYTRANSITIONSIMPLE_H
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/distribution/Normal.h>
#include "MyState.h"
#include "MyControl.h"
class MyTransitionSimple : public K::ParticleFilterTransition<MyState, MyControl> {
private:
/** a simple normal distribution */
K::NormalDistribution distX;
K::NormalDistribution distY;
K::NormalDistribution distZ;
K::NormalDistribution distBaro;
public:
/** ctor */
MyTransitionSimple() : distX(0, 1.0), distY(0, 1.0), distZ(0, 1.0), distBaro(0.3, 0.05) {
distX.setSeed(1234);
distY.setSeed(1235);
distZ.setSeed(1236);
distBaro.setSeed(5678);
}
public:
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 = ts - this->ts;
this->ts = ts;
}
}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
for (K::Particle<MyState>& p : particles) {
p.state.heading_old = p.state.heading;
p.state.x_cm_old = p.state.x_cm;
p.state.y_cm_old = p.state.y_cm;
p.state.z_nr_old = p.state.z_nr;
p.state.x_cm += (distX.draw() * deltaMS / 1000.0) * 250.0;
p.state.y_cm += (distY.draw() * deltaMS / 1000.0) * 250.0;
p.state.z_nr += (distZ.draw() * deltaMS / 1000.0) * 0.25;
p.state.heading = Helper::angleBetween(p.state.x_cm_old, p.state.y_cm_old, p.state.x_cm, p.state.y_cm);
// if (p.state.z_nr < 0.5) {p.state.z_nr = 0.5;}
// if (p.state.z_nr > 3.5) {p.state.z_nr = 3.5;}
// if (p.state.x_cm < 0) {p.state.x_cm = 0;}
// if (p.state.y_cm < 0) {p.state.y_cm = 0;}
//update barometer
p.state.hPa += (p.state.z_nr_old - p.state.z_nr) * distBaro.draw();
// update walked distance (2D)
const double dx = p.state.x_cm_old - p.state.x_cm;
const double dy = p.state.y_cm_old - p.state.y_cm;
p.state.distanceWalkedCM = std::sqrt((dx*dx) + (dy*dy));
}
}
};
#endif // MYTRANSITIONSIMPLE_H

4715
code/plan.svg Executable file
View File

File diff suppressed because it is too large Load Diff
Side by Side

After

Width:  |  Height:  |  Size: 226 KiB

4957
code/plan_new.svg Normal file
View File

File diff suppressed because it is too large Load Diff
Side by Side

After

Width:  |  Height:  |  Size: 239 KiB

355
code/plan_plots.svg Normal file
View File

@@ -0,0 +1,355 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="724"
height="1800"
viewBox="0 0 724.00001 1800"
id="svg5100"
version="1.1"
inkscape:version="0.91 r13725"
sodipodi:docname="plan_plots.svg">
<defs
id="defs5102" />
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="0.98994949"
inkscape:cx="451.23575"
inkscape:cy="136.10259"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="true"
fit-margin-top="0"
fit-margin-left="0"
fit-margin-right="0"
fit-margin-bottom="0"
inkscape:object-nodes="true"
inkscape:window-width="1600"
inkscape:window-height="845"
inkscape:window-x="0"
inkscape:window-y="0"
inkscape:window-maximized="1"
units="px">
<inkscape:grid
type="xygrid"
id="grid5900"
originx="0"
originy="6.8902557e-05"
empspacing="4" />
</sodipodi:namedview>
<metadata
id="metadata5105">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="test1"
inkscape:groupmode="layer"
id="layer1"
transform="translate(814.21766,568.50997)">
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -810.32002,-557.59658 0.10236,1785.08658 318.89764,0 0,-162.9922"
id="path11563"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,1021.9782 0,-28.34651 -318.89764,0"
id="path11567"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,993.63169 0,-38.9764"
id="path11569"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,919.22219 0,-123.81877"
id="path11571"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,320.40335 0,-464.17327"
id="path11577"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,-179.203 0,-109.84247"
id="path11581"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,320.40342 -246,0"
id="path11587"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -711.32002,320.40342 -98.89764,-7e-5"
id="path11589"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,-288.59658 -319,0"
id="path11593"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -475.32002,-345.59665 -235.68504,-0.14174 0,-131.10236"
id="path11597"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -475.32002,-551.59665 0,206"
id="path11605"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -711.00506,-515.81713 0,-42.51968"
id="path11607"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,1227.49 287.00787,0 -0.008,-1117.08658"
id="path11619"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -425.32002,795.40342 150,0"
id="path11623"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,864.40342 209,0"
id="path11627"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.36727,1032.6081 209.05512,0"
id="path11631"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,864.40342 -0.0472,302.85038"
id="path11633"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,795.40342 0,-207"
id="path11637"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -420.45388,-345.73843 53.14961,0"
id="path11669"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -321.24128,-345.73843 38.97638,0"
id="path11671"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -409.82396,-427.23449 0,81.49606"
id="path11673"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -409.82396,-427.23449 173.62205,0"
id="path11675"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -321.24128,-345.73843 0,-81.49606"
id="path11677"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -236.20191,-558.33685 0,212.59842 139.88189,0.14185"
id="path11679"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,-223.59658 0,544"
id="path11757"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -123.32002,-223.59658 27,0"
id="path11761"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -310.61136,-491.01402 -102.75591,0"
id="path11822"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -563.32002,696.40342 150,0"
id="path4208"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -392.10742,-427.23449 0,-63.77953"
id="path5833"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -392.10742,-491.01402 0,-67.32283"
id="path5847"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -810.32002,-557.59658 715,0"
id="path5902"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -355.32002,110.40342 260,0"
id="path5923"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -96.32002,-557.59658 0,668"
id="path5925"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,660.40339 0,-139.99997"
id="path5929"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -810.32002,384.40342 183,0"
id="path5931"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -627.32002,384.40342 60,24 52,52"
id="path5933"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -515.32002,460.40342 24,60"
id="path5935"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,-223.59658 246,0"
id="path5937"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,795.40342 -78,0"
id="path5939"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,600.40339 134,3e-5"
id="path5941"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -563.32002,696.40342 72,98.99997"
id="path5945"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,528.40342 134,0"
id="path5947"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,320.40342 22,0"
id="path5951"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -359.32002,320.40342 155,0"
id="path5953"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,540.40342 0,-156"
id="path5955"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -239.32002,600.40339 35,3e-5"
id="path5959"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -239.32002,528.40339 35,3e-5"
id="path5961"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,110.40342 22,0"
id="path5965"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -235.32002,795.40342 31,-3e-5"
id="path5967"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 15 KiB

66
code/reader/SensorReader.h Executable file
View File

@@ -0,0 +1,66 @@
#ifndef SENSORREADER_H
#define SENSORREADER_H
#include <fstream>
/** entry for one sensor */
struct SensorEntry {
/** timestamp of occurrence */
uint64_t ts;
/** sensor's number */
int idx;
/** sensor data */
std::string data;
};
/** read sensor data from CSV */
class SensorReader {
private:
std::string file;
std::ifstream fp;
public:
SensorReader(const std::string& file) : file(file) {
rewind();
}
bool hasNext() {
return !fp.bad() && !fp.eof();
}
/** read the next sensor entry */
SensorEntry getNext() {
char delim;
SensorEntry entry;
fp >> entry.ts;
fp >> delim;
fp >> entry.idx;
fp >> delim;
fp >> entry.data;
return entry;
}
/** start again */
void rewind() {
fp.close();
fp.open(file);
assert(fp.is_open());
}
};
#endif // SENSORREADER_H

49
code/reader/SensorReaderStep.h Executable file
View File

@@ -0,0 +1,49 @@
#ifndef SENSORREADERSTEP_H
#define SENSORREADERSTEP_H
#include <fstream>
/** entry for one sensor */
struct SensorEntryStep {
/** sensor data */
float ts; //timestamp of the step
};
/** read sensor data from CSV */
class SensorReaderStep {
private:
std::ifstream fp;
public:
SensorReaderStep(const std::string& file) {
fp.open(file);
assert(fp.is_open());
}
bool hasNext() {
return !fp.bad() && !fp.eof();
}
/** read the next sensor entry */
SensorEntryStep getNext() {
SensorEntryStep entry;
fp >> entry.ts;
return entry;
}
};
#endif // SENSORREADERSTEP_H

56
code/reader/SensorReaderTurn.h Executable file
View File

@@ -0,0 +1,56 @@
#ifndef SENSORREADERTURN_H
#define SENSORREADERTURN_H
#include <fstream>
/** entry for one sensor */
struct SensorEntryTurn {
/** timestamp of occurrence */
float ts;
/** sensor data */
float delta_heading;
float delta_motion;
};
/** read sensor data from CSV */
class SensorReaderTurn {
private:
std::ifstream fp;
public:
SensorReaderTurn(const std::string& file) {
fp.open(file);
assert(fp.is_open());
}
bool hasNext() {
return !fp.bad() && !fp.eof();
}
/** read the next sensor entry */
SensorEntryTurn getNext() {
char delim;
SensorEntryTurn entry;
fp >> entry.ts;
fp >> delim;
fp >> entry.delta_heading;
fp >> delim;
fp >> entry.delta_motion;
return entry;
}
};
#endif // SENSORREADERTURN_H

65
code/toni/BarometerEvaluation.h Executable file
View File

@@ -0,0 +1,65 @@
#pragma once
#include "../particles/MyState.h"
#include "BarometerObservation.h"
//#include "barometric.h"
//#include <KLib/math/distribution/Normal.h>
static constexpr double g_BarometerObservation = 0.0;
class BarometerEvaluation {
public:
static float barometerSigma;//= 0.12+0.04;
public:
double getProbability(const MyState& state, const BarometerObservation* obs) const {
// //rho_z
//double barometerSigma = 0.06;//0.12+0.04;//0.09;
// //The height of the single floor levels.
// const static double floor_height[3] = {4.1, 3.4, 3.4};
// if(USE_BAROMETRIC_FORMULAR){
// //height the particle has climbed.
// double h_1 = 0.0;
// for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
// h_1 += floor_height[i];
// }
// if(h_1 != 0.0){
// // use the barometric formular to calculate the relative pressure
// // the calculation is done assuming sea level height at every floor.
// double mslp = BarometricFormular::s_getSeaLevelPressure();
// double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
// barometerSigma = std::abs(mslp - pressure);
// }
// }
// else {
// // constant value for sigma if we assume all floors are same in height
// barometerSigma = 0.30 / 1.0; //hPa
// }
// // evaluate the current particle with a normal distribution
const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma, obs->hpa);
// //Just for the visualization. i'm a lazy bastard
//g_BarometerObservation = obs->hpa;
assert(barometerProbability == barometerProbability);
assert(state.hPa == state.hPa);
assert(obs->hpa == obs->hpa);
// //std::cout << barometerProbability << std::endl;
//return pow(2.0, barometerProbability);
return barometerProbability;
}
};

17
code/toni/BarometerObservation.h Executable file
View File

@@ -0,0 +1,17 @@
#pragma once
#include <cstdint>
struct BarometerObservation {
double hpa;
BarometerObservation() { ; }
BarometerObservation(const float hpa) : hpa(hpa) {
;}
};

70
code/toni/BarometerSensorReader.h Executable file
View File

@@ -0,0 +1,70 @@
#pragma once
//#include "circular.h"
#include "BarometerObservation.h"
#include "../reader/SensorReader.h"
#include <sstream>
#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/MovingMedian.h>
//circular_buffer<double> measurementHistory(1000);
class BarometerSensorReader{
private:
// NOTE: median or avg?
MovingMedian<float> avg;
MovingMedian<float> avgStart;
// avg: lower size, median: bigger still fine
static constexpr int avgSize = 10;
static constexpr int startAvgSize = 10;
// skip the first 1-2 seconds and let the sensor settle itself
uint64_t skipTS = 0;
public:
BarometerSensorReader(): avg(avgSize), avgStart(startAvgSize) {
;
}
BarometerObservation* readBarometer(const SensorEntry& se) {
// skip the first few 1.5 seconds
if (skipTS == 0) {skipTS = se.ts;}
if (se.ts - skipTS < 3000) {return nullptr;}
std::string tmp = se.data;
BarometerObservation* obs = new BarometerObservation();
const float cur = std::stof(tmp);
// get the next hPa reading and average it
avg.add(cur);
// average the first few readings as reference
if (avgStart.getNumUsed() < startAvgSize) {
avgStart.add(cur);
}
// current average relative to the start-average
obs->hpa = avg.get() - avgStart.get();
// done
return obs;
}
// //TODO
// void readVerticalAcceleration(const SensorEntry& se){
// //Problem: Koordinatensystem LinearAcceleraton ist relativ zum Telefon und nicht zum
// //Weltkoordinatensystem. Brauchen die Beschleunigung nach Oben in Weltkoordinaten.
// }
};

52
code/toni/TFRingBuffer.h Executable file
View File

@@ -0,0 +1,52 @@
#ifndef TFObjectPool_TFRingBuffer_h
#define TFObjectPool_TFRingBuffer_h
#include <atomic>
#include <cstddef>
template <typename T> class TFRingBuffer {
T *m_buffer;
std::atomic<size_t> m_head;
std::atomic<size_t> m_tail;
const size_t m_size;
size_t next(size_t current) {
return (current + 1) % m_size;
}
public:
TFRingBuffer(const size_t size) : m_size(size), m_head(0), m_tail(0) {
m_buffer = new T[size];
}
virtual ~TFRingBuffer() {
delete[] m_buffer;
}
bool push(const T &object) {
size_t head = m_head.load(std::memory_order_relaxed);
size_t nextHead = next(head);
if (nextHead == m_tail.load(std::memory_order_acquire)) {
return false;
}
m_buffer[head] = object;
m_head.store(nextHead, std::memory_order_release);
return true;
}
bool pop(T &object) {
size_t tail = m_tail.load(std::memory_order_relaxed);
if (tail == m_head.load(std::memory_order_acquire)) {
return false;
}
object = m_buffer[tail];
m_tail.store(next(tail), std::memory_order_release);
return true;
}
};
#endif

111
code/toni/barometric.h Executable file
View File

@@ -0,0 +1,111 @@
#define BAROMETRIC
#ifndef BAROMETRIC
#define BAROMETRIC
const static double mslp = 980.25; // mean sea level spressure
const static double int_lapse_rate = 0.0065; // a
const static double int_exponent = 5.255; // international barometric formular exponent calculated from (M * g) / (R * a)
//The height of the single floor levels.
const static double floor_height[5] = {0.0, 4.1, 3.4, 3.4, 3.4};
class BarometricFormular
{
private:
const double temperature; // T in Kelvin
const double universal_gas_constant; // R
const double molar_mass; // M
const double gravitational_acceleration; // g
const double lapse_rate; // a
double _exponent;
public:
/** ctor */
BarometricFormular(const double R, const double M, const double g, const double a, const double T):
universal_gas_constant(R), molar_mass(M), gravitational_acceleration(g), lapse_rate(a), temperature(T){
_exponent = (M * g) / (R * a);
}
/** ctor only with Temperature*/
BarometricFormular(const double T) :
universal_gas_constant(8.314), molar_mass(0.02896), gravitational_acceleration(9.80665), lapse_rate(0.0065), temperature(T){
_exponent = (molar_mass * gravitational_acceleration) / (universal_gas_constant * lapse_rate);
}
/** Atmospheric Pressure Calculation */
double getAtmosphericPressure(double p_0, double h_1) const{
return p_0 * std::pow((1.0 - ((lapse_rate * h_1)/temperature)), _exponent);
}
/** Atmospheric Pressure Calculation above sea level*/
double getAtmosphericPressure(double h_1) const{
return mslp * std::pow((1.0 - ((lapse_rate * h_1)/temperature)), _exponent);
}
//TODO:: Height from pressure for the general formular
//Static Functions
/** International Barometric Formular*/
static double s_getAtmosphericPressure(double p_0, double h_1, double kelvin){
return p_0 * std::pow((1.0 - ((int_lapse_rate * h_1)/kelvin)), int_exponent);
}
/** International Barometric Formular above Sea Level*/
static double s_getAtmosphericPressure(double h_1, double kelvin){
return mslp * std::pow((1.0 - ((int_lapse_rate * h_1)/kelvin)), int_exponent);
}
/** International Barometric Formular above Sea Level at 15 degree*/
static double s_getAtmosphericPressure(double height_above_sea_level){
return mslp * std::pow((1.0 - ((int_lapse_rate * height_above_sea_level)/288.15)), int_exponent);
}
/** Get the height above sea level using a pressure measurment above sea level*/
static double getHeightAboveSeaLevel(double p, double kelvin){
// http://www.wolframalpha.com/input/?i=solve+for+h+++p+%3D+980.25*%281+-+0.0065+*+h%2FT%29^5.255
return 41.4811 * ((3.70882 * kelvin) - (std::pow(p, 0.1902949571836346) * kelvin));
}
/** This is a helper Class only for gnupplot visualization for ipin2015*/
static double getHeightForVisualizationOnly(double p, double z_0, double kelvin){
// the height of the reference (first) pressure measurement
double h_0 = 0.0;
for(int i = 0; i <= z_0; i++){
h_0 += floor_height[i];
}
// pressure value of h_0 above sea level
// we define that the bottom of floor 0 is sea level ;).
double p_0 = s_getAtmosphericPressure(h_0, kelvin);
// pressure value of the current particle above floor 0 (sea level)
double p_height = p_0 + p;
// height of the particle above floor 0 (sea level)
return getHeightAboveSeaLevel(p_height, kelvin);
}
static double s_getSeaLevelPressure(){
return mslp;
}
static double getPressureOfFloorForVizualization(double z, double kelvin){
int i = z + 0.5;
double h_z = floor_height[i+1];
double p_z = s_getAtmosphericPressure(h_z, kelvin);
return std::abs(mslp - p_z);
}
};
#endif // BAROMETRIC

492
code/toni/circular.h Executable file
View File

@@ -0,0 +1,492 @@
/******************************************************************************
* $Id: $
* $Name: $
*
* Author: Pete Goodliffe
*
* ----------------------------------------------------------------------------
* Copyright 2002 Pete Goodliffe All rights reserved.
*
* ----------------------------------------------------------------------------
* Purpose: STL-style circular buffer
*
* ----------------------------------------------------------------------------
* History: See source control system log.
*
*****************************************************************************/
#ifndef CIRCULAR_BUFFER_H
#define CIRCULAR_BUFFER_H
#include <exception>
#include <iterator>
#include <memory>
/******************************************************************************
* Iterators
*****************************************************************************/
/**
* Iterator type for the circular_buffer class.
*
* This one template class provides all variants of forward/reverse
* const/non const iterators through plentiful template magic.
*
* You don't need to instantiate it directly, use the good public functions
* availble in circular_buffer.
*/
template <typename T, //circular_buffer type
//(incl const)
typename T_nonconst, //with any consts
typename elem_type = typename T::value_type> //+ const for const iter
class circular_buffer_iterator
{
public:
typedef circular_buffer_iterator<T, T_nonconst, elem_type> self_type;
typedef T cbuf_type;
typedef std::random_access_iterator_tag iterator_category;
typedef typename cbuf_type::value_type value_type;
typedef typename cbuf_type::size_type size_type;
typedef typename cbuf_type::pointer pointer;
typedef typename cbuf_type::const_pointer const_pointer;
typedef typename cbuf_type::reference reference;
typedef typename cbuf_type::const_reference const_reference;
typedef typename cbuf_type::difference_type difference_type;
circular_buffer_iterator(cbuf_type *b, size_type p)
: buf_(b), pos_(p) {}
// Converting a non-const iterator to a const iterator
circular_buffer_iterator
(const circular_buffer_iterator<T_nonconst, T_nonconst,
typename T_nonconst::value_type>
&other)
: buf_(other.buf_), pos_(other.pos_) {}
friend class circular_buffer_iterator<const T, T, const elem_type>;
// Use compiler generated copy ctor, copy assignment operator and dtor
elem_type &operator*() { return (*buf_)[pos_]; }
elem_type *operator->() { return &(operator*()); }
self_type &operator++()
{
pos_ += 1;
return *this;
}
self_type operator++(int)
{
self_type tmp(*this);
++(*this);
return tmp;
}
self_type &operator--()
{
pos_ -= 1;
return *this;
}
self_type operator--(int)
{
self_type tmp(*this);
--(*this);
return tmp;
}
self_type operator+(difference_type n) const
{
self_type tmp(*this);
tmp.pos_ += n;
return tmp;
}
self_type &operator+=(difference_type n)
{
pos_ += n;
return *this;
}
self_type operator-(difference_type n) const
{
self_type tmp(*this);
tmp.pos_ -= n;
return tmp;
}
self_type &operator-=(difference_type n)
{
pos_ -= n;
return *this;
}
difference_type operator-(const self_type &c) const
{
return pos_ - c.pos_;
}
bool operator==(const self_type &other) const
{
return pos_ == other.pos_ && buf_ == other.buf_;
}
bool operator!=(const self_type &other) const
{
return pos_ != other.pos_ && buf_ == other.buf_;
}
bool operator>(const self_type &other) const
{
return pos_ > other.pos_;
}
bool operator>=(const self_type &other) const
{
return pos_ >= other.pos_;
}
bool operator<(const self_type &other) const
{
return pos_ < other.pos_;
}
bool operator<=(const self_type &other) const
{
return pos_ <= other.pos_;
}
private:
cbuf_type *buf_;
size_type pos_;
};
template <typename circular_buffer_iterator_t>
circular_buffer_iterator_t operator+
(const typename circular_buffer_iterator_t::difference_type &a,
const circular_buffer_iterator_t &b)
{
return circular_buffer_iterator_t(a) + b;
}
template <typename circular_buffer_iterator_t>
circular_buffer_iterator_t operator-
(const typename circular_buffer_iterator_t::difference_type &a,
const circular_buffer_iterator_t &b)
{
return circular_buffer_iterator_t(a) - b;
}
/******************************************************************************
* circular_buffer
*****************************************************************************/
/**
* This class provides a circular buffer in the STL style.
*
* You can add data to the end using the @ref push_back function, read data
* using @ref front() and remove data using @ref pop_front().
*
* The class also provides random access through the @ref operator[]()
* function and its random access iterator. Subscripting the array with
* an invalid (out of range) index number leads to undefined results, both
* for reading and writing.
*
* This class template accepts three template parameters:
* <li> T The type of object contained
* <li> always_accept_data_when_full Determines the behaviour of
* @ref push_back when the buffer is full.
* Set to true new data is always added, the
* old "end" data is thrown away.
* Set to false, the new data is not added.
* No error is returned neither is an
* exception raised.
* <li> Alloc Allocator type to use (in line with other
* STL containers).
*
* @short STL style circule buffer
* @author Pete Goodliffe
* @version 1.00
*/
template <typename T,
bool always_accept_data_when_full = true,
typename Alloc = std::allocator<T> >
class circular_buffer
{
public:
enum
{
version_major = 1,
version_minor = 0
};
// Typedefs
typedef circular_buffer<T, always_accept_data_when_full, Alloc>
self_type;
typedef Alloc allocator_type;
typedef typename Alloc::value_type value_type;
typedef typename Alloc::pointer pointer;
typedef typename Alloc::const_pointer const_pointer;
typedef typename Alloc::reference reference;
typedef typename Alloc::const_reference const_reference;
typedef typename Alloc::size_type size_type;
typedef typename Alloc::difference_type difference_type;
typedef circular_buffer_iterator
<self_type, self_type>
iterator;
typedef circular_buffer_iterator
<const self_type, self_type, const value_type>
const_iterator;
typedef std::reverse_iterator<iterator> reverse_iterator;
typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
// Lifetime
enum { default_capacity = 100 };
explicit circular_buffer(size_type capacity = default_capacity)
: array_(alloc_.allocate(capacity)), array_size_(capacity),
head_(1), tail_(0), contents_size_(0)
{
}
circular_buffer(const circular_buffer &other)
: array_(alloc_.allocate(other.array_size_)),
array_size_(other.array_size_),
head_(other.head_), tail_(other.tail_),
contents_size_(other.contents_size_)
{
try
{
assign_into(other.begin(), other.end());
}
catch (...)
{
destroy_all_elements();
alloc_.deallocate(array_, array_size_);
throw;
}
}
template <class InputIterator>
circular_buffer(InputIterator from, InputIterator to)
: array_(alloc_.allocate(1)), array_size_(1),
head_(1), tail_(0), contents_size_(0)
{
circular_buffer tmp;
tmp.assign_into_reserving(from, to);
swap(tmp);
}
~circular_buffer()
{
destroy_all_elements();
alloc_.deallocate(array_, array_size_);
}
circular_buffer &operator=(const self_type &other)
{
circular_buffer tmp(other);
swap(tmp);
return *this;
}
void swap(circular_buffer &other)
{
std::swap(array_, other.array_);
std::swap(array_size_, other.array_size_);
std::swap(head_, other.head_);
std::swap(tail_, other.tail_);
std::swap(contents_size_, other.contents_size_);
}
allocator_type get_allocator() const { return alloc_; }
// Iterators
iterator begin() { return iterator(this, 0); }
iterator end() { return iterator(this, size()); }
const_iterator begin() const { return const_iterator(this, 0); }
const_iterator end() const { return const_iterator(this, size()); }
reverse_iterator rbegin() { return reverse_iterator(end()); }
reverse_iterator rend() { return reverse_iterator(begin()); }
const_reverse_iterator rbegin() const
{
return const_reverse_iterator(end());
}
const_reverse_iterator rend() const
{
return const_reverse_iterator(begin());
}
// Size
size_type size() const { return contents_size_; }
size_type capacity() const { return array_size_; }
bool empty() const { return !contents_size_; }
size_type max_size() const
{
return alloc_.max_size();
}
void reserve(size_type new_size)
{
if (capacity() < new_size)
{
circular_buffer tmp(new_size);
tmp.assign_into(begin(), end());
swap(tmp);
}
}
// Accessing
reference front() { return array_[head_]; }
reference back() { return array_[tail_]; }
const_reference front() const { return array_[head_]; }
const_reference back() const { return array_[tail_]; }
void push_back(const value_type &item)
{
size_type next = next_tail();
if (contents_size_ == array_size_)
{
if (always_accept_data_when_full)
{
array_[next] = item;
increment_head();
}
}
else
{
alloc_.construct(array_ + next, item);
}
increment_tail();
}
void pop_front()
{
size_type destroy_pos = head_;
increment_head();
alloc_.destroy(array_ + destroy_pos);
}
void clear()
{
for (size_type n = 0; n < contents_size_; ++n)
{
alloc_.destroy(array_ + index_to_subscript(n));
}
head_ = 1;
tail_ = contents_size_ = 0;
}
reference operator[](size_type n) { return at_unchecked(n); }
const_reference operator[](size_type n) const { return at_unchecked(n); }
reference at(size_type n) { return at_checked(n); }
const_reference at(size_type n) const { return at_checked(n); }
private:
reference at_unchecked(size_type index) const
{
return array_[index_to_subscript(index)];
}
reference at_checked(size_type index) const
{
if (size >= contents_size_)
{
throw std::out_of_range();
}
return at_unchecked(index);
}
// Rounds an unbounded to an index into array_
size_type normalise(size_type n) const { return n % array_size_; }
// Converts external index to an array subscript
size_type index_to_subscript(size_type index) const
{
return normalise(index + head_);
}
void increment_tail()
{
++contents_size_;
tail_ = next_tail();
}
size_type next_tail()
{
return (tail_ + 1 == array_size_) ? 0 : tail_ + 1;
}
void increment_head()
{
// precondition: !empty()
++head_;
--contents_size_;
if (head_ == array_size_) head_ = 0;
}
template <typename f_iter>
void assign_into(f_iter from, f_iter to)
{
if (contents_size_) clear();
while (from != to)
{
push_back(*from);
++from;
}
}
template <typename f_iter>
void assign_into_reserving(f_iter from, f_iter to)
{
if (contents_size_) clear();
while (from != to)
{
if (contents_size_ == array_size_)
{
reserve(static_cast<size_type>(array_size_ * 1.5));
}
push_back(*from);
++from;
}
}
void destroy_all_elements()
{
for (size_type n = 0; n < contents_size_; ++n)
{
alloc_.destroy(array_ + index_to_subscript(n));
}
}
allocator_type alloc_;
value_type *array_;
size_type array_size_;
size_type head_;
size_type tail_;
size_type contents_size_;
};
template <typename T,
bool consume_policy,
typename Alloc>
bool operator==(const circular_buffer<T, consume_policy, Alloc> &a,
const circular_buffer<T, consume_policy, Alloc> &b)
{
return a.size() == b.size() && std::equal(a.begin(), a.end(), b.begin());
}
template <typename T,
bool consume_policy,
typename Alloc>
bool operator!=(const circular_buffer<T, consume_policy, Alloc> &a,
const circular_buffer<T, consume_policy, Alloc> &b)
{
return a.size() != b.size() || !std::equal(a.begin(), a.end(), b.begin());
}
template <typename T,
bool consume_policy,
typename Alloc>
bool operator<(const circular_buffer<T, consume_policy, Alloc> &a,
const circular_buffer<T, consume_policy, Alloc> &b)
{
return std::lexicographical_compare(a.begin(), a.end(), b.begin(), b.end());
}
#endif