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

added helper methods for debug printing

Browse Source 
fixed issue when reading wifi entries within old walk files
worked on earth-registration
added corresponding test-cases
other minor changes
This commit is contained in:
FrankE
2017-03-24 10:07:08 +01:00
parent b03804d378
commit c0cef979bb
10 changed files with 214 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
floorplan/v2/FloorplanHelper.h
View File

@@ -29,6 +29,17 @@ 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;
}
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

5
main.cpp
View File

@@ -29,7 +29,10 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
::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(); 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 {

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

@@ -86,12 +86,14 @@ public:
}
/** make the given AP (and its parameters) known to the model */
void addAP(const MACAddress& accessPoint, const APEntry& params) {
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?");

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