FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

initial commit before ownership transfer

Browse Source
This commit is contained in:
FrankE
2016-01-25 17:57:49 +01:00
parent 36056ad002
commit 353bba8342
37 changed files with 7639 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
code/CMakeLists.txt Executable file
View File

@@ -0,0 +1,91 @@
# 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})

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

73
code/Vis.h Normal file
View File

@@ -0,0 +1,73 @@
#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 <Indoor/geo/Length.h>
#include <Indoor/floorplan/Floor.h>
class Vis {
public:
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementLines floors;
K::GnuplotSplotElementPoints gridNodes;
K::GnuplotSplotElementLines gridEdges;
public:
Vis() {
gp << "set hidden3d front\n";
gp << "set view equal xy\n";
gp << "set ticslevel 0\n";
// attach all layers
splot.add(&floors);
splot.add(&gridNodes);
splot.add(&gridEdges);
}
/** 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) {
for (const T& n1 : grid) {
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
gridNodes.add(p1);
for (const T& n2 : grid.neighbors(n1)) {
const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
gridEdges.addSegment(p1, p2);
}
}
return *this;
}
/** show (plot) the current setup */
void show() {
gp.draw(splot);
gp.flush();
}
};
#endif // VIS_H

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

@@ -0,0 +1,108 @@
#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 = 20.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;
// floor difference?
const double floorDist = std::abs(beacon->zNr - state.z_nr);
// 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) * 2.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 "../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

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

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

@@ -0,0 +1,23 @@
#ifndef POSITIONEDBEACON_H
#define POSITIONEDBEACON_H
#include "WiFiAP.h"
#include "Position3D.h"
class PositionedBeacon : public Position3D {
public:
MACAddress mac;
double tx;
double 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) {
;
}
};
#endif // POSITIONEDBEACON_H

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

@@ -0,0 +1,20 @@
#ifndef POSITIONEDWIFIAP_H
#define POSITIONEDWIFIAP_H
#include "WiFiAP.h"
#include "Position3D.h"
class PositionedWifiAP : public WiFiAP, public Position3D {
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) {
;
}
};
#endif // POSITIONEDWIFIAP_H

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

@@ -0,0 +1,136 @@
#ifndef SETTINGS_H
#define SETTINGS_H
#include "PositionedWiFiAP.h"
#include "PositionedBeacon.h"
#include "MACAddress.h"
#include <unordered_map>
class Settings {
private:
std::unordered_map<MACAddress, PositionedWifiAP*> aps;
std::unordered_map<MACAddress, PositionedBeacon*> beacons;
public:
Settings() {
const double pl = 2.7;
const double tx = -46;
addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, 3, tx, pl-0.5);
addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, 3, tx, pl-0.5);
addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, 3, tx, pl-0.5);
addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, 3, tx, pl);
addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, 3, tx, pl);
// 2nd floor (vague AP position)
addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, 2, tx, pl-0.5);
addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, 2, tx, pl);
addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, 2, tx, pl);
addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, 2, tx, pl-0.5);
addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, 2, tx, pl-0.5);
// 1st floor (vague AP position)
addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, 1, tx, pl);
addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, 1, tx, pl);
addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, 1, tx, pl-0.25);
addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, 1, tx, pl-0.25);
addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, 1, tx, pl);
addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1, tx, pl);
// 0th floor (exact AP position)
addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, 0, tx, pl);
addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, 0, tx, pl);
addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, 0, tx, pl);
addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, 0, tx, pl);
addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, 0, tx, pl);
addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, 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, 3); // id:16
addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, 2); // id:18
addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, 1); // id:11
addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, 0); // id:20
addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, 2); // idis switchboard
addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, 2); // idis outside
addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, 2); // idis toni
addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, 1); // i.1.47
addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, 1); // fachschaft
addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, 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

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

@@ -0,0 +1,44 @@
#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 */
MACAddress mac;
/** the AP's readable SSID */
std::string ssid;
double tx;
/** path loss for this ap. for testing */
double 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

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

@@ -0,0 +1,120 @@
#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 double waf = 7;//10.0;
const double floor_height_cm = 350;
// 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 double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0;
// floor difference?
const double floorDist = std::abs(ap->zNr - state.z_nr);
// estimate the rssi depending on above distance
const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->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); // absolute
//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI); // relative
//prob *= p;
prob += std::log(p);
}
const double lambda = 0.25; //0.12;
return lambda * exp(- lambda * (-prob));
//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 "../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()

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

