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
This commit is contained in:
@@ -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;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user