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

added competition code

Browse Source
This commit is contained in:
FrankE
2016-08-29 19:09:54 +02:00
parent 7e1d001fd4
commit 087953df2e
14 changed files with 2077 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
competition/src/competition/APParams.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef APPARAMS_H
#define APPARAMS_H
struct APParams {
float x;
float y;
float z;
float txp;
float exp;
float waf;
Point3 getPos() const {return Point3(x,y,z);}
APParams() {;}
APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}
};
#endif // APPARAMS_H

84
competition/src/competition/CMakeLists.txt Executable file
View File

@@ -0,0 +1,84 @@
# 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(Competition)
IF(NOT CMAKE_BUILD_TYPE)
MESSAGE(STATUS "No build type selected. Default to Debug")
SET(CMAKE_BUILD_TYPE "Debug")
ENDIF()
INCLUDE_DIRECTORIES(
../
)
FILE(GLOB HEADERS
./notes.txt
./*.h
./*/*.h
./*/*/*.h
./*/*/*/*.h
./*/*/*/*/*.h
./*/*/*/*/*/*.h
)
FILE(GLOB SOURCES
./*.cpp
./*/*.cpp
./*/*/*.cpp
./*/*/*/*.cpp
../Indoor/lib/tinyxml/tinyxml2.cpp
)
# system specific compiler flags
ADD_DEFINITIONS(
-std=gnu++11
-Wall
-Werror=return-type
-Wextra
-Wpedantic
-fstack-protector-all
-g3
-O2
-march=native
-DWITH_TESTS
-DWITH_ASSERTIONS
-DWITH_DEBUG_LOG
)
# 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})

301
competition/src/competition/FileReader.h Normal file
View File

@@ -0,0 +1,301 @@
#ifndef FILEREADER_H
#define FILEREADER_H
#include <fstream>
#include <Indoor/Exception.h>
#include <vector>
#include <Indoor/math/Interpolator.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/pressure/BarometerData.h>
#include <Indoor/sensors/imu/AccelerometerData.h>
#include <Indoor/sensors/imu/GyroscopeData.h>
#include <unordered_map>
#include "Structs.h"
class FileReader {
public:
template <typename T> struct TS {
const float ts;
const float tsSens;
T data;
TS(const float ts) : ts(ts) {;}
TS(const float ts, const float tsSens, const T& data) : ts(ts), tsSens(tsSens), data(data) {;}
};
struct Position {
double counter;
double lat;
double lon;
float floorNr;
int building;
Position() {;}
Position(const int counter, const double lat, const double lon, const float floorNr, const int building) :
counter(counter), lat(lat), lon(lon), floorNr(floorNr), building(building) {;}
Position operator + (const Position& o) const {
return (Position(counter+o.counter, lat+o.lat, lon+o.lon, floorNr+o.floorNr, building+o.building));
}
Position operator - (const Position& o) const {
return (Position(counter-o.counter, lat-o.lat, lon-o.lon, floorNr-o.floorNr, building-o.building));
}
Position operator * (const float v) const {
return Position(counter*v, lat*v, lon*v, floorNr*v, building*v);
}
};
// struct WiFiPos {
// APMeasurement wifi;
// Position pos;
// WiFiPos(APMeasurement wifi, Position pos) : wifi(wifi), pos(pos) {;}
// };
enum class Sensor {
ACC,
GYRO,
WIFI,
GPS,
POS,
BARO,
};
struct Entry {
Sensor type;
float ts;
int idx;
Entry(Sensor type, float ts, int idx) : type(type), ts(ts), idx(idx) {;}
};
std::vector<TS<Position>> positions;
std::vector<TS<WiFiMeasurements>> wifi;
std::vector<TS<AccelerometerData>> acc;
std::vector<TS<GyroscopeData>> gyro;
std::vector<TS<GPS>> gps;
std::vector<TS<BarometerData>> barometer;
/** ALL entries */
std::vector<Entry> entries;
public:
FileReader(const std::string& file) {
parse(file);
}
const std::vector<Entry>& getEntries() const {return entries;}
const std::vector<TS<Position>>& getPositions() const {return positions;}
const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
const std::vector<TS<GPS>>& getGPS() const {return gps;}
const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
const Interpolator<float, Position> getPath() const {
Interpolator<float, Position> interpol;
for (const TS<Position>& pos : positions) {
interpol.add(pos.ts, pos.data);
}
return interpol;
}
private:
void parse(const std::string& file) {
std::ifstream inp(file);
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
while(!inp.eof()) {
char line[2048];
inp.getline(line, 2048);
parseLine(line);
}
inp.close();
}
void parseLine(const std::string& line) {
if (line.size() == 0) {return;} // empty lines
if (line[0] == '%') {return;} // comment lines
// mode
const std::string what = line.substr(0, 4);
// app timestamp
const int pos1 = line.find(';', 5);
const std::string sAppTS = line.substr(5, pos1-5);
const float appTS = std::stof(sAppTS);
// sensor timestamp
const int pos2 = line.find(';', pos1+1);
const std::string sSensTS = line.substr(pos1+1, pos2-pos1-1);
const float sensTS = std::stof(sSensTS);
// data part
const std::string data = line.substr(pos2+1);
int i = 0; (void) i;
if (what == "WIFI") {parseWiFi(appTS, sensTS, data);}
else if (what == "POSI") {parsePosition(appTS, sensTS, data);}
else if (what == "ACCE") {parseAccelerometer(appTS, sensTS, data);}
else if (what == "GYRO") {parseGyroscope(appTS, sensTS, data);}
else if (what == "GNSS") {parseGPS(appTS, sensTS, data);}
else if (what == "PRES") {parseBarometer(appTS, sensTS, data);}
}
/** split the given string */
std::vector<std::string> split(const std::string& data, const char delim = ';') {
std::vector<std::string> res;
int s = 0;
for (int i = 0; i < (int) data.length(); ++i) {
if (data[i] == delim) {
res.push_back(data.substr(s, i-s));
s = i+1;
}
}
if (s != (int) data.length() - 1) {
res.push_back(data.substr(s));
}
return res;
}
void parseGPS(const float appTS, const float sensTS, const std::string& data) {
(void) sensTS;
const std::vector<std::string> arg = split(data);
const GPS gps(Timestamp::fromSec(appTS), std::stof(arg[0]), std::stof(arg[1]), std::stof(arg[2]), std::stof(arg[4]) ); // skip [3]: bearing
this->gps.push_back(TS<GPS>(appTS, sensTS, gps));
entries.push_back(Entry(Sensor::GPS, appTS, this->gps.size()-1));
}
void parseAccelerometer(const float appTS, const float sensTS, const std::string& data) {
(void) sensTS;
const int pos1 = data.find(';');
const int pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
TS<AccelerometerData> elem(appTS, sensTS, AccelerometerData(std::stof(x), std::stof(y), std::stof(z)));
acc.push_back(elem);
entries.push_back(Entry(Sensor::ACC, appTS, acc.size()-1));
}
void parseGyroscope(const float appTS, const float sensTS, const std::string& data) {
(void) sensTS;
const int pos1 = data.find(';');
const int pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
TS<GyroscopeData> elem(appTS, sensTS, GyroscopeData(std::stof(x), std::stof(y), std::stof(z)));
gyro.push_back(elem);
entries.push_back(Entry(Sensor::GYRO, appTS, gyro.size()-1));
}
void parseWiFi(const float appTS, const float sensTS, const std::string& data) {
(void) sensTS;
const int pos1 = data.find(';');
const int pos2 = data.find(';', pos1+1);
//const int pos3 = data.find(';', pos2+1);
const std::string ssid = data.substr(0, pos1);
const std::string mac = data.substr(pos1+1, pos2-pos1-1);
const std::string rssi = data.substr(pos2+1);
// belongs to new scan or current scan?
if (wifi.empty() || wifi.back().ts != appTS) {
// create a new empty scan-entry
wifi.push_back(TS<WiFiMeasurements>(appTS, appTS, WiFiMeasurements()));
entries.push_back(Entry(Sensor::WIFI, appTS, wifi.size()-1));
}
// append AP to current scan-entry
wifi.back().data.entries.push_back(WiFiMeasurement(AccessPoint(mac, ssid), std::stoi(rssi), Timestamp::fromSec(appTS)));
}
void parsePosition(const float appTS, const int counter, const std::string& data) {
const int pos1 = data.find(';');
const int pos2 = data.find(';', pos1+1);
const int pos3 = data.find(';', pos2+1);
const std::string lat = data.substr(0, pos1);
const std::string lon = data.substr(pos1+1, pos2-pos1-1);
const std::string floor = data.substr(pos2+1, pos3-pos2-1);
const std::string building = data.substr(pos3+1);
TS<Position> elem(appTS, appTS, Position(counter, std::stod(lat), std::stod(lon), std::stoi(floor), std::stoi(building)));
positions.push_back(elem);
}
void parseBarometer(const float appTS, const float sensTS, const std::string& data) {
const int pos1 = data.find(';');
const std::string hPa = data.substr(0, pos1);
TS<BarometerData> elem(appTS, sensTS, BarometerData(std::stof(hPa)));
barometer.push_back(elem);
entries.push_back(Entry(Sensor::BARO, appTS, barometer.size()-1));
}
};
#endif // FILEREADER_H