@@ -0,0 +1,51 @@
#ifndef STEPEVALUATION_H
#define STEPEVALUATION_H
#include "../particles/MyState.h"
#include "StepObservation.h"
#include <math.h>
static double mu_walk = 40;
static double sigma_walk = 15;
static double mu_stop = 0;
static double sigma_stop = 5;
class StepEvaluation {
public:
double getProbability(const MyState& state, const StepObservation* obs) const {
double distance = state.distanceWalkedCM;
double a = 1.0;
double mu_distance = 0; //cm
double sigma_distance = 10.0; //cm
if(obs->step) {
a = 1.0;
mu_distance = mu_walk;//80.0; //cm
sigma_distance = sigma_walk;//40.0; //cm
}
else {
a = 0.0;
mu_distance = mu_stop; //cm
sigma_distance = sigma_stop; //cm
}
//Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
(1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
return p;
}
};
#endif // STEPEVALUATION_H

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

@@ -0,0 +1,14 @@
#ifndef STEPOBSERVATION_H
#define STEPOBSERVATION_H
struct StepObservation {
float ts;
bool step;
StepObservation() {;}
StepObservation(const float ts) : ts(ts), step(false){;}
};
#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()

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

@@ -0,0 +1,76 @@
#ifndef TURNEVALUATION_H
#define TURNEVALUATION_H
#include "../particles/MyState.h"
#include "TurnObservation.h"
#include <boost/math/special_functions/bessel.hpp>
#include <math.h>
static double sigma_heading = 35;
class TurnEvaluation {
//All calculations use degree not rad!!!
public:
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
//Particle's heading change
double delta_heading_particle = state.heading - state.heading_old;
//Correct offset of the heading change
if (delta_heading_particle < -180) {
delta_heading_particle += 360;
}
else if (delta_heading_particle > 180) {
delta_heading_particle -= 360;
}
//Switch between simple and improved evaluation
//"Simple" only evaluates the deviation between the measured heading and the particle heading change using
//normal distribution
if(simple) {
double sigma_delta_heading = sigma_heading;
const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
return p;
}
//use the von Mises distribution
else {
//Here some calculations must be done in rad
double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0;
double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0;
//Equation for estimating kappa value of von Mises distribution
//empirically estimated
double kappa = 0.0;
kappa = 5.0 / exp(2 * delta_motion_rad);
double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0;
//pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution)
const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa));
return p;
}
}
};
#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(delta_motion) {;}
};
#endif // TURNOBSERVATION_H

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

@@ -0,0 +1,36 @@
#ifndef TURNREADER_H
#define TURNREADER_H
#include "../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

58
code/main.cpp Normal file
View File

@@ -0,0 +1,58 @@
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>
#include "Vis.h"
namespace Settings {
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
const int gridSize_cm = 200;
}
struct MyNode : public GridNode, public GridPoint {
public:
MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
};
int align(const int val) {
return val / Settings::gridSize_cm * Settings::gridSize_cm;
}
int main(void) {
Grid<MyNode> grid(Settings::gridSize_cm);
GridFactory<MyNode> gridFac(grid);
FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222);
Floor f0 = fpFac.getFloor("floor_0");
Floor f1 = fpFac.getFloor("floor_1");
Floor f2 = fpFac.getFloor("floor_2");
Floor f3 = fpFac.getFloor("floor_3");
Stairs f01 = fpFac.getStairs("staircase_0_1");
Stairs f12 = fpFac.getStairs("staircase_1_2");
Stairs f23 = fpFac.getStairs("staircase_2_3");
const LengthF h0 = LengthF::cm(align(0));
const LengthF h1 = LengthF::cm(align(360));
const LengthF h2 = LengthF::cm(align(360+340));
const LengthF h3 = LengthF::cm(align(360+340+340));
gridFac.addFloor(f0, h0.cm());
gridFac.addFloor(f1, h1.cm());
gridFac.addFloor(f2, h2.cm());
gridFac.addFloor(f3, h3.cm());
//gridFac.removeIsolated();
Vis vis;
vis.addFloor(f0, h0).addFloor(f1, h1).addFloor(f2, h2).addFloor(f3, h3);
vis.addGrid(grid);
vis.show();
sleep(1000);
return 0;
}

4645
code/plan.svg Executable file
View File

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

After

Width:  |  Height:  |  Size: 223 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

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

@@ -0,0 +1,51 @@
#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() {
char delim;
SensorEntryStep entry;
fp >> entry.ts;
int i = 0;
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

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

@@ -0,0 +1,58 @@
#pragma once
#include "../particles/MyState.h"
#include "BarometerObservation.h"
#include "barometric.h"
#include <KLib/math/distribution/Normal.h>
double g_BarometerObservation = 0.0;
class BarometerEvaluation {
public:
double getProbability(const MyState& state, const BarometerObservation* obs) const {
//rho_z
double barometerSigma = 0.3;
//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/2, 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) {
;}
};

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

@@ -0,0 +1,91 @@
#pragma once
#include "circular.h"
#include "BarometerObservation.h"
#include "../SensorReader.h"
#include <sstream>
//circular_buffer<double> measurementHistory(1000);
class BarometerSensorReader{
private:
circular_buffer<double> measurementHistory;
public:
BarometerSensorReader(){
if(!USE_STATIC_CIRCULAR_BUFFERING){
//8.33min
measurementHistory.reserve(10000);
}
else{
//30 * 500ms = 1,5s
measurementHistory.reserve(30);
}
}
BarometerObservation* readBarometer(const SensorEntry& se) {
std::string tmp = se.data;
BarometerObservation* obs = new BarometerObservation();
//Read the hPa
double hPa = stod(tmp);
// load the measurement at current time into the history
double currentMeasurement = hPa - measurementHistory[0];
if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){
//smoothing with alpha value
if(measurementHistory.size() > 1){
double alpha = 0.1;
double lastMeasurement = measurementHistory[measurementHistory.size() - 1];
currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement);
obs->hpa = currentMeasurement;
}else{
obs->hpa = 0;
}
measurementHistory.push_back(currentMeasurement);
}
else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){
currentMeasurement = hPa;
measurementHistory.push_back(currentMeasurement);
// calculate the relative air pressure by getting the mean of the first and last three entrys of the history
// and subtract them.
if (measurementHistory.size() > 5){
double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0;
double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0;
obs->hpa = meanHead - meanTail;
}
else{
obs->hpa = 0;
}
}
else //no data smoothing
{
measurementHistory.push_back(currentMeasurement);
obs->hpa = currentMeasurement;
}
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

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

@@ -0,0 +1,109 @@
#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