added helper methods for debug printing

fixed issue when reading wifi entries within old walk files
worked on earth-registration
added corresponding test-cases
other minor changes

floorplan/v2/FloorplanHelper.h

@@ -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 */

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<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 lat = center_earth.lat + (xy.y / m_per_deg_lat);
-		const double lon = cenLon + (xy.x / m_per_deg_lon);
+		const double lon = center_earth.lon + (xy.x / m_per_deg_lon);
-		const float height = pos.z;
+		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;
+		// move to center and scale
-		const double x_m = +(lon-cenLon) * m_per_deg_lon;
+		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);
+		Point2 xy(x_m, y_m);
-		pos = pos.rotated(rotDeg / 180 * M_PI);
+		xy = xy.rotated(degToRad(-rotation_deg));
 
-		// apply movement
+		// append height
-		pos += mapCenter_m;
+		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;
 	}
 
 };

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

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*";
 

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) );}

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);

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 <typename T> struct TS {

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]");
+		if (assertSafe) {
-		Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
+			Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
-		Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
+			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?");
 

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<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) {
 

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