10
competition/src/competition/Optimize.h Normal file
View File

@@ -0,0 +1,10 @@
#ifndef OPTIMIZE_H
#define OPTIMIZE_H
#include <Indoor/floorplan/v2/Floorplan.h>
class Optimize {
};
#endif // OPTIMIZE_H

344
competition/src/competition/Optimizer.h Normal file
View File

@@ -0,0 +1,344 @@
#ifndef OPTIMIZER_H
#define OPTIMIZER_H
#include "Structs.h"
#include "APParams.h"
#include "Scaler.h"
#include "FileReader.h"
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
#include <Indoor/sensors/radio/VAPGrouper.h>
#include <Indoor/geo/BBox3.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
#include <KLib/math/optimization/NumOptAlgoGenetic.h>
#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>
template <typename Scalar> class Outlier {
private:
std::vector<Scalar> data;
public:
/** add the given scalar */
void add(const Scalar& s) {
auto it = std::lower_bound(data.begin(), data.end(), s);
data.insert(it, s);
}
/** get the sum of all values between start-percent, and end-percent */
Scalar getSum(const float pStart, const float pEnd) {
const int i1 = (data.size()) * pStart;
const int i2 = (data.size()) * pEnd;
Scalar sum = 0;
for (int i = i1; i < i2; ++i) {sum += data[i];}
return sum;
}
/** get the average of all values between start-percent, and end-percent */
Scalar getAvg(const float pStart, const float pEnd) {
const int i1 = (data.size()) * pStart;
const int i2 = (data.size()) * pEnd;
const int cnt = i2-i1+1;
return getSum(pStart, pEnd) / cnt;
}
Outlier operator += (const Scalar& s) {add(s); return *this;}
};
struct Optimizer {
public:
Floorplan::IndoorMap* map;
Scaler scaler;
std::vector<FileReader> records;
// /** debug only */
// struct WiFiEntry {
// std::string mac;
// int rssi;
// WiFiEntry(const std::string& mac, const int rssi) : mac(mac), rssi(rssi) {;}
// };
// /** group entries by mac+timestamp */
// struct WiFiEntries {
// FileReader::Position pos; // ground-truth position at time of scan
// std::vector<WiFiEntry> debug; // all VAP entries at this time
// int rssiSum; // sum of all VAP rssis
// int cnt; // number of all VAP rssis
// float getRSSI() const {return (float) rssiSum / (float) cnt;}
// };
/** group a wifi-scan with the best-matching ground-truth position */
struct WiFiOptBase {
WiFiMeasurement apScan;
FileReader::Position pos;
WiFiOptBase(const WiFiMeasurement& apScan, const FileReader::Position pos) : apScan(apScan), pos(pos) {;}
};
/** group entries by mac (faster access) */
std::unordered_map<MACAddress, std::vector<WiFiOptBase>> wifiMap;
// /** group entries by mac+timestamp */
// std::unordered_map<MACAddress, std::unordered_map<float, WiFiEntries>> wifiMap;
//std::vector<WiFiMeasurementsAtGroundTruthPosition> wifiPos;
public:
Optimizer(Floorplan::IndoorMap* map, Scaler scaler) : map(map), scaler(scaler) {;}
/** add the given record */
void addRecord(const std::string& file) {
records.push_back(FileReader(file));
buildWiFiMap();
}
/** add all of the given records */
void addRecords(const std::vector<std::string>& files) {
for (const std::string& file : files) {
addRecord(file);
}
}
// std::vector<std::string> getAllMACs() {
// std::unordered_set<std::string> macs;
// for (const FileReader& record : records) {
// for (const FileReader::TS<FileReader::WiFi>& scan : record.getWiFi()) {
// macs.insert(scan.data.mac);
// }
// }
// auto comp = [] (const std::string& a, const std::string& b) {return a < b;};
// std::vector<std::string> sorted;
// for (std::string mac : macs) {sorted.push_back(mac);}
// std::sort(sorted.begin(), sorted.end(), comp);
// return sorted;
// }
std::vector<MACAddress> getAllMACs() {
//buildWiFiMap();
std::vector<MACAddress> res;
for (auto it : wifiMap) {res.push_back(it.first);}
return res;
// for (const WiFiMeasurementsAtGroundTruthPosition& e : wifiPos) {
// for (const WiFiMeasurement& m : e.measurements.entries) {
// res.insert(m.getAP().getMAC());
// }
// }
// std::vector<MACAddress> vec;
// vec.insert(vec.end(), res.begin(), res.end());
// return vec;
}
void buildWiFiMap() {
wifiMap.clear();
// how to combine VAPs
const VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM);
// parse each training data file
for (const FileReader& record : records) {
// ground-truth interpolation for the given record-data-set
const Interpolator<float, FileReader::Position> path = record.getPath();
// process each wifi-scan within the record-data-set
for (const FileReader::TS<WiFiMeasurements>& scan : record.getWiFiGroupedByTime()) {
for (const WiFiMeasurement& m : scan.data.entries) {
std::cout << m.getAP().getMAC().asString() << ":" << m.getRSSI() << std::endl;
}
std::cout << "----------------------------------" << std::endl;
// group all VAPs within one scan
const WiFiMeasurements vapGrouped = vg.group(scan.data);
for (const WiFiMeasurement& m : vapGrouped.entries) {
std::cout << m.getAP().getMAC().asString() << ":" << m.getRSSI() << std::endl;
}
// add entries grouped by MAC (faster access for the optimizer)
for (const WiFiMeasurement& m : vapGrouped.entries) {
// ombine scanned AP with ground-truth position during scan and add to the map
const FileReader::Position pos = path.get(m.getTimestamp());
wifiMap[m.getAP().getMAC()].push_back( WiFiOptBase(m, pos) );
}
}
}
}
// optimize the given AP
APParams optimize(const MACAddress& mac, float& errResult) {
// starting parameters do not matter for the current optimizer!
APParams params(0,0,0, -40, 2.5, 0.0);
// // construct vector of all readings: signal-strength -> position
// std::vector<FileReader::WiFiPos> readings;
// for (const FileReader& record : records) {
// const std::vector<FileReader::WiFiPos> tmp = record.getWiFiPos(mac);
// readings.insert(readings.end(), tmp.begin(), tmp.end());
// }
//std::cout << "use different errors: diff diff² diff³ and compare the result?" << std::endl;
const float hugeError = 1e10;
auto func = [&] (const float* data) {
const APParams* params = (APParams*) data;
if (params->waf > 0) {return hugeError;}
if (params->txp < -50) {return hugeError;}
if (params->txp > -30) {return hugeError;}
if (params->exp > 4) {return hugeError;}
if (params->exp < 1) {return hugeError;}
// current position guess for the AP;
const Point3 apPos(params->x, params->y, params->z);
// signal-strength-prediction-model...
WiFiModelLogDistCeiling model(map);
// ... with the current parameter guess: position, txp, exp, waf
model.addAP( mac, WiFiModelLogDistCeiling::APEntry(apPos, params->txp, params->exp, params->waf));
//Outlier<float> out;
float err = 0;
int cnt = 0;
// fetch all measurements (with ground-truth) available for THE REQUESTED AP
const std::vector<WiFiOptBase>& entries = wifiMap[mac];
// process each measurement
for (const WiFiOptBase& reading : entries) {
// get the corresponding ground truth ESTIMATION
const FileReader::Position worldPos = reading.pos;
// convert it from world(lat,lon) to map(x,y)
//const float fixedFloorHeight = 4.0;
const Point3 mapPos = scaler.convert3D(worldPos.lat, worldPos.lon, worldPos.floorNr);
//const float z = fixedFloorHeight * worldPos.floor;
//const Point3 mapPos = Point3(mapPos2.x, mapPos2.y, z); // TODO! z coordinate
// model estimation for the AP
const float rssiModel = model.getRSSI(mac, mapPos);
// difference between guess and measurement?
const float diff = std::abs(rssiModel - reading.apScan.getRSSI());
// append
err += diff*diff;
++cnt;
// max distance penality
if (apPos.getDistance(mapPos) > 120) { // unlikely!
err += 999999;
}
}
//float err1 = out.getAvg(0.0, 1.0);
//float err = std::sqrt(out.getAvg(0.0, 0.99));
err /= cnt;
err = std::sqrt(err);
if (params->txp < -50) {err += 999999;}
if (params->txp > -35) {err += 999999;}
if (params->exp > 3.5) {err += 999999;}
if (params->exp < 1.0) {err += 999999;}
// if (params->x < -100) {err += 999999;}
// if (params->y < -100) {err += 999999;}
return err;
};
//
const BBox3 mapBBox = FloorplanHelper::getBBox(map);
using LeOpt = K::NumOptAlgoRangeRandom<float>;
const std::vector<LeOpt::MinMax> valRegion = {
LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20), // x
LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20), // y
LeOpt::MinMax(mapBBox.getMin().z - 5, mapBBox.getMax().z + 5), // z
LeOpt::MinMax(-50,-30), // txp
LeOpt::MinMax(1,3), // exp
LeOpt::MinMax(-10,-4), // waf
};
std::cout << "use more rounds for production" << std::endl;
LeOpt opt(valRegion);
opt.setPopulationSize(250); // USE MORE FOR PRODUCTION
opt.setNumIerations(40);
opt.calculateOptimum(func, (float*) &params);
// using LeOpt = K::NumOptAlgoGenetic<float>;
// LeOpt opt(6);
// opt.setPopulationSize(750);
// opt.setMaxIterations(50);
// opt.setElitism(0.05f);
// opt.setMutation(0.75f);
// //opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
// opt.setValRegion(valRegion);
// K::NumOptAlgoDownhillSimplex<float, 6> opt;
// opt.setMaxIterations(100);
// opt.setNumRestarts(10);
opt.calculateOptimum(func, (float*) &params);
std::cout << params.x << "," << params.y << "," << params.z << " txp: " << params.txp << " exp: " << params.exp << " waf: " << params.waf << " err: " << func((float*)&params) << "dB" << std::endl;
errResult = func((float*)&params);
return params;
}
};
#endif // OPTIMIZER_H

