From 7ddd8ad533e8d0cab9ae982431c1f4098bd97209 Mon Sep 17 00:00:00 2001 From: 30hours Date: Wed, 28 Feb 2024 12:00:09 +0000 Subject: [PATCH] Fix geometry lla2ecef and ecef2lla --- docker-compose.yml | 1 + .../algorithm/coordreg/EllipsoidParametric.py | 16 +++- event/algorithm/geometry/Geometry.py | 92 ++++++++++++++++++- event/event.py | 7 +- event/requirements.txt | 3 +- test/event/TestGeometry.py | 35 +++++++ 6 files changed, 150 insertions(+), 4 deletions(-) create mode 100644 test/event/TestGeometry.py diff --git a/docker-compose.yml b/docker-compose.yml index ecee4d9..3649269 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -29,6 +29,7 @@ services: - 3lips volumes: - ./common:/app/common + - ./test:/app/test container_name: 3lips-event cesium-apache: diff --git a/event/algorithm/coordreg/EllipsoidParametric.py b/event/algorithm/coordreg/EllipsoidParametric.py index 464afbc..0577b01 100644 --- a/event/algorithm/coordreg/EllipsoidParametric.py +++ b/event/algorithm/coordreg/EllipsoidParametric.py @@ -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() \ No newline at end of file diff --git a/event/algorithm/geometry/Geometry.py b/event/algorithm/geometry/Geometry.py index 5855f32..29d766b 100644 --- a/event/algorithm/geometry/Geometry.py +++ b/event/algorithm/geometry/Geometry.py @@ -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 \ No newline at end of file diff --git a/event/event.py b/event/event.py index 93f758f..a556dec 100644 --- a/event/event.py +++ b/event/event.py @@ -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 diff --git a/event/requirements.txt b/event/requirements.txt index fcdfabb..9f4f994 100644 --- a/event/requirements.txt +++ b/event/requirements.txt @@ -1,2 +1,3 @@ numpy==1.26.4 -requests==2.31.0 \ No newline at end of file +requests==2.31.0 +pyproj==3.6.1 \ No newline at end of file diff --git a/test/event/TestGeometry.py b/test/event/TestGeometry.py new file mode 100644 index 0000000..dcaeb01 --- /dev/null +++ b/test/event/TestGeometry.py @@ -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()