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

worked on filtering

Browse Source 
plots improved
worked on the scaler
added some tests
improved wifi-ap-optimization
This commit is contained in:
FrankE
2016-09-07 10:21:13 +02:00
parent dc63f1d39f
commit 51d189272d
7 changed files with 214 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
competition/src/competition/Optimizer.h
View File

@@ -61,6 +61,7 @@ public:
Floorplan::IndoorMap* map;
Scaler scaler;
VAPGrouper::Mode vapMode;
std::vector<FileReader> records;
// /** debug only */
@@ -98,7 +99,7 @@ public:
public:
Optimizer(Floorplan::IndoorMap* map, Scaler scaler) : map(map), scaler(scaler) {;}
Optimizer(Floorplan::IndoorMap* map, Scaler scaler, VAPGrouper::Mode vapMode) : map(map), scaler(scaler), vapMode(vapMode) {;}
/** add the given record */
void addRecord(const std::string& file) {
@@ -157,7 +158,7 @@ public:
wifiMap.clear();
// how to combine VAPs
const VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM);
const VAPGrouper vg(vapMode, VAPGrouper::Aggregation::MAXIMUM);
// parse each training data file
@@ -311,8 +312,8 @@ public:
std::cout << "use more rounds for production" << std::endl;
LeOpt opt(valRegion);
opt.setPopulationSize(250); // USE MORE FOR PRODUCTION
opt.setNumIerations(40);
opt.setPopulationSize(500); // USE MORE FOR PRODUCTION
opt.setNumIerations(100);
opt.calculateOptimum(func, (float*) &params);
// using LeOpt = K::NumOptAlgoGenetic<float>;

2
competition/src/competition/Plotti.h
View File

@@ -198,7 +198,7 @@ struct Plotti {
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);
K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
pParticles.add(pos / 100.0f);
}
}

65
competition/src/competition/Scaler.h
View File

@@ -4,6 +4,13 @@
#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>
struct IPIN {
double lat;
double lon;
double floorNr;
IPIN(const double lat, const double lon, const double floorNr) : lat(lat), lon(lon), floorNr(floorNr) {;}
};
class Scaler {
private:
@@ -15,43 +22,73 @@ private:
double rotDeg;
double mPerPx;
Point2 mapCenter_m;
double refLat;
double m_per_deg_lat;
double m_per_deg_lon;
const float fixedFloorHeight = 4.0f;
public:
/** image width, image height, map-center (lat/lon), map-rotation, meter<->pixel conversion factor */
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) {
;
mapCenter_m = Point2( (w*mPerPx/2), (h*mPerPx/2) );
refLat = cenLat / 180.0 * M_PI;
m_per_deg_lat = 111132.954 - 559.822 * std::cos( 2.0 * refLat ) + 1.175 * std::cos( 4.0 * refLat);
m_per_deg_lon = 111132.954 * std::cos ( refLat );
}
/** convert from (x,y,z) to (lat,lon,floorNr) */
IPIN toIPIN3(const float x, const float y, const float z) const {
Point2 pos(x,y);
// move to (0,0)
pos -= mapCenter_m;
// undo the rotation
pos = pos.rotated(-rotDeg / 180 * M_PI);
const double lat = cenLat + (pos.y / m_per_deg_lat);
const double lon = cenLon + (pos.x / m_per_deg_lon);
const double floorNr = z / fixedFloorHeight;
return IPIN(lat, lon, floorNr);
}
IPIN toIPIN3(const Point3 p) const {
return toIPIN3(p.x, p.y, p.z);
}
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;
// rotate (our map is axis aligned)
Point2 pos(x_m, y_m);
pos = pos.rotated(rotDeg / 180 * M_PI);
const Point2 center( (w*mPerPx/2), (h*mPerPx/2) );
pos += center;
// apply movement
pos += mapCenter_m;
return pos;

52
competition/src/competition/filter/Logic.h
View File