213
competition/src/competition/Plotti.h Normal file
View File

@@ -0,0 +1,213 @@
#ifndef PLOTTI_H
#define PLOTTI_H
#include "filter/Structs.h"
#include <functional>
#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
struct Plotti {
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementPoints pGrid;
K::GnuplotSplotElementLines pFloor;
K::GnuplotSplotElementLines pOutline;
K::GnuplotSplotElementLines pStairs;
K::GnuplotSplotElementPoints pAPs;
K::GnuplotSplotElementPoints pInterest;
K::GnuplotSplotElementPoints pParticles;
K::GnuplotSplotElementColorPoints pColorPoints;
K::GnuplotSplotElementLines gtPath;
Plotti() {
gp << "set xrange[0-50:70+50]\nset yrange[0-50:50+50]\nset ticslevel 0\n";
splot.add(&pGrid); pGrid.setPointSize(0.25); pGrid.setColorHex("#888888");
splot.add(&pAPs); pAPs.setPointSize(0.7);
splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
splot.add(&pParticles); pParticles.setColorHex("#0000ff"); pParticles.setPointSize(0.4f);
splot.add(&pFloor);
splot.add(&pOutline); pOutline.setColorHex("#999999");
splot.add(&pStairs); pStairs.setColorHex("#000000");
splot.add(&pInterest); pInterest.setPointSize(2); pInterest.setColorHex("#ff0000");
splot.add(&gtPath); gtPath.setLineWidth(2); gtPath.setColorHex("#000000");
}
void addLabel(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "'\n";
}
void addLabelV(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "' rotate by 90\n";
}
void showAngle(const int idx, const float rad) {
Point2 cen(0.9, 0.9);
Point2 rot(0, 1);
Point2 pos = cen + rot.rotated(rad) * 0.05;
gp << "set arrow "<<idx<<" from screen " << cen.x << "," << cen.y << " to screen " << pos.x << "," << pos.y << "\n";
}
void setEst(const Point3 pos) {
gp << "set arrow 991 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+1 << " nohead lw 1 front \n";
}
void setGT(const Point3 pos) {
gp << "set arrow 995 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+0.3 << " nohead lw 3 front \n";
gp << "set arrow 996 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x+0.3 << "," << pos.y << "," << pos.z << " nohead lw 3 front \n";
}
void addGroundTruthNode(const Point3 pos) {
K::GnuplotPoint3 gp(pos.x, pos.y, pos.z);
gtPath.add(gp);
}
// void debugWiFi(const WiFiModelX& model, const APScan& scan, const float curTS) {
// pColorPoints.clear();
// const float step = 2.0;
// float z = 0;
// for (float x = -20; x < 90; x += step) {
// for (float y = -10; y < 60; y += step) {
// double prob = model.getProbability(scan, Point3(x,y,z), curTS);
// pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
// }
// }
// }
template <typename Node> void debugProb(Grid<Node>& grid, std::function<double(const MyObs&, const Point3& pos)> func, const MyObs& obs) {
pColorPoints.clear();
// const float step = 2.0;
// float z = 0;
// for (float x = -20; x < 90; x += step) {
// for (float y = -10; y < 60; y += step) {
// const Point3 pos_m(x,y,z);
// const double prob = func(obs, pos_m);
// pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
// }
// }
std::minstd_rand gen;
std::uniform_int_distribution<int> dist(0, grid.getNumNodes()-1);
float min = +9999;
float max = -9999;
for (int i = 0; i < 10000; ++i) {
int idx = dist(gen);
Node& n = grid[idx];
const Point3 pos_cm(n.x_cm, n.y_cm, n.z_cm);
const Point3 pos_m = pos_cm / 100.0f;
const double prob = func(obs, pos_m);
if (prob < min) {min = prob;}
if (prob > max) {max = prob;}
pColorPoints.add(K::GnuplotPoint3(pos_m.x, pos_m.y, pos_m.z), prob);
}
if (min == max) {min -= 1;}
gp << "set cbrange [" << min << ":" << max << "]\n";
}
void addStairs(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::Stair* stair : f->stairs) {
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), f);
for (const Floorplan::Quad3& quad : quads) {
for (int i = 0; i < 4; ++i) {
int idx1 = i;
int idx2 = (i+1) % 4;
pStairs.addSegment(
K::GnuplotPoint3(quad[idx1].x,quad[idx1].y, quad[idx1].z),
K::GnuplotPoint3(quad[idx2].x,quad[idx2].y, quad[idx2].z)
);
}
}
}
}
}
void addFloors(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::FloorObstacle* obs : f->obstacles) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line) {
K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight);
K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight);
pFloor.addSegment(p1, p2);
}
}
}
}
void addOutline(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::FloorOutlinePolygon* poly : f->outline) {
const int cnt = poly->poly.points.size();
for (int i = 0; i < cnt; ++i) {
Point2 p1 = poly->poly.points[(i+0)];
Point2 p2 = poly->poly.points[(i+1)%cnt];
K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
pOutline.addSegment(gp1, gp2);
}
}
}
}
template <typename Node> void addGrid(Grid<Node>& grid) {
pGrid.clear();
for (const Node& n : grid) {
K::GnuplotPoint3 p(n.x_cm, n.y_cm, n.z_cm);
pGrid.add(p/100.0f);
}
}
template <typename State> void addParticles(const std::vector<K::Particle<State>>& particles) {
pParticles.clear();
int i = 0;
for (const K::Particle<State>& p : particles) {
if (++i % 25 != 0) {continue;}
K::GnuplotPoint3 pos(p.state.pos.x_cm, p.state.pos.y_cm, p.state.pos.z_cm);
pParticles.add(pos / 100.0f);
}
}
void show() {
gp.draw(splot);
gp.flush();
}
};
#endif // PLOTTI_H

