merged

floorplan/v2/Floorplan.h

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

floorplan/v2/FloorplanCeilings.h

@@ -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! \

floorplan/v2/FloorplanHelper.h

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

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

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

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

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/WiFiModel.h

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

sensors/radio/model/WiFiModelLogDistCeiling.h

@@ -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());
+				Log::add("WiModLDC", "AP added! given: " + ap->mac + " -> after VAP: " + mac.asString());
-				addAP(MACAddress(mac), ape);
+				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]");
+		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?");
 
@@ -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;
 
 	}
 

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