Fix geometry lla2ecef and ecef2lla

This commit is contained in:
30hours 2024-02-28 12:00:09 +00:00
parent a17b49149a
commit 7ddd8ad533
6 changed files with 150 additions and 4 deletions

View file

@ -29,6 +29,7 @@ services:
- 3lips
volumes:
- ./common:/app/common
- ./test:/app/test
container_name: 3lips-event
cesium-apache:

View file

@ -6,6 +6,7 @@
from data.Ellipsoid import Ellipsoid
from algorithm.geometry.Geometry import Geometry
import numpy as np
import math
class EllipsoidParametric:
@ -93,7 +94,7 @@ class EllipsoidParametric:
"""
# rotation matrix
phi = ellipsoid.pitch
phi = math.pi/2 - ellipsoid.pitch
theta = ellipsoid.yaw
R = np.array([
[np.cos(phi)*np.cos(theta), -np.sin(phi)*np.cos(theta), np.sin(theta)],
@ -114,4 +115,17 @@ class EllipsoidParametric:
r_1 = np.dot(r, R) + ellipsoid.midpoint
lat, lon, alt = Geometry.ecef2lla(ellipsoid.midpoint[0],
ellipsoid.midpoint[1], ellipsoid.midpoint[2])
print(lat, flush=True)
print(lon, flush=True)
print(alt, flush=True)
lat, lon, alt = Geometry.ecef2lla(ellipsoid.f1[0],
ellipsoid.f1[1], ellipsoid.f1[2])
print(ellipsoid.f1, flush=True)
print(lat, flush=True)
print(lon, flush=True)
print(alt, flush=True)
return r_1.tolist()

View file

@ -5,6 +5,7 @@
import math
import numpy as np
import pyproj
class Geometry:
@ -24,6 +25,7 @@ class Geometry:
# WGS84 constants
a = 6378137.0 # semi-major axis in meters
f = 1 / 298.257223563 # flattening
e = 0.081819190842622
# Convert latitude and longitude to radians
lat_rad = math.radians(latitude)
@ -37,6 +39,94 @@ class Geometry:
# Calculate ECEF coordinates
ecef_x = (N + altitude) * cos_lat * math.cos(lon_rad)
ecef_y = (N + altitude) * cos_lat * math.sin(lon_rad)
ecef_z = (N * (1 - f) + altitude) * sin_lat
#ecef_z = (N * (1 - f) + altitude) * sin_lat
ecef_z = ((1-(e**2)) * N + altitude) * sin_lat
return ecef_x, ecef_y, ecef_z
# def ecef2lla(x, y, z):
# # WGS84 parameters
# a = 6378137.0 # semi-major axis in meters
# f = 1 / 298.257223563 # flattening
# b = (1 - f) * a # semi-minor axis
# # Calculate eccentricity squared
# e_squared = (a**2 - b**2) / a**2
# # Calculate longitude
# lon = math.atan2(y, x)
# # Calculate distance from the origin to the XY plane
# r = math.sqrt(x**2 + y**2)
# # Calculate latitude
# lat = math.atan2(z, r)
# # Calculate altitude
# sin_lat = math.sin(lat)
# N = a / math.sqrt(1 - e_squared * sin_lat**2)
# alt = r / math.cos(lat) - N
# return math.degrees(lat), math.degrees(lon), alt
# def ecef2lla(x, y, z):
# # WGS84 ellipsoid constants
# a = 6378137
# es = (8.1819190842622e-2) ** 2
# # Calculations
# b = np.sqrt(a ** 2 * (1 - es))
# ep = (a ** 2 - b ** 2) / b ** 2
# p = np.sqrt(x ** 2 + y ** 2)
# th = np.arctan2(a * z, b * p)
# lon = np.arctan2(y, x)
# lat = np.arctan2(z + ep ** 2 * b * np.sin(th) ** 3, p - es ** 2 * a * np.cos(th) ** 3)
# N = a / np.sqrt(1 - es * np.sin(lat) ** 2)
# alt = p / np.cos(lat) - N
# # Return lon in range [0, 2*pi)
# lon = np.mod(lon, 2 * np.pi)
# # Correct for numerical instability in altitude near exact poles
# # (after this correction, error is about 2 millimeters, which is about
# # the same as the numerical precision of the overall function)
# k = np.abs(x) < 1e-3 # Use x for the condition
# alt = np.where(k, np.abs(z) - b, alt)
# # Convert radians to degrees
# lat = lat * (180 / np.pi)
# lon = lon * (180 / np.pi)
# return lat, lon, alt
def ecef2lla(x, y, z):
# WGS84 ellipsoid constants:
a = 6378137
e = 8.1819190842622e-2
# Calculations:
b = np.sqrt(a**2 * (1 - e**2))
ep = np.sqrt((a**2 - b**2) / b**2)
p = np.sqrt(x**2 + y**2)
th = np.arctan2(a * z, b * p)
lon = np.arctan2(y, x)
lat = np.arctan2((z + ep**2 * b * np.sin(th)**3), (p - e**2 * a * np.cos(th)**3))
N = a / np.sqrt(1 - e**2 * np.sin(lat)**2)
alt = p / np.cos(lat) - N
# Return lon in range [0, 2*pi)
lon = np.mod(lon, 2 * np.pi)
# Correct for numerical instability in altitude near exact poles:
# (after this correction, error is about 2 millimeters, which is about
# the same as the numerical precision of the overall function)
k = np.logical_and(np.abs(x) < 1e-10, np.abs(y) < 1e-10)
alt = np.where(k, np.abs(z) - b, alt)
lat = np.degrees(lat)
lon = np.degrees(lon)
return lat, lon, alt

View file

@ -123,7 +123,12 @@ async def event():
[x_rx, y_rx, z_rx],
'radar4.30hours.dev'
)
localised_dets["test"]["points"] = ellipsoidParametric.sample(ellipsoid, 10000, 5)
pointsEcef = ellipsoidParametric.sample(ellipsoid, 10000, 5)
pointsLla = []
for point in pointsEcef:
lat, lon, alt = Geometry.ecef2lla(point[0], point[1], point[2])
pointsLla.append([lat, lon, alt])
localised_dets["test"]["points"] = pointsLla
# output data to API
item["detections_associated"] = associated_dets

View file

@ -1,2 +1,3 @@
numpy==1.26.4
requests==2.31.0
pyproj==3.6.1

View file

@ -0,0 +1,35 @@
import unittest
from algorithm.geometry.Geometry import Geometry
class TestGeometry(unittest.TestCase):
def test_lla2ecef(self):
# Test case 1
result = Geometry.lla2ecef(-34.9286, 138.5999, 50)
self.assertAlmostEqual(result[0], -3926830.77177051, places=3)
self.assertAlmostEqual(result[1], 3461979.19806774, places=3)
self.assertAlmostEqual(result[2], -3631404.11418915, places=3)
# Test case 2
result = Geometry.lla2ecef(0, 0, 0)
self.assertAlmostEqual(result[0], 6378137.0, places=3)
self.assertAlmostEqual(result[1], 0, places=3)
self.assertAlmostEqual(result[2], 0, places=3)
# Add more test cases as needed
def test_ecef2lla(self):
# Test case 1
result = Geometry.ecef2lla(-3926830.77177051, 3461979.19806774, -3631404.11418915)
self.assertAlmostEqual(result[0], -34.9286, places=4)
self.assertAlmostEqual(result[1], 138.5999, places=4)
self.assertAlmostEqual(result[2], 50, places=3)
# Test case 2
result = Geometry.ecef2lla(6378137.0, 0, 0)
self.assertAlmostEqual(result[0], 0, places=4)
self.assertAlmostEqual(result[1], 0, places=4)
self.assertAlmostEqual(result[2], 0, places=3)
if __name__ == '__main__':
unittest.main()