diff --git a/floorplan/v2/FloorplanHelper.h b/floorplan/v2/FloorplanHelper.h index 473202c..53605cd 100644 --- a/floorplan/v2/FloorplanHelper.h +++ b/floorplan/v2/FloorplanHelper.h @@ -29,6 +29,17 @@ public: return std::make_pair(nullptr, nullptr); } + /** get all APs within the map */ + static std::vector> getAPs(const Floorplan::IndoorMap* map) { + std::vector> 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 */ diff --git a/geo/EarthMapping.h b/geo/EarthMapping.h index 8ff10da..aa1471c 100644 --- a/geo/EarthMapping.h +++ b/geo/EarthMapping.h @@ -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 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; } }; diff --git a/geo/EarthPos.h b/geo/EarthPos.h index c8bac44..e46c50d 100644 --- a/geo/EarthPos.h +++ b/geo/EarthPos.h @@ -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 diff --git a/main.cpp b/main.cpp index de06c02..411cec6 100755 --- a/main.cpp +++ b/main.cpp @@ -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*"; diff --git a/sensors/gps/GPSData.h b/sensors/gps/GPSData.h index e314d85..7d63cac 100644 --- a/sensors/gps/GPSData.h +++ b/sensors/gps/GPSData.h @@ -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) );} diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index 99d0fd4..b5372ef 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -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); diff --git a/sensors/offline/Sensors.h b/sensors/offline/Sensors.h index 47ffec6..32168cd 100644 --- a/sensors/offline/Sensors.h +++ b/sensors/offline/Sensors.h @@ -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 struct TS { diff --git a/sensors/radio/model/WiFiModelLogDistCeiling.h b/sensors/radio/model/WiFiModelLogDistCeiling.h index f0a7e05..c823f9d 100644 --- a/sensors/radio/model/WiFiModelLogDistCeiling.h +++ b/sensors/radio/model/WiFiModelLogDistCeiling.h @@ -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?"); diff --git a/sensors/radio/setup/WiFiOptimizer.h b/sensors/radio/setup/WiFiOptimizer.h index c9996c9..125fb7c 100644 --- a/sensors/radio/setup/WiFiOptimizer.h +++ b/sensors/radio/setup/WiFiOptimizer.h @@ -32,6 +32,13 @@ namespace WiFiOptimizer { return res; } + /** get all [VAPGrouped, Averaged] fingerprints for the given mac */ + virtual const std::vector& 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) { diff --git a/tests/geo/TestEarth.cpp b/tests/geo/TestEarth.cpp new file mode 100644 index 0000000..955c6f0 --- /dev/null +++ b/tests/geo/TestEarth.cpp @@ -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