@@ -10,7 +10,7 @@
#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/WalkModuleRelativePressureControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
@@ -20,9 +20,9 @@
#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 MyWalkState : public WalkState, public WalkStateHeading, public WalkStateRelativePressure {
// MyWalkState(const GridPoint& gp, const Heading head, const float relativePressure) : WalkState(gp), WalkStateHeading(head, 0), WalkStateRelativePressure(relativePressure) {;}
//};
struct MyNode : public GridPoint, public GridNode, public GridNodeImportance {
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
@@ -79,9 +79,10 @@ struct PFInit : public K::ParticleFilterInitializer<MyState> {
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
p.state.position = grid[idx]; // random position
p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading
p.state.heading.error = 0;
p.state.relPres = 0; // start with a relative pressure of 0
}
}
@@ -93,20 +94,20 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
GridWalker<MyNode, MyWalkState> walker;
GridWalker<MyNode, MyState> 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;
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
WalkModuleNodeImportance<MyNode, MyState> modImportance;
// WalkModuleRelativePressureControl<MyNode, MyState, MyControl> modPressure;
WalkModuleSpread<MyNode, MyState> modSpread;
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
//WalkModuleWiFi modWifi;
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl), modPressure(ctrl, 0.100) {
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, 3.5f) {//, modPressure(ctrl, 0.100) {
walker.addModule(&modHead);
//walker.addModule(&modHeadUgly);
walker.addModule(&modImportance);
@@ -128,18 +129,23 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
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);
// --------- OLD
// transfer from state to walkstate
// const MyWalkState start(grid.getNodeFor(p.state.pos), p.state.head, p.state.relPres);
// 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();
// p.state.relPres = end.pressureRelToT0;
// p.state.pos = end.position;
// p.state.head = end.heading.direction.getRAD();
// -------------- NEW
// update the particle in-place
p.state = walker.getDestination(grid, p.state, dist);
tmpSum2 += p.state.relPres;
++tmpCnt;
@@ -196,7 +202,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
for (K::Particle<MyState>& p : particles) {
const double prob = getALL(observation, p.state.pos.inMeter());
const double prob = getALL(observation, p.state.position.inMeter());
p.weight = prob;
sum += prob;

81
competition/src/competition/filter/Structs.h
View File

@@ -8,54 +8,77 @@
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
struct MyState {
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkStateHeading.h>
struct MyState : public WalkState, public WalkStateHeading {
static Floorplan::IndoorMap* map;
GridPoint pos; // current position
float head; // current heading
// GridPoint pos; // current position
// float head; // current heading
// CURRENTLY OBSOLETE (did not work as expected)
float relPres; // current pressure relative to t0
MyState() : pos(), head(0) {;}
MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0) {;}
MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0) {;}
MyState(GridPoint pos, float head) : pos(pos), head(head) {;}
MyState& operator += (const MyState& o) {
this->pos += o.pos;
this->head += o.head;
this->position += o.position;
return *this;
}
MyState& operator /= (const double d) {
this->pos /= d;
this->head /= d;
this->position /= d;
return *this;
}
MyState operator * (const double d) const {
return MyState(pos*d, head*d);
return MyState(this->position*d);
}
bool belongsToRegion(const MyState& o) const {
return pos.inMeter().getDistance(o.pos.inMeter()) < 3.0;
return position.inMeter().getDistance(o.position.inMeter()) < 3.0;
}
// 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);
}
// 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);
// }
};

8
competition/src/competition/filter/WiFi.h
View File

