Indoor/geo/EarthMapping.h

/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef EARTHMAPPING_H
#define EARTHMAPPING_H

#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
 */
class EarthMapping {

private:

	/** one 3D position within the floorplan */
	Point3 center_map_m;

	/** corresponding 3D position on earth */
	EarthPos center_earth;

	/** rotation [in degrees] to ensure that the map's y-up-axis points towards the north */
	float rotation_deg;

	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 {

		// move to (0,0,0)
		Point3 pos = mapPos_m - center_map_m;

		// undo the rotation
		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 = 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);

	}

	/** convert from earth-coordinates to map-coordinates */
	Point3 worldToMap(const EarthPos earthPos) const {

		// 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 xy(x_m, y_m);
		xy = xy.rotated(degToRad(-rotation_deg));

		// append height
		Point3 pos3(xy.x, xy.y, z_m);

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

};

#endif // EARTHMAPPING_H