63
competition/src/competition/Scaler.h Normal file
View File

@@ -0,0 +1,63 @@
#ifndef SCALER_H
#define SCALER_H
#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>
class Scaler {
private:
int w;
int h;
double cenLat;
double cenLon;
double rotDeg;
double mPerPx;
public:
Scaler(int w, int h, double cenLat, double cenLon, double rotDeg, double mPerPx) : w(w), h(h), cenLat(cenLat), cenLon(cenLon), rotDeg(rotDeg), mPerPx(mPerPx) {
;
}
Point3 convert3D(double lat, double lon, float floorNr) const {
Point2 p2 = convert2D(lat, lon);
const float fixedFloorHeight = 4.0f;
return Point3(p2.x, p2.y, floorNr * fixedFloorHeight);
}
Point2 convert2D(double lat, double lon) const {
const double refLat = cenLat / 180.0 * M_PI;
const double m_per_deg_lat = 111132.954 - 559.822 * std::cos( 2.0 * refLat ) + 1.175 * std::cos( 4.0 * refLat);
const double m_per_deg_lon = 111132.954 * std::cos ( refLat );
const double y_m = +(lat-cenLat) * m_per_deg_lat;
const double x_m = +(lon-cenLon) * m_per_deg_lon;
Point2 pos(x_m, y_m);
pos = pos.rotated(rotDeg / 180 * M_PI);
const Point2 center( (w*mPerPx/2), (h*mPerPx/2) );
pos += center;
return pos;
}
};
#endif // SCALER_H

87
competition/src/competition/Structs.h Normal file
View File

