mirror of
https://github.com/30hours/3lips.git
synced 2024-11-18 12:33:58 +00:00
Fix geometry lla2ecef and ecef2lla
This commit is contained in:
parent
a17b49149a
commit
7ddd8ad533
6 changed files with 150 additions and 4 deletions
|
@ -29,6 +29,7 @@ services:
|
||||||
- 3lips
|
- 3lips
|
||||||
volumes:
|
volumes:
|
||||||
- ./common:/app/common
|
- ./common:/app/common
|
||||||
|
- ./test:/app/test
|
||||||
container_name: 3lips-event
|
container_name: 3lips-event
|
||||||
|
|
||||||
cesium-apache:
|
cesium-apache:
|
||||||
|
|
|
@ -6,6 +6,7 @@
|
||||||
from data.Ellipsoid import Ellipsoid
|
from data.Ellipsoid import Ellipsoid
|
||||||
from algorithm.geometry.Geometry import Geometry
|
from algorithm.geometry.Geometry import Geometry
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
import math
|
||||||
|
|
||||||
class EllipsoidParametric:
|
class EllipsoidParametric:
|
||||||
|
|
||||||
|
@ -93,7 +94,7 @@ class EllipsoidParametric:
|
||||||
"""
|
"""
|
||||||
|
|
||||||
# rotation matrix
|
# rotation matrix
|
||||||
phi = ellipsoid.pitch
|
phi = math.pi/2 - ellipsoid.pitch
|
||||||
theta = ellipsoid.yaw
|
theta = ellipsoid.yaw
|
||||||
R = np.array([
|
R = np.array([
|
||||||
[np.cos(phi)*np.cos(theta), -np.sin(phi)*np.cos(theta), np.sin(theta)],
|
[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
|
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()
|
return r_1.tolist()
|
|
@ -5,6 +5,7 @@
|
||||||
|
|
||||||
import math
|
import math
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
import pyproj
|
||||||
|
|
||||||
class Geometry:
|
class Geometry:
|
||||||
|
|
||||||
|
@ -24,6 +25,7 @@ class Geometry:
|
||||||
# WGS84 constants
|
# WGS84 constants
|
||||||
a = 6378137.0 # semi-major axis in meters
|
a = 6378137.0 # semi-major axis in meters
|
||||||
f = 1 / 298.257223563 # flattening
|
f = 1 / 298.257223563 # flattening
|
||||||
|
e = 0.081819190842622
|
||||||
|
|
||||||
# Convert latitude and longitude to radians
|
# Convert latitude and longitude to radians
|
||||||
lat_rad = math.radians(latitude)
|
lat_rad = math.radians(latitude)
|
||||||
|
@ -37,6 +39,94 @@ class Geometry:
|
||||||
# Calculate ECEF coordinates
|
# Calculate ECEF coordinates
|
||||||
ecef_x = (N + altitude) * cos_lat * math.cos(lon_rad)
|
ecef_x = (N + altitude) * cos_lat * math.cos(lon_rad)
|
||||||
ecef_y = (N + altitude) * cos_lat * math.sin(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
|
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
|
|
@ -123,7 +123,12 @@ async def event():
|
||||||
[x_rx, y_rx, z_rx],
|
[x_rx, y_rx, z_rx],
|
||||||
'radar4.30hours.dev'
|
'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
|
# output data to API
|
||||||
item["detections_associated"] = associated_dets
|
item["detections_associated"] = associated_dets
|
||||||
|
|
|
@ -1,2 +1,3 @@
|
||||||
numpy==1.26.4
|
numpy==1.26.4
|
||||||
requests==2.31.0
|
requests==2.31.0
|
||||||
|
pyproj==3.6.1
|
35
test/event/TestGeometry.py
Normal file
35
test/event/TestGeometry.py
Normal 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()
|
Loading…
Reference in a new issue