FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

merged

Browse Source
This commit is contained in:
toni
2017-03-24 17:01:51 +01:00
parent 59502931e5 e34e773e31
commit 749ee0b783
13 changed files with 267 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
floorplan/v2/Floorplan.h
View File

@@ -197,7 +197,7 @@ namespace Floorplan {
using FloorPOIs = std::vector<POI*>;
using FloorStairs = std::vector<Stair*>;
using FloorElevators = std::vector<Elevator*>;
using FloorGroundTruthPoints = std::vector<GroundTruthPoint*>;
using FloorGroundTruthPoints = std::vector<GroundTruthPoint*>;
/** describes one floor within the map, starting at a given height */
struct Floor {
@@ -215,7 +215,7 @@ namespace Floorplan {
FloorPOIs pois; // POIs within the floor
FloorStairs stairs; // all stairs within one floor
FloorElevators elevators; // all elevators within one floor
FloorGroundTruthPoints gtpoints; // all ground truth points within one floor
FloorGroundTruthPoints gtpoints; // all ground truth points within one floor
//FloorKeyValue other; // other, free elements
Floor() {;}
@@ -251,9 +251,10 @@ namespace Floorplan {
/** a GroundTruthPoint located somewhere on a floor */
struct GroundTruthPoint {
int id; //TODO: this value can be changed and isn't set incremental within the indoormap
Point3 pos;
Point3 pos; // TODO: splint into 2D position + float for "heightAboveGround" [waypoints' height is relative to the floor's height!
GroundTruthPoint() : id(), pos() {;}
GroundTruthPoint(const int& id, const Point3& pos) : id(id), pos(pos) {;}
const Point3 getPosition(const Floor& f) const {return pos + Point3(0,0,f.atHeight);}
bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
};

2
floorplan/v2/FloorplanCeilings.h
View File

@@ -87,7 +87,7 @@ namespace Floorplan {
if (diff < 0.1) {++numNear;} else {++numFar;}
}
if ((numNear + numFar) > 150000) {
Assert::isTrue(numNear < numFar*0.1,
Assert::isTrue(numNear < numFar*0.15,
"many requests to Floorplan::Ceilings::numCeilingsBetween address nodes (very) near to a ground! \
due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
expect very wrong outputs! \

36
floorplan/v2/FloorplanHelper.h
View File

@@ -29,6 +29,42 @@ public:
return std::make_pair(nullptr, nullptr);
}
/** get all APs within the map */
static std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> getAPs(const Floorplan::IndoorMap* map) {
std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> res;
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::AccessPoint* ap : f->accesspoints) {
res.push_back(std::make_pair(ap,f));
}
}
return res;
}
/** get all ground-truth points within the map as hash-map: id->pos */
static std::unordered_map<int, Point3> getGroundTruthPoints(const Floorplan::IndoorMap* map) {
std::unordered_map<int, Point3> res;
for (const Floorplan::Floor* f : map->floors) {
for (const Floorplan::GroundTruthPoint* gtp : f->gtpoints) {
res[gtp->id] = gtp->getPosition(*f);
}
}
return res;
}
/** get all ground-truth points, identified by the given indices */
static std::vector<Point3> getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> indices) {
// get a map id->pos for all ground-truth-points within the map
const std::unordered_map<int, Point3> src = getGroundTruthPoints(map);
std::vector<Point3> res;
for (const int idx : indices) {
const auto& it = src.find(idx);
if (it == src.end()) {throw Exception("map does not contain a ground-truth-point with ID " + std::to_string(idx));}
res.push_back(it->second);
}
return res;
}
public:
/** align all floorplan values to the given grid size. needed for the grid factory */

138
geo/EarthMapping.h
View File

@@ -3,6 +3,9 @@
#include "Point3.h"
#include "EarthPos.h"
#include "../floorplan/v2/Floorplan.h"
#include "../floorplan/v2/FloorplanHelper.h"
#include "../misc/Debug.h"
/**
* mapping between positions on earth and positions within the floorplan
@@ -23,30 +26,119 @@ private:
double m_per_deg_lat;
double m_per_deg_lon;
static constexpr const char* NAME = "EarthMap";
public:
/** ctor with parameters */
EarthMapping(Point3 center_map_m, EarthPos center_earth, float rotation_deg) :
center_map_m(center_map_m), center_earth(center_earth), rotation_deg(rotation_deg) {
precalc();
}
/** ctor for a given map */
EarthMapping(Floorplan::IndoorMap* map) {
// get the map<->earth correspondences from the floorplan
const std::vector<Floorplan::EarthPosMapPos*> cor = map->earthReg.correspondences;
// sanity check
if (cor.size() < 3) {
throw Exception("for EarthMapping to work, the map needs at least 3 correspondence points");
}
Log::add(NAME, "calculating map<->earth correspondence using " + std::to_string(cor.size()) + " reference points");
// 1)
// to reduce errors, use the average of all correspondces for earth<->map mapping
Point3 _mapSum(0,0,0);
EarthPos _earthSum(0,0,0);
for (const Floorplan::EarthPosMapPos* epmp : cor) {
_mapSum += epmp->posOnMap_m;
_earthSum.lat += epmp->posOnEarth.lat;
_earthSum.lon += epmp->posOnEarth.lon;
_earthSum.height += epmp->posOnEarth.height;
}
const Point3 _mapAvg = _mapSum / cor.size();
const EarthPos _earthAvg = EarthPos(_earthSum.lat/cor.size(), _earthSum.lon/cor.size(), _earthSum.height/cor.size());
// 2)
// initialize the mapper with those values
// this allows a first mapping, but the map is not facing towards the north!
rotation_deg = 0; // currently unkown
center_map_m = _mapAvg;
center_earth = _earthAvg;
precalc();
Log::add(NAME, "avg. reference points: " + _mapAvg.asString() + " <-> " + _earthAvg.asString());
// 3)
// now we use this initial setup to determine the rotation angle between map and world
float deltaAngleSum = 0;
for (Floorplan::EarthPosMapPos* epmp : cor) {
// angle between mapAvg and mapCorrespondencePoint
const float angleMap = std::atan2(_mapAvg.y - epmp->posOnMap_m.y, _mapAvg.x - epmp->posOnMap_m.x);
// use the current setup to convert from map to world, WITHOUT correct rotation
const Point3 repro = this->worldToMap(epmp->posOnEarth);
// get the angle between mapAvg and projectedCorrespondencePoint
const float angleEarth = std::atan2(_mapAvg.y - repro.y, _mapAvg.x - repro.x);
// the difference between angleMap and angleEarth contains the angle needed to let the map face northwards
// we use the average of all those angles determined by each correspondence
const float dx_rad = angleEarth - angleMap;
float dx_deg = (dx_rad * 180 / M_PI);
if (dx_deg < 0) {dx_deg = 360 + dx_deg;}
deltaAngleSum += dx_deg;
}
const float deltaSumAvg = deltaAngleSum / cor.size();
Log::add(NAME, "avg angular difference [north-rotation]: " + std::to_string(deltaSumAvg));
// 4)
// the current center is not the rotation center we actually need:
// e.g. when correspondence points were outside of the building, the rotation center is wrong
// as real rotation center, we use the building's bbox center and the correspondence lon/lat on earth
const BBox3 bbox = FloorplanHelper::getBBox(map);
const Point2 _mapCenter2 = ((bbox.getMax() - bbox.getMin()) / 2).xy();
const Point3 _mapCenter3 = Point3(_mapCenter2.x, _mapCenter2.y, this->center_map_m.z); // keep original z!
this->center_earth = mapToWorld(_mapCenter3);
this->center_map_m = _mapCenter3;
Log::add(NAME, "setting rotation center from bbox: " + center_map_m.asString() + " <-> " + center_earth.asString());
// 5)
// finally, let the mapper know the north-angle
this->rotation_deg = deltaSumAvg;
}
void build() {
// TODO
precalc();
}
/** convert from map-coordinates to earth-coordinates */
EarthPos mapToWorld(const Point3 mapPos_m) const {
Point3 pos = mapPos_m;
// move to (0,0,0)
pos -= center_map_m;
Point3 pos = mapPos_m - center_map_m;
// undo the rotation
const Point2 xy = pos.xy().rotated(-rotation_deg / 180.0 * (float) M_PI);
const Point2 xy = pos.xy().rotated(degToRad(+rotation_deg));
// convert this "delta to (0,0,0)" to lon/lat and move it to the lon/lat-center
const double lat = cenLat + (xy.y / m_per_deg_lat);
const double lon = cenLon + (xy.x / m_per_deg_lon);
const float height = pos.z;
const double lat = center_earth.lat + (xy.y / m_per_deg_lat);
const double lon = center_earth.lon + (xy.x / m_per_deg_lon);
const float height = center_earth.height + pos.z;
// done
return EarthPos(lat, lon, height);
@@ -56,18 +148,36 @@ public:
/** convert from earth-coordinates to map-coordinates */
Point3 worldToMap(const EarthPos earthPos) const {
const double y_m = +(lat-cenLat) * m_per_deg_lat;
const double x_m = +(lon-cenLon) * m_per_deg_lon;
// move to center and scale
const double y_m = +(earthPos.lat - center_earth.lat) * m_per_deg_lat;
const double x_m = +(earthPos.lon - center_earth.lon) * m_per_deg_lon;
const double z_m = (earthPos.height - center_earth.height);
// rotate (our map is axis aligned)
Point2 pos(x_m, y_m);
pos = pos.rotated(rotDeg / 180 * M_PI);
Point2 xy(x_m, y_m);
xy = xy.rotated(degToRad(-rotation_deg));
// apply movement
pos += mapCenter_m;
// append height
Point3 pos3(xy.x, xy.y, z_m);
return pos;
// move from center
pos3 += center_map_m;
return pos3;
}
private:
/** perform some pre-calculations to speed things up */
void precalc() {
const double refLat = center_earth.lat / 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 );
}
static inline float degToRad(const float deg) {
return deg / 180.0f * (float) M_PI;
}
};

4
geo/EarthPos.h
View File

@@ -21,6 +21,10 @@ struct EarthPos {
;
}
std::string asString() const {
return "(lat: " + std::to_string(lat) + "°, lon: " + std::to_string(lon) + "°, alt: " + std::to_string(height) + ")";
}
};
#endif // EARTHPOS_H

7
main.cpp
View File

@@ -28,8 +28,11 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
//::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
::testing::GTEST_FLAG(filter) = "*KullbackLeibler*";
//::testing::GTEST_FLAG(filter) = "*Offline.readWrite*";
//::testing::GTEST_FLAG(filter) = "*Offline.readWrite*";
::testing::GTEST_FLAG(filter) = "*Earth*";
//::testing::GTEST_FLAG(filter) = "*Barometer*";
//::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*";

4
sensors/gps/GPSData.h
View File

@@ -38,6 +38,10 @@ struct GPSData {
EQ_OR_NAN(speed, o.speed);
}
std::string asString() const {
return "(lat: " + std::to_string(lat) + ", lon: " + std::to_string(lon) + ", alt: " + std::to_string(alt) + " acur: " + std::to_string(accuracy) + ")";
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}

3
sensors/offline/FileReader.h
View File

@@ -193,7 +193,8 @@ namespace Offline {
WiFiMeasurements wifi;
Splitter s(data, sep);
for (size_t i = 0; i < s.size()-1; i += 3) {
// the -1 is due to some old files containing a trailing ";" resulting in one additional stray column
for (size_t i = 0; i < s.size()-1; i += 3) {
const std::string mac = s.get(i+0);
const float freq = s.getFloat(i+1);

4
sensors/offline/Sensors.h
View File

@@ -9,12 +9,12 @@ namespace Offline {
LIN_ACC = 2,
GYRO = 3,
BARO = 5,
COMPASS = 6, // also called "orientatioN"
WIFI = 8,
BEACON = 9,
COMPASS = 15,
GPS = 16,
GROUND_TRUTH = 99,
POS = 1001, // IPIN2016
POS = 1001, // IPIN2016
};
template <typename T> struct TS {

3
sensors/radio/model/WiFiModel.h
View File

@@ -14,6 +14,9 @@ class WiFiModel {
public:
/** dtor */
virtual ~WiFiModel() {;}
// /** get the given access-point's RSSI at the provided location */
// virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;

35
sensors/radio/model/WiFiModelLogDistCeiling.h
View File

@@ -71,27 +71,36 @@ public:
}
/** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
/**
* load AP information from the floorplan.
* use the given fixed TXP/EXP/WAF for all APs.
* usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false!
*/
void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f, const bool assertSafe = true) {
for (const Floorplan::Floor* floor : map->floors) {
for (const Floorplan::AccessPoint* ap : floor->accesspoints) {
const APEntry ape(ap->getPos(floor), txp, exp, waf);
const MACAddress mac = vg.getBaseMAC(MACAddress(ap->mac));
Log::add("WiModLDC", "AP: " + ap->mac + " -> " + mac.asString());
addAP(MACAddress(mac), ape);
Log::add("WiModLDC", "AP added! given: " + ap->mac + " -> after VAP: " + mac.asString());
addAP(MACAddress(mac), ape, assertSafe);
}
}
}
/** make the given AP (and its parameters) known to the model */
void addAP(const MACAddress& accessPoint, const APEntry& params) {
/**
* make the given AP (and its parameters) known to the model
* usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false!
*/
void addAP(const MACAddress& accessPoint, const APEntry& params, const bool assertSafe = true) {
// sanity check
//Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
//Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
//Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
if (assertSafe) {
Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
}
Assert::equal(accessPoints.find(accessPoint), accessPoints.end(), "AccessPoint already present! VAP-Grouping issue?");
@@ -124,7 +133,13 @@ public:
const float wafLoss = params.waf * ceilings.numCeilingsBetween(position_m, params.position_m);
// combine
return rssiLOS + wafLoss;
const float res = rssiLOS + wafLoss;
// sanity check
Assert::isNotNaN(res, "detected NaN within WiFiModelLogDistCeiling::getRSSI()");
// ok
return res;
}

7
sensors/radio/setup/WiFiOptimizer.h
View File

@@ -32,6 +32,13 @@ namespace WiFiOptimizer {
return res;
}
/** get all [VAPGrouped, Averaged] fingerprints for the given mac */
virtual const std::vector<RSSIatPosition>& getFingerprintsFor(const MACAddress& mac) {
const auto& it = apMap.find(mac);
if (it == apMap.end()) {throw Exception("mac not found: " + mac.asString());}
return it->second;
}
/** add a new fingerprint to the optimizer as data-source */
virtual void addFingerprint(const WiFiFingerprint& fp) {

50
tests/geo/TestEarth.cpp Normal file
View File

@@ -0,0 +1,50 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../geo/EarthMapping.h"
TEST(Earth, fordwardBackward) {
const Point3 mapCenter(20, 30, 10);
const EarthPos earthCenter(50, 10, 100);
// earth-mapping including rotation
EarthMapping em(mapCenter, earthCenter, 40);
// as-is
const Point3 p1 = em.worldToMap( earthCenter );
ASSERT_EQ(mapCenter.x, p1.x);
ASSERT_EQ(mapCenter.y, p1.y);
ASSERT_EQ(mapCenter.z, p1.z);
// as-is
const EarthPos ep1 = em.mapToWorld( mapCenter );
ASSERT_EQ(earthCenter.lat, ep1.lat);
ASSERT_EQ(earthCenter.lon, ep1.lon);
ASSERT_EQ(earthCenter.height, ep1.height);
// + 20 height
const Point3 p2 = em.worldToMap( EarthPos(50,10,120) );
ASSERT_EQ(mapCenter.x, p2.x);
ASSERT_EQ(mapCenter.y, p2.y);
ASSERT_EQ(30, p2.z);
// - 20 height
const EarthPos ep2 = em.mapToWorld( Point3(20, 30, -10) );
ASSERT_EQ(earthCenter.lat, ep2.lat);
ASSERT_EQ(earthCenter.lon, ep2.lon);
ASSERT_EQ(earthCenter.height-20, ep2.height);
}
TEST(Earth, estimateNorth) {
}
#endif