@@ -0,0 +1,87 @@
#ifndef STRUCTS_H
#define STRUCTS_H
#include <string>
#include <unordered_map>
#include <vector>
#include <cmath>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/imu/AccelerometerData.h>
#include <Indoor/sensors/imu/GyroscopeData.h>
///** one detected accecss point */
//struct APMeasurement {
// std::string ssid;
// std::string mac;
// float rssi;
// APMeasurement() {;}
// APMeasurement(const std::string& ssid, const std::string& mac, const float rssi) : ssid(ssid), mac(mac), rssi(rssi) {;}
//};
///** multiple detected access points */
//struct APScan {
// Timestamp ts;
// APScan() {;}
// APScan(const float tsSec) : ts(Timestamp::fromSec(tsSec)) {;}
// std::vector<APMeasurement> entries;
// struct TMP {float rssiSum; int cnt;};
// void groupVAPs() {
// std::unordered_map<std::string, TMP> map;
// for (const APMeasurement& entry : entries) {
// map[VAP::macToVAP(entry.mac)].rssiSum += entry.rssi;
// map[VAP::macToVAP(entry.mac)].cnt += 1;
// }
// entries.clear();
// for (auto it : map) {
// entries.push_back(APMeasurement("", it.first, it.second.rssiSum/it.second.cnt));
// }
// }
//};
//struct Accelerometer {
// float x;
// float y;
// float z;
// Accelerometer(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
// float getMag() const {return std::sqrt(x*x + y*y + z*z);}
//};
//struct Gyroscope {
// float x;
// float y;
// float z;
// Gyroscope(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
//};
struct GPS {
Timestamp ts;
float lat; // deg
float lon; // deg
float alt; // m
float accuracy; // m [might be NAN]
float speed; // m/s [might be NAN]
GPS() : ts(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
GPS(const Timestamp ts, const float lat, const float lon, const float alt) : ts(ts), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN) {;}
GPS(const Timestamp ts, const float lat, const float lon, const float alt, const float accuracy) : ts(ts), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN) {;}
};
#endif // STRUCTS_H

212
competition/src/competition/filter/Logic.h Normal file
View File

@@ -0,0 +1,212 @@
#ifndef FLOGIC_H
#define FLOGIC_H
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/GridWalkerMulti.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleRelativePressureControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/factory/v2/GridNodeImportance.h>
#include "WiFi.h"
#include "../Scaler.h"
#include "Structs.h"
struct MyWalkState : public WalkState, public WalkStateHeading, public WalkStateRelativePressure {
MyWalkState(const GridPoint& gp, const Heading head, const float relativePressure) : WalkState(gp), WalkStateHeading(head), WalkStateRelativePressure(relativePressure) {;}
};
struct MyNode : public GridPoint, public GridNode, public GridNodeImportance {
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
};
//class WalkModuleWiFi : public WalkModule<MyNode, MyWalkState> {
//public:
// WiFiModelX model;
// MyObs* obs = nullptr;
// WalkModuleWiFi() {
// model.read(wifiModelFile);
// }
// /** update the given WalkState before starting the walk. e.g. based on sensor readings */
// virtual void updateBefore(MyWalkState& state) override {
// }
// /** get the probability p(e) from curNode to potentialNode */
// virtual double getProbability(const MyWalkState& state, const MyNode& startNode, const MyNode& curNode, const MyNode& potentialNode) const override {
// const Point3 pos = potentialNode.inMeter();
// const double prob = model.getProbability(obs->wifis, pos, Timestamp::fromSec(obs->curTS));
// return prob;
// //return 1;
// }
// /** one step (edge) is taken */
// virtual void step(MyWalkState& state, const MyNode& curNode, const MyNode& nextNode) override {
// }
// /** update the walk state based on the given transition (if any update is necssary) */
// virtual void updateAfter(MyWalkState& state, const MyNode& startNode, const MyNode& endNode) override {
// }
//};
/** particle-filter init */
struct PFInit : public K::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
PFInit(Grid<MyNode>& grid) : grid(grid) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
for (K::Particle<MyState>& p : particles) {
int idx = rand() % grid.getNumNodes();
p.state.pos = grid[idx]; // random position
p.state.head = (rand() % 360) / 180.0 * M_PI; // random heading
p.state.relPres = 0; // start with a relative pressure of 0
}
}
};
/** particle-filter transition */
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
GridWalker<MyNode, MyWalkState> walker;
// GridWalkerMulti<MyNode, MyWalkState> walker;
WalkModuleHeading<MyNode, MyWalkState> modHeadUgly; // stupid
WalkModuleHeadingControl<MyNode, MyWalkState, MyControl> modHead;
WalkModuleNodeImportance<MyNode, MyWalkState> modImportance;
WalkModuleRelativePressureControl<MyNode, MyWalkState, MyControl> modPressure;
WalkModuleSpread<MyNode, MyWalkState> modSpread;
WalkModuleFavorZ<MyNode, MyWalkState> modFavorZ;
//WalkModuleWiFi modWifi;
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl), modPressure(ctrl, 0.100) {
walker.addModule(&modHead);
//walker.addModule(&modHeadUgly);
walker.addModule(&modImportance);
//walker.addModule(&modPressure);
//walker.addModule(&modSpread);
walker.addModule(&modFavorZ);
//walker.addModule(&modWifi);
}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
Distribution::Normal<float> distStep(1, 0.10);
float tmpSum1 = 0;
float tmpSum2 = 0;
int tmpCnt = 0;
for (K::Particle<MyState>& p : particles) {
tmpSum1 += p.state.relPres;
// transfer from state to walkstate
const MyWalkState start(grid.getNodeFor(p.state.pos), p.state.head, p.state.relPres);
const float var = distStep.draw();
const float dist = control->numSteps * var * 0.75; // 75cm + variance for every detected step
const MyWalkState end = walker.getDestination(grid, start, dist);
// transfer from walkstate to state
p.state.relPres = end.pressureRelToT0;
p.state.pos = end.startPos;
p.state.head = end.startHeading.getRAD();
tmpSum2 += p.state.relPres;
++tmpCnt;
}
std::cout << "rel pres: " << (tmpSum1/tmpCnt) << std::endl;
std::cout << "rel pres: " << (tmpSum2/tmpCnt) << std::endl;
}
};
struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
WiFiModelX model;
Scaler scaler;
PFEval(Scaler scaler, WiFiModelX wifiModel) : scaler(scaler), model(wifiModel) {
}
/** probability for WIFI */
inline double getWIFI(const MyObs& observation, const Point3 pos_m) const {
return model.getProbability(observation.wifis, pos_m, Timestamp::fromSec(observation.curTS));
}
/** probability for GPS */
inline double getGPS(const MyObs& observation, const Point3 pos_m) const {
// NaN? -> not yet valid!
if (observation.gps.lon != observation.gps.lon) {return 1;}
const float age = observation.curTS - observation.gps.ts.sec();
const float accuracy = observation.gps.accuracy*1.30f + (2.0f * age); // accuracy + 25% + 2m per second
if (age > 3) {return 1.0f;} // after 3 seconds -> unknown!
const Point2 gpsPos = scaler.convert2D(observation.gps.lat, observation.gps.lon);
const float dist_m = (pos_m.xy() - gpsPos).length();
//return Distribution::Normal<double>::getProbability(0, accuracy, dist_m);
return Distribution::Region<float>::getProbability(0, accuracy, dist_m);
}
/** probability for ALL sensors */
inline double getALL(const MyObs& observation, const Point3 pos_m) const {
const double pWifi = getWIFI(observation, pos_m);
const double pGps = getGPS(observation, pos_m);
return pWifi*pGps;
}
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
double sum = 0;
for (K::Particle<MyState>& p : particles) {
const double prob = getALL(observation, p.state.pos.inMeter());
p.weight = prob;
sum += prob;
}
return sum;
}
};
#endif // FLOGIC_H