@@ -14,6 +14,8 @@ class WiFiModelX : public WiFiModel {
private:
VAPGrouper::Mode vapMode;
std::unordered_map<MACAddress, APParams> aps;
std::vector<float> ceilingsAtHeight_m;
@@ -21,6 +23,10 @@ private:
public:
WiFiModelX (VAPGrouper::Mode vapMode) : vapMode(vapMode) {
;
}
void write(const std::string& file) {
std::ofstream out(file);
for (auto it : aps) {
@@ -108,7 +114,7 @@ public:
if (scan.entries.empty()) {return 1.0;}
VAPGrouper vg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM);
VAPGrouper vg(vapMode, VAPGrouper::Aggregation::MAXIMUM);
double prob = 0;
for (const WiFiMeasurement& ap : scan.entries) {

90
competition/src/competition/main.cpp
View File

@@ -4,9 +4,6 @@
#include "filter/Structs.h"
#include "Plotti.h"
#include "filter/WiFi.h"
#include "filter/Logic.h"
@@ -29,10 +26,6 @@
#include "Optimizer.h"
#include "Scaler.h"
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
@@ -45,7 +38,36 @@
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
void testScaler() {
Scaler s(100, 100, 50, 10, 0.231, 1.0);
{
const Point3 p3 = s.convert3D(50,10,0);
Assert::isNear(p3.x, 50.0f, 0.1f, "err");
Assert::isNear(p3.y, 50.0f, 0.1f, "err");
Assert::isNear(p3.z, 0.0f, 0.1f, "err");
}
const double sd = 0.00000001;
{
const Point3 p3 = s.convert3D(50.01, 10.01, 0);
const IPIN ii = s.toIPIN3(p3.x, p3.y, p3.z);
Assert::isNear(ii.lat, 50.01, sd, "err");
Assert::isNear(ii.lon, 10.01, sd, "err");
Assert::isNear(ii.floorNr, 0.0, sd, "err");
}
{
const Point3 p3 = s.convert3D(49.99, 9.98, 0);
const IPIN ii = s.toIPIN3(p3.x, p3.y, p3.z);
Assert::isNear(ii.lat, 49.99, sd, "err");
Assert::isNear(ii.lon, 9.98, sd, "err");
Assert::isNear(ii.floorNr, 0.0, sd, "err");
}
}
@@ -60,6 +82,7 @@ struct DataSetup {
std::vector<std::string> training;
std::string wifiParams;
int minWifiOccurences;
VAPGrouper::Mode vapMode;
Scaler scaler;
};
@@ -79,6 +102,7 @@ struct Data {
dataDir + "car/wifiParams.txt",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
Scaler (1282, 818, 40.31320308, -3.48367648, -8.77680000, 0.05207600)
@@ -99,6 +123,7 @@ struct Data {
dataDir + "uah/wifiParams.txt",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
Scaler (1869, 1869, 40.51312440, -3.34959080, -40.73112000, 0.07596002)
@@ -113,22 +138,44 @@ struct Data {
dataDir + "uji-ti/logfile_UJITI_R2_NEXUS5.txt",
},
dataDir + "uah/wifiParams.txt",
dataDir + "uji-ti/wifiParams.txt",
15,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
Scaler (748, 1046, 39.99322125, -0.06862410, 29.57638723, 0.08556493)
};
DataSetup UJI_UB = {
mapDir + "UJI-UB/UJI-UB4.xml",
{
dataDir + "uji-ub/logfile_UJIUB_R1n_S3.txt",
dataDir + "uji-ub/logfile_UJIUB_R1r_S3.txt",
dataDir + "uji-ub/logfile_UJIUB_R2n_S3.txt",
dataDir + "uji-ub/logfile_UJIUB_R2r_S3.txt",
dataDir + "uji-ub/logfile_UJIUB_R3_S3.txt",
},
dataDir + "uji-ub/wifiParams.txt",
12,
VAPGrouper::Mode::DISABLED,
// BEWARE! not all images have the same size!
Scaler (4362, 1959, 39.99380328, -0.07389222, 29.54974856, 0.01700000)
};
} 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) {
void optimize(Floorplan::IndoorMap* map, Scaler& scaler, VAPGrouper::Mode vapMode, 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);
Optimizer opti(map, scaler, vapMode);
// all training-data-files (measurements at a known ground-truth)
opti.addRecords(trainingFiles);
@@ -144,7 +191,7 @@ void optimize(Floorplan::IndoorMap* map, Scaler& scaler, const int minWifiOccure
int errCnt = 0;
std::string modelFile = outFile;
WiFiModelX model;
WiFiModelX model(vapMode);
int numValid = 0;
int numInvalid = 0;
@@ -196,23 +243,24 @@ Floorplan::IndoorMap* MyState::map;
int main(int argc, char** argv) {
testScaler();
// the dataset to use
DataSetup setup = data.UJI_TI;
DataSetup setup = data.CAR;
// 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);
//optimize(map, setup.scaler, setup.vapMode, setup.minWifiOccurences, setup.training, setup.wifiParams);
// testing
map->floors[0]->obstacles.clear();
// fetch previously optimized wifi paramters
WiFiModelX model;
WiFiModelX model(setup.vapMode);
model.read(setup.wifiParams, map);
// build the grid
@@ -225,7 +273,7 @@ int main(int argc, char** argv) {
FileReader fr(setup.training[0]);
FileReader fr(setup.training[3]);
//partikel gehen nicht schnell genug nach oben oder unten
@@ -251,7 +299,7 @@ int main(int argc, char** argv) {
MyControl ctrl; ctrl.numSteps = 0;
MyObs obs;
int numParticles = 3000;
int numParticles = 10000;
PFEval* eval = new PFEval(setup.scaler, model);
@@ -299,18 +347,18 @@ int main(int argc, char** argv) {
obs.wifis = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == FileReader::Sensor::ACC) {
if (sd.isStep(ts, fr.getAccelerometer()[e.idx].data)) {
if (sd.add(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);
td.addAccelerometer(Timestamp::fromSec(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);
const float delta = td.addGyroscope(Timestamp::fromSec(ts), _gyr.data);
ctrl.turnAngle += delta;
tt.rad += delta;
@@ -327,7 +375,7 @@ int main(int argc, char** argv) {
}
if (ms - lastUpdateMS > 1000) {
if (ms - lastUpdateMS > 500) {
obs.curTS = e.ts;
@@ -341,7 +389,7 @@ int main(int argc, char** argv) {
MyState est = pf.update(&ctrl, obs);
Point3 estPos = est.pos.inMeter();
Point3 estPos = est.position.inMeter();
plot.pInterest.clear();