Spaces:
Running
Running
| from __future__ import annotations | |
| from dataclasses import dataclass | |
| import numpy as np | |
| class TriangulationResult: | |
| module_a: np.ndarray | |
| module_b: np.ndarray | |
| direction_a: np.ndarray | |
| direction_b: np.ndarray | |
| closest_a: np.ndarray | |
| closest_b: np.ndarray | |
| estimated_position: np.ndarray | |
| residual_m: float | |
| range_a_m: float | |
| range_b_m: float | |
| def direction_from_az_el(azimuth_deg: float, elevation_deg: float) -> np.ndarray: | |
| """Convert ENU azimuth/elevation angles to a unit direction vector.""" | |
| az = np.deg2rad(azimuth_deg) | |
| el = np.deg2rad(elevation_deg) | |
| direction = np.array( | |
| [ | |
| np.cos(el) * np.sin(az), | |
| np.cos(el) * np.cos(az), | |
| np.sin(el), | |
| ], | |
| dtype=np.float64, | |
| ) | |
| return direction / np.linalg.norm(direction) | |
| def az_el_from_points(origin: np.ndarray, target: np.ndarray) -> tuple[float, float]: | |
| """Return ENU azimuth/elevation from origin to target.""" | |
| delta = np.asarray(target, dtype=np.float64) - np.asarray(origin, dtype=np.float64) | |
| horizontal = np.hypot(delta[0], delta[1]) | |
| azimuth = (np.rad2deg(np.arctan2(delta[0], delta[1])) + 360.0) % 360.0 | |
| elevation = np.rad2deg(np.arctan2(delta[2], horizontal)) | |
| return float(azimuth), float(elevation) | |
| def closest_points_between_rays( | |
| origin_a: np.ndarray, | |
| direction_a: np.ndarray, | |
| origin_b: np.ndarray, | |
| direction_b: np.ndarray, | |
| ) -> tuple[np.ndarray, np.ndarray, float, float]: | |
| """Find the closest points between two 3D rays.""" | |
| p = np.asarray(origin_a, dtype=np.float64) | |
| q = np.asarray(origin_b, dtype=np.float64) | |
| u = np.asarray(direction_a, dtype=np.float64) | |
| v = np.asarray(direction_b, dtype=np.float64) | |
| u = u / np.linalg.norm(u) | |
| v = v / np.linalg.norm(v) | |
| w = p - q | |
| a = float(np.dot(u, u)) | |
| b = float(np.dot(u, v)) | |
| c = float(np.dot(v, v)) | |
| d = float(np.dot(u, w)) | |
| e = float(np.dot(v, w)) | |
| denom = a * c - b * b | |
| if abs(denom) < 1e-9: | |
| t = 0.0 | |
| s = max(0.0, e / c) | |
| else: | |
| t = (b * e - c * d) / denom | |
| s = (a * e - b * d) / denom | |
| t = max(0.0, t) | |
| s = max(0.0, s) | |
| closest_a = p + t * u | |
| closest_b = q + s * v | |
| return closest_a, closest_b, float(t), float(s) | |
| def triangulate_from_az_el( | |
| module_a: np.ndarray, | |
| azimuth_a_deg: float, | |
| elevation_a_deg: float, | |
| module_b: np.ndarray, | |
| azimuth_b_deg: float, | |
| elevation_b_deg: float, | |
| ) -> TriangulationResult: | |
| """Triangulate a 3D position from two module rays.""" | |
| direction_a = direction_from_az_el(azimuth_a_deg, elevation_a_deg) | |
| direction_b = direction_from_az_el(azimuth_b_deg, elevation_b_deg) | |
| closest_a, closest_b, range_a_m, range_b_m = closest_points_between_rays( | |
| module_a, | |
| direction_a, | |
| module_b, | |
| direction_b, | |
| ) | |
| estimated = (closest_a + closest_b) / 2.0 | |
| residual = float(np.linalg.norm(closest_a - closest_b)) | |
| return TriangulationResult( | |
| module_a=np.asarray(module_a, dtype=np.float64), | |
| module_b=np.asarray(module_b, dtype=np.float64), | |
| direction_a=direction_a, | |
| direction_b=direction_b, | |
| closest_a=closest_a, | |
| closest_b=closest_b, | |
| estimated_position=estimated, | |
| residual_m=residual, | |
| range_a_m=range_a_m, | |
| range_b_m=range_b_m, | |
| ) | |