4
competition/src/competition/filter/MyObs.h Normal file
View File

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

91
competition/src/competition/filter/Structs.h Normal file
View File

@@ -0,0 +1,91 @@
#ifndef FSTRUCTS_H
#define FSTRUCTS_H
#include <Indoor/grid/Grid.h>
#include <Indoor/math/Distributions.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
struct MyState {
static Floorplan::IndoorMap* map;
GridPoint pos; // current position
float head; // current heading
float relPres; // current pressure relative to t0
MyState() : pos(), head(0) {;}
MyState(GridPoint pos, float head) : pos(pos), head(head) {;}
MyState& operator += (const MyState& o) {
this->pos += o.pos;
this->head += o.head;
return *this;
}
MyState& operator /= (const double d) {
this->pos /= d;
this->head /= d;
return *this;
}
MyState operator * (const double d) const {
return MyState(pos*d, head*d);
}
bool belongsToRegion(const MyState& o) const {
return pos.inMeter().getDistance(o.pos.inMeter()) < 3.0;
}
MyState(const float* params) {
pos.x_cm = params[0];
pos.y_cm = params[1];
pos.z_cm = params[2];
}
void fillKernelDenstityParameters(float* params) const {
params[0] = pos.x_cm;
params[1] = pos.y_cm;
params[2] = pos.z_cm;
}
float getKernelDensityProbability(const float* params) const {
GridPoint gp(params[0], params[1], params[2]);
const float dist = gp.getDistanceInMeter(pos);
if (dist > 6.0) {return 0;}
const bool iSect = FloorplanHelper::intersectsObstacle(map, Point3(pos.x_cm,pos.y_cm,pos.z_cm)/100, Point3(params[0],params[1],params[2])/100);
if (iSect) {return 0;}
return Distribution::Normal<float>::getProbability(0, 3.25, dist);
}
};
struct MyControl {
int numSteps = 0;
float turnAngle = 0;
struct Baro {
// sensor type 1
float hPaRelativeToT0 = 0;
float estimatedSigma = 0;
// sensor type 2
float tendence = 0;
} barometer;
};
struct MyObs {
float curTS;
WiFiMeasurements wifis;
GPS gps;
};
#endif // FSTRUCTS_H

174
competition/src/competition/filter/WiFi.h Normal file
View File

@@ -0,0 +1,174 @@
#ifndef WIFI_H
#define WIFI_H
#include "../APParams.h"
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/radio/VAPGrouper.h>
#include <Indoor/math/Distributions.h>
/** WIFI Eval */
class WiFiModelX : public WiFiModel {
private:
std::unordered_map<MACAddress, APParams> aps;
std::vector<float> ceilingsAtHeight_m;
public:
void write(const std::string& file) {
std::ofstream out(file);
for (auto it : aps) {
MACAddress mac = it.first;
APParams par = it.second;
out << mac.asString() << "," << par.x << "," << par.y << "," << par.z << "," << par.txp << "," << par.exp << "," << par.waf << std::endl;
}
out.close();
}
void read(const std::string& file, Floorplan::IndoorMap* map) {
std::ifstream inp(file);
if (!inp.is_open() || inp.bad()) {throw Exception("could not open wifi params!");}
while (!inp.eof()) {
char line[2048];
inp.getline(line, 2048);
parse(line);
}
inp.close();
for (Floorplan::Floor* f : map->floors) {
ceilingsAtHeight_m.push_back(f->atHeight);
}
}
void parse(const std::string& line) {
if (line.empty()) {return;}
int pos1 = line.find(',');
int pos2 = line.find(',', pos1+1);
int pos3 = line.find(',', pos2+1);
int pos4 = line.find(',', pos3+1);
int pos5 = line.find(',', pos4+1);
int pos6 = line.find(',', pos5+1);
MACAddress mac = line.substr(0, pos1);
float x = std::stof(line.substr(pos1+1, pos2-pos1-1));
float y = std::stof(line.substr(pos2+1, pos3-pos2-1));
float z = std::stof(line.substr(pos3+1, pos4-pos3-1));
float txp = std::stof(line.substr(pos4+1, pos5-pos4-1));
float exp = std::stof(line.substr(pos5+1, pos6-pos5-1));
float waf = std::stof(line.substr(pos6+1));
addParam(mac, APParams(x,y,z, txp, exp, waf));
}
void addParam(MACAddress mac, const APParams& params) {
aps[mac] = params;
}
// /** get the given access-point's RSSI at the provided location */
// float getRSSI(const LocatedAccessPoint& ap, const Point3 p) override {
// return getRSSI(ap.getMAC(), p);
// }
float getRSSI(const MACAddress& mac, const Point3 p) const override {
auto it = aps.find(mac);
if (it == aps.end()) {throw "error!";}
APParams params = it->second;
Point3 apPos = params.getPos();
const float dist_m = apPos.getDistance(p);
float rssi1 = LogDistanceModel::distanceToRssi(params.txp, params.exp, dist_m);
float waf = params.waf * numCeilingsBetween(apPos.z, p.z);
return rssi1 + waf;
}
float rssiSigma(float rssi) const {
if (rssi > -40) {rssi = -40;}
if (rssi < -100) {rssi = -100;}
const float sigma = std::pow( ((rssi+100)/38), 4);
return sigma;
}
/** get the probability for measuring the given scan-data at the provided location */
double getProbability(const WiFiMeasurements& scan, const Point3 pos, const Timestamp curTS) const {
if (scan.entries.empty()) {return 1.0;}
VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM);
double prob = 0;
for (const WiFiMeasurement& ap : scan.entries) {
const MACAddress vapMAC = vg.getBaseMAC(ap.getAP().getMAC());
if (vapMAC != ap.getAP().getMAC()) {continue;}
// skip APs the model does not know
if (aps.find(MACAddress(vapMAC)) == aps.end()) {continue;}
const float scanRSSI = ap.getRSSI();
const float modelRSSI = getRSSI(MACAddress(vapMAC), pos);
// older entry? higher sigma!
// high signal strength? higher sigma!
const Timestamp age = curTS - ap.getTimestamp();
//const float sigma = 10.0 + 0.5 * age.sec() + rssiSigma(scanRSSI);
//double apProb = Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
const float sigma = 8.0 + 0.5 * age.sec() + rssiSigma(scanRSSI);
const double apProb = Distribution::Region<float>::getProbability(modelRSSI, sigma, scanRSSI);
prob += std::log(apProb);
}
// e.g. empty scan
if (prob == 0) {return 1;}
const double res = std::exp(prob);
//if (res == 0) {res = 1;}
//const double res = std::exp(prob/3);
//double res = std::pow(std::exp(prob), 0.25);
if (res != res) {throw "NAN";}
return res;
}
/** get the number of ceilings between z1 and z2 */
int numCeilingsBetween(const float z1, const float z2) const {
int cnt = 0;
const float zMin = std::min(z1, z2);
const float zMax = std::max(z1, z2);
for (float z : ceilingsAtHeight_m) {
if (zMin < z && zMax > z) {++cnt;}
}
return cnt;
}
};
#endif // WIFI_H

