关于ENU与LLA坐标系互相转换的python代码

关于ENU与LLA坐标系互相转换的python代码

import math

# WGS84 ellipsoid parameters
WGS_PARAMS = {
    'a': 6378137,
    'f': 1 / 298.257223563,
    'b': 6378137 * (1 - 1 / 298.257223563),
}
wgs84_a = 6378137
wgs84_b = 6378137 * (1 - 1 / 298.257223563)
wgs84_f = 1 / 298.257223563
# wgs84_f = 1.0 / 298.257223563
pow_e_2 = wgs84_f * (2.0 - wgs84_f)
def deg2radian(deg):
    return (deg * math.pi) / 180.0
def radian2deg(rad):
    return (rad * 180.0) / math.pi
def wgs2ecef(wgs):
    lng_r = deg2radian(wgs['lng'])
    lat_r = deg2radian(wgs['lat'])
    a = WGS_PARAMS['a']
    b = WGS_PARAMS['b']
    N = a ** 2 / math.sqrt(a ** 2 * math.cos(lat_r) ** 2 + b ** 2 * math.sin(lat_r) ** 2)
    x = (N + wgs['alt']) * math.cos(lat_r) * math.cos(lng_r)
    y = (N + wgs['alt']) * math.cos(lat_r) * math.sin(lng_r)
    z = (N * (b / a) ** 2 + wgs['alt']) * math.sin(lat_r)
    return {'x': x, 'y': y, 'z': z}

def ecef2wgs(x, y, z):
    a = WGS_PARAMS['a']
    b = WGS_PARAMS['b']
    e = math.sqrt(a ** 2 - b ** 2)
    r = math.sqrt(x ** 2 + y ** 2 + z ** 2)
    u = math.sqrt(0.5 * (r ** 2 - e ** 2) + 0.5 * math.sqrt((r ** 2 - e ** 2) ** 2 + 4 * e ** 2 * z ** 2))
    Q = math.hypot(x, y)
    huE = math.hypot(u, e)
    Beta = math.atan((huE / u) * (z / Q))
    eps = ((b * u - a * huE + e ** 2) * math.sin(Beta)) / ((a * huE * 1) / math.cos(Beta) - e ** 2 * math.cos(Beta))
    Beta += eps
    lat = math.atan((a / b) * math.tan(Beta))
    lng = math.atan2(y, x)
    alt = math.hypot(z - b * math.sin(Beta), Q - a * math.cos(Beta))
    inside = (x ** 2 / a ** 2 + y ** 2 / a ** 2 + z ** 2 / b ** 2) < 1
    if inside:
        alt = -alt

    lat = radian2deg(lat)
    lng = radian2deg(lng)

    return lat, lng, alt


def enu2uvw(east, north, up, lng0, lat0):
    lng_r = deg2radian(lng0)
    lat_r = deg2radian(lat0)

    t = math.cos(lat_r) * up - math.sin(lat_r) * north
    w = math.sin(lat_r) * up + math.cos(lat_r) * north
    u = math.cos(lng_r) * t - math.sin(lng_r) * east
    v = math.sin(lng_r) * t + math.cos(lng_r) * east

    return {'u': u, 'v': v, 'w': w}


def enu2ecef(enu, wgs0):
    ecef0 = wgs2ecef(wgs0)
    uvw = enu2uvw(enu['x'], enu['y'], enu['z'], wgs0['lng'], wgs0['lat'])

    return {
        'x': ecef0['x'] + uvw['u'],
        'y': ecef0['y'] + uvw['v'],
        'z': ecef0['z'] + uvw['w'],
    }


def enu2wgs(enu, wgs0):
    ecef = enu2ecef(enu, wgs0)
    return ecef2wgs(ecef['x'], ecef['y'], ecef['z'])


def pos2ecef(lon0, lat0, alt0):
    # print('pos2ecef', lon, lat, alt)
    lon = math.radians(float(lon0))
    lat = math.radians(float(lat0))
    alt = float(alt0)

    cosLon = math.cos(lon)
    cosLat = math.cos(lat)
    sinLon = math.sin(lon)
    sinLat = math.sin(lat)

    N = wgs84_a / math.sqrt(1.0 - pow_e_2 * sinLat * sinLat)

    ecef_x = (N + alt) * cosLat * cosLon
    ecef_y = (N + alt) * cosLat * sinLon
    ecef_z = (N * (1.0 - pow_e_2) + alt) * sinLat

    print('pos2ecef', lon0, lat0, alt0, '->', ecef_x, ecef_y, ecef_z)

    return (ecef_x, ecef_y, ecef_z)


def pos2enu(lon, lat, alt, ref_lon, ref_lat, ref_alt):
    # lon0, lat0, alt0 = p0_lon, p0_lat, p0_alt

    ecef_x1, ecef_y1, ecef_z1 = pos2ecef(ref_lon, ref_lat, ref_alt)
    ecef_x0, ecef_y0, ecef_z0 = pos2ecef(lon, lat, alt)

    # offset_x, offset_y, offset_z = ecef_x1-ecef_x0, ecef_y1-ecef_y0, ecef_z1-ecef_z0
    offset_x, offset_y, offset_z = ecef_x0 - ecef_x1, ecef_y0 - ecef_y1, ecef_z0 - ecef_z1
    # array_delta = np.array( [[offset_x], [offset_y], [offset_z]], dtype=np.float64)
    # print(array_delta.reshape(1,3))

    # lon0 = math.radians(lon)
    # lat0 = math.radians(lat)

    lon0 = math.radians(ref_lon)
    lat0 = math.radians(ref_lat)

    cosLon = math.cos(lon0)
    cosLat = math.cos(lat0)
    sinLon = math.sin(lon0)
    sinLat = math.sin(lat0)

    x = -1 * sinLon * offset_x + cosLon * offset_y
    y = -1 * sinLat * cosLon * offset_x - 1 * sinLat * sinLon * offset_y + cosLat * offset_z
    z = cosLat * cosLon * offset_x + cosLat * sinLon * offset_y + sinLat * offset_z
    return (x, y, z)

    # array_S1 = np.array([-1 * sinLon,           cosLon,                 0], dtype=np.float64)
    # array_S2 = np.array([-1 * sinLat * cosLon,  -1 * sinLat *sinLon,    cosLat], dtype=np.float64)
    # array_S3 = np.array([cosLat * cosLon,       cosLat * sinLon,        sinLat], dtype=np.float64)
    # array_S = np.array([array_S1, array_S2, array_S3], dtype=np.float64)

    # v = np.dot(array_S, array_delta)
    # # print(v.reshape(1,3))
    # # print(v[0][0], v[1][0], v[2][0])
    # return (v[0][0], v[1][0], v[2][0])


# Example usage
enu = {'x': -16.47, 'y': 8.667, 'z': 0}
wgs0 = {'lng': 121.5442525290947, 'lat': 31.229345976263158, 'alt': 0}
result = enu2wgs(enu, wgs0)
print(result)

p0_lon, p0_lat, p0_alt = 121.5442525290947, 31.229345976263158, 0
p1_lon, p1_lat, p1_alt = result[1], result[0], result[2]
v1 = pos2enu(p1_lon, p1_lat, p1_alt, p0_lon, p0_lat, p0_alt)  # p0_lon, p0_lat, p0_alt为基准坐标
print(v1)