Fix geometry lla2ecef and ecef2lla

docker-compose.yml

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

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()

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

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

event/requirements.txt

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

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()