def approximate_geo_to_world(coords_geo_rad, coords_world):
geo_origin = coords.GeodeticCoordinate(*np.mean(coords_geo_rad, axis=0))
lat2m, lon2m = approximate_geo_to_meter_NED(geo_origin, coords_geo_rad)
coords_ned = transform_geo_to_meter(geo_origin, lat2m, lon2m, coords_geo_rad)
world_angle = estimate_rotation(coords_world[:,:2], coords_ned[:,:2])
coords_world_parallel = transform3d(rotation3d_z(world_angle), coords_ned)
world_offset = np.mean(coords_world - coords_world_parallel,axis=0)
return geo_origin, lat2m, lon2m, world_angle, world_offset
def tf_world_geo(origin, lat2m, lon2m, world_angle, world_offset):
"""
Transformation matrix transforming from geo coordinates to metric world coordinates.
:param origin: The geo origin
:param lat2m: The latitude to meter conversion factor
:param lon2m: The longitude to meter conversion factor
:param world_angle: The world angle
:param world_offset: The world offset
:type origin: coords.GeodeticCoordinate
:type lat2m: number
:type lon2m: number
:type world_angle: number [radian]
:type world_offset: number [meter]
:returns: Transformation matrix
:rtype: np.ndarray with shape [4,4]
"""
rotation = rotation3d_z(world_angle)
translation = translation3d(*world_offset)
return translation @ rotation @ tf_meter_geo(origin, lat2m, lon2m)
def tf_map_geo(origin, lat2m, lon2m, world_angle, world_offset, map_origin_in_world):
"""
Transformation matrix transforming from geo coordinates to metric map coordinates.
:param origin: The geo origin
:param lat2m: The latitude to meter conversion factor
:param lon2m: The longitude to meter conversion factor
:param world_angle: The world angle
:param world_offset: The world offset
:param map_origin_in_world: The map origin in the world
:type origin: coords.GeodeticCoordinate
:type lat2m: number
:type lon2m: number
:type world_angle: number
:type world_offset: np.ndarray with shape(3,)
:type map_origin_in_world: np.ndarray with shape(3,)
:returns: Transformation matrix
:rtype: np.ndarray with shape [4,4]
"""
rotation = rotation3d_z(world_angle)
translation = translation3d(*(world_offset-map_origin_in_world))
return translation @ rotation @ tf_meter_geo(origin, lat2m, lon2m)