437
competition/src/competition/main.cpp Executable file
View File

@@ -0,0 +1,437 @@
#include "FileReader.h"
#include "filter/Structs.h"
#include "Plotti.h"
#include "filter/WiFi.h"
#include "filter/Logic.h"
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/geo/Point2.h>
#include <KLib/math/statistics/Statistics.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/pressure/PressureTendence.h>
#include "Optimizer.h"
#include "Scaler.h"
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationKernelDensity.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/geo/Heading.h>
const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
const std::string dataDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/src/data/";
/** describes one dataset (map, training, parameter-estimation, ...) */
struct DataSetup {
std::string map;
std::vector<std::string> training;
std::string wifiParams;
int minWifiOccurences;
Scaler scaler;
};
/** all configured datasets */
struct Data {
DataSetup CAR = {
mapDir + "CAR/CAR9.xml",
{
dataDir + "car/logfile_CAR_R1_S3.txt",
dataDir + "car/logfile_CAR_R1_S3mini.txt",
dataDir + "car/logfile_CAR_R2_S3.txt",
dataDir + "car/logfile_CAR_R2_S4.txt"
},
dataDir + "car/wifiParams.txt",
40,
Scaler (1282, 818, 40.31320308, -3.48367648, -8.77680000, 0.05207600)
};
DataSetup UAH = {
mapDir + "UAH/UAH7.xml",
{
dataDir + "uah/logfile_UAH_R1_S3.txt",
dataDir + "uah/logfile_UAH_R1_S4.txt",
dataDir + "uah/logfile_UAH_R2_S3.txt",
dataDir + "uah/logfile_UAH_R2_S4.txt",
dataDir + "uah/logfile_UAH_R4_S3.txt",
dataDir + "uah/logfile_UAH_R4_S4.txt"
},
dataDir + "uah/wifiParams.txt",
40,
Scaler (1869, 1869, 40.51312440, -3.34959080, -40.73112000, 0.07596002)
};
DataSetup UJI_TI = {
mapDir + "UJI-TI/UJI-TI4.xml",
{
dataDir + "uji-ti/logfile_UJITI_R1_NEXUS5.txt",
dataDir + "uji-ti/logfile_UJITI_R2_NEXUS5.txt",
},
dataDir + "uah/wifiParams.txt",
15,
Scaler (748, 1046, 39.99322125, -0.06862410, 29.57638723, 0.08556493)
};
} data;
/** perform wifi AP optimization */
void optimize(Floorplan::IndoorMap* map, Scaler& scaler, const int minWifiOccurences, const std::vector<std::string> trainingFiles, const std::string outFile) {
// the optimizer to determine values for each AP
Optimizer opti(map, scaler);
// all training-data-files (measurements at a known ground-truth)
opti.addRecords(trainingFiles);
// debug plotting
static Plotti plot;
plot.addFloors(map);
plot.addOutline(map);
std::vector<MACAddress> macs = opti.getAllMACs();
float errSum = 0;
int errCnt = 0;
std::string modelFile = outFile;
WiFiModelX model;
int numValid = 0;
int numInvalid = 0;
// optimize each AP (or VAP-Group))
for (const MACAddress& mac : macs) {
std::cout << "NEXT: " << mac.asString() << std::endl;
std::cout << "HAS " << opti.wifiMap[mac].size() << " MEASUREMENTS" << std::endl;
// if we have only a few measurements, we can NOT determine the APs position correctly -> skip this one completely
if (opti.wifiMap[mac].size() < minWifiOccurences) {
std::cout << "NOT ENOUGH MEASUREMENTS! SKIPPING!" << std::endl;
++numInvalid;
continue;
}
float curErr;
APParams params = opti.optimize(mac, curErr);
plot.pAPs.add(K::GnuplotPoint3(params.x, params.y, params.z));
plot.addLabelV(9000 + numValid, Point3(params.x, params.y, params.z+1), mac.asString(), 8);
model.addParam(mac, params);
model.write(modelFile);
++numValid;
errSum += curErr;
++errCnt;
plot.show();
}
std::cout << "GOT " << numValid << " VALID APS" << std::endl;
std::cout << "SKIPPED " << numInvalid << " APS DUE TO NOT ENOUGH MEASUREMENTS" << std::endl;
float avgErr = errSum / errCnt;
std::cout << "------------------------------------------------" << std::endl;
std::cout << avgErr << std::endl;
}
Floorplan::IndoorMap* MyState::map;
int main(int argc, char** argv) {
// the dataset to use
DataSetup setup = data.UJI_TI;
// load the floorplan
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
MyState::map = map;
// optimize (and save) wifi parameters
//optimize(map, setup.scaler, setup.minWifiOccurences, setup.training, setup.wifiParams);
// testing
map->floors[0]->obstacles.clear();
// fetch previously optimized wifi paramters
WiFiModelX model;
model.read(setup.wifiParams, map);
// build the grid
Grid<MyNode> grid(20);
GridFactory<MyNode> factory(grid);
factory.build(map);
Importance::addImportance(grid);
FileReader fr(setup.training[0]);
//partikel gehen nicht schnell genug nach oben oder unten
//und wlan zwischen oben/unten ist nicht unterschiedlich genug?
std::vector<FileReader::TS<WiFiMeasurements>> scans = fr.getWiFiGroupedByTime();
Interpolator<float, FileReader::Position> path = fr.getPath();
Plotti plot;
plot.addFloors(map);
plot.addOutline(map);
plot.addStairs(map);
//plot.addGrid(grid);
for (FileReader::TS<FileReader::Position> pos : fr.positions) {
Point3 p3 = setup.scaler.convert3D(pos.data.lat, pos.data.lon, pos.data.floorNr);
plot.addGroundTruthNode(p3);
}
MyControl ctrl; ctrl.numSteps = 0;
MyObs obs;
int numParticles = 3000;
PFEval* eval = new PFEval(setup.scaler, model);
//std::unique_ptr<PFInit> init =
K::ParticleFilter<MyState, MyControl, MyObs> pf(numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
pf.setEvaluation(std::unique_ptr<PFEval>(eval));
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.04)));
pf.setNEffThreshold(1.0);
//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.6)));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
float lastUpdateMS = 0;
//StepDetector sd;
//TurnDetector td;
StepDetection sd;
TurnDetection td;
RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
PressureTendence baroTend(Timestamp::fromSec(5));
K::Statistics<float> errorStats;
struct TurnTest {
float rad = 0;
Point3 pos = Point3(81, 49, 0);
} tt;
// parse each sensor-value within the offline data
for (const FileReader::Entry& e : fr.getEntries()) {
const float ms = e.ts * 1000;
const Timestamp ts = Timestamp::fromMS(ms);
if (e.type == FileReader::Sensor::WIFI) {
obs.wifis = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == FileReader::Sensor::ACC) {
if (sd.isStep(ts, fr.getAccelerometer()[e.idx].data)) {
++ctrl.numSteps;
tt.pos.x += std::cos(tt.rad) * 0.7;
tt.pos.y += std::sin(tt.rad) * 0.7;
}
const FileReader::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
td.addAccelerometer(Timestamp::fromSec(_acc.ts), _acc.data);
} else if (e.type == FileReader::Sensor::GYRO) {
const FileReader::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
//td.update(ctrl.turnAngle, e.ts * 1000, fr.getGyroscope()[e.idx].data);
const float delta = td.addGyroscope(Timestamp::fromSec(_gyr.ts), _gyr.data);
ctrl.turnAngle += delta;
tt.rad += delta;
} else if (e.type == FileReader::Sensor::GPS) {
obs.gps = fr.getGPS()[e.idx].data;
} else if (e.type == FileReader::Sensor::BARO) {
relBaro.add(ts, fr.getBarometer()[e.idx].data);
//baroTend.add(ts, fr.getBarometer()[e.idx].data);
//std::cout << "add tendence" << std::endl;
ctrl.barometer.hPaRelativeToT0 = relBaro.getPressureRealtiveToStart();
ctrl.barometer.estimatedSigma = relBaro.getSigma();
ctrl.barometer.tendence = baroTend.get();
}
if (ms - lastUpdateMS > 1000) {
obs.curTS = e.ts;
// ((PFTrans*)pf.getTransition())->modWifi.obs = &obs;
FileReader::Position worldPos = path.get(e.ts);
//Point2 mapPos2 = setup.scaler.convert(worldPos.lat, worldPos.lon);
//Point3 mapPos = Point3(mapPos2.x, mapPos2.y, 0);
const Point3 mapPos = setup.scaler.convert3D(worldPos.lat, worldPos.lon, worldPos.floorNr);
MyState est = pf.update(&ctrl, obs);
Point3 estPos = est.pos.inMeter();
plot.pInterest.clear();
static float angleSum = 0; angleSum += ctrl.turnAngle;
//plot.showAngle(1, ctrl.turnAngle);
plot.showAngle(2, angleSum + M_PI);
//plot.debugWiFi(eval->model, obs.wifis, obs.curTS);
//plot.debugProb(grid, std::bind(&PFEval::getGPS, eval, std::placeholders::_1, std::placeholders::_2), obs);
//plot.debugProb(grid, std::bind(&PFEval::getWIFI, eval, std::placeholders::_1, std::placeholders::_2), obs);
//plot.debugProb(grid, std::bind(&PFEval::getALL, eval, std::placeholders::_1, std::placeholders::_2), obs);
plot.setEst(estPos);
plot.setGT(mapPos);
plot.addParticles(pf.getParticles());
plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n";
plot.gp << "set label 1002 at screen 0.02, 0.94 'tend:" << ((std::abs(baroTend.get()) > 0.06) ? "1" : "0") << " val: " << baroTend.get() << "'\n";
// error between GT and estimation
float err_m = mapPos.getDistance(estPos);
errorStats.add(err_m);
// debug
std::cout << err_m << std::endl;
std::cout << errorStats.asString() << std::endl;
std::cout << "ACHSENERKENNUNG AUF ACC FUER ROTATION??" << std::endl;
plot.show();
usleep(1000*10);
lastUpdateMS = ms;
// reset control
ctrl.numSteps = 0;
ctrl.turnAngle = 0;
}
}
/*
K::Statistics<double> stats;
for (FileReader::TS<FileReader::WiFis> scan : scans) {
//scan.data.groupVAPs();
plot.pColorPoints.clear();
// ground truth
plot.pInterest.clear();
FileReader::Position worldPos = path.get(scan.ts);
Point2 mapPos2 = scaler.convert(worldPos.lat, worldPos.lon);
Point3 mapPos(mapPos2.x, mapPos2.y, 0);
plot.pInterest.add(K::GnuplotPoint3(mapPos.x, mapPos.y, mapPos.z));
// update
stats.add( model.getProbability(scan, mapPos) );
const float step = 1.0;
float z = 0;
for (float x = -20; x < 90; x += step) {
for (float y = -10; y < 60; y += step) {
double prob = model.getProbability(scan, Point3(x,y,z));
plot.pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
}
}
plot.show();
usleep(1000*50);
}
std::cout << stats.asString() << std::endl;
*/
sleep(1000);
}

39
competition/src/competition/notes.txt Normal file
View File

@@ -0,0 +1,39 @@
some minor tests on one dataset
Optimizing WiFi Parameter
Remove Strong outliers during error-optimization:
Does NOT seem to help at all!
While wifi-opt is better, overwall result is worst
0% out: med(3.85227) avg(5.04607)
1% out: med(3.84351) avg(5.10242)
3% out: med(3.84224) avg(5.19669)
med(3.84321) avg(5.17647)
5% out: med(4.00438) avg(5.28421)
10% out: med(4.02074) avg(5.43715)
WiFiProbability
NormalDist: med(3.86891) avg(5.42092)
Region: med(3.90684) avg(5.16647)
Step Length:
70cm: med(4.0231) avg(5.52792)
80cm: med(4.01928) avg(5.30433)
85cm: med(3.85938) avg(5.10636)
90cm: med(3.87452) avg(5.06212)
Map/NonMap
NonMap: med(3.7643) avg(5.20786)
with Map: med(4.53366) avg(6.66799)