From aace56fad50bc61edd418dd76277fa63998cbe96 Mon Sep 17 00:00:00 2001 From: Benjamin Pelletier Date: Sun, 14 Jun 2026 22:01:52 +0000 Subject: [PATCH 1/3] Improve accuracy of geospatial point translation --- .basedpyright/baseline.json | 16 --- monitoring/monitorlib/geo.py | 141 +++++++++++++++++++---- monitoring/prober/monitorlib/test_geo.py | 55 +++++++++ 3 files changed, 172 insertions(+), 40 deletions(-) diff --git a/.basedpyright/baseline.json b/.basedpyright/baseline.json index 03b1506a3b..fcffc813fd 100644 --- a/.basedpyright/baseline.json +++ b/.basedpyright/baseline.json @@ -4173,14 +4173,6 @@ "lineCount": 1 } }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 49, - "endColumn": 78, - "lineCount": 1 - } - }, { "code": "reportOptionalMemberAccess", "range": { @@ -4189,14 +4181,6 @@ "lineCount": 1 } }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 58, - "endColumn": 87, - "lineCount": 1 - } - }, { "code": "reportOptionalIterable", "range": { diff --git a/monitoring/monitorlib/geo.py b/monitoring/monitorlib/geo.py index defe408c90..b6dc8e3cde 100644 --- a/monitoring/monitorlib/geo.py +++ b/monitoring/monitorlib/geo.py @@ -365,31 +365,62 @@ def transform(self, transformation: Transformation) -> Volume3D: ) def translate_relative(self, translation: RelativeTranslation) -> Volume3D: - def offset(p0: LatLngPoint, p: LatLngPoint) -> LatLngPoint: - s2_p0 = p0.as_s2sphere() - xy = flatten(s2_p0, p.as_s2sphere()) - if "meters_east" in translation and translation.meters_east: - xy = (xy[0] + translation.meters_east, xy[1]) - if "meters_north" in translation and translation.meters_north: - xy = (xy[0], xy[1] + translation.meters_north) - p1 = LatLngPoint.from_s2(unflatten(s2_p0, xy)) - if "degrees_east" in translation and translation.degrees_east: - p1.lng += translation.degrees_east - if "degrees_north" in translation and translation.degrees_north: - p1.lat += translation.degrees_north - return p1 + if self.outline_polygon is not None: + src_center = self.outline_polygon.vertex_average() + elif self.outline_circle is not None: + src_center = self.outline_circle.center + else: + raise ValueError("Neither outline_circle nor outline_polygon specified") + + meters_east = ( + translation.meters_east + if "meters_east" in translation and translation.meters_east is not None + else 0.0 + ) + meters_north = ( + translation.meters_north + if "meters_north" in translation and translation.meters_north is not None + else 0.0 + ) + degrees_east = ( + translation.degrees_east + if "degrees_east" in translation and translation.degrees_east is not None + else 0.0 + ) + degrees_north = ( + translation.degrees_north + if "degrees_north" in translation and translation.degrees_north is not None + else 0.0 + ) + + dst_center = src_center.offset(meters_east, meters_north) + dst_center.lng += degrees_east + dst_center.lat += degrees_north + + r_matrix = make_rotation_matrix( + src_center.as_s2sphere(), dst_center.as_s2sphere() + ) kwargs = {k: v for k, v in self.items() if v is not None} if self.outline_circle is not None: + new_circle_center = LatLngPoint.from_s2( + apply_rotation(r_matrix, self.outline_circle.center.as_s2sphere()) + ) kwargs["outline_circle"] = Circle( - center=offset(self.outline_circle.center, self.outline_circle.center), + center=new_circle_center, radius=self.outline_circle.radius, ) - if self.outline_polygon is not None: - ref0 = self.outline_polygon.vertex_average() - vertices = [offset(ref0, p) for p in self.outline_polygon.vertices] + if ( + self.outline_polygon is not None + and self.outline_polygon.vertices is not None + ): + vertices = [ + LatLngPoint.from_s2(apply_rotation(r_matrix, p.as_s2sphere())) + for p in self.outline_polygon.vertices + ] kwargs["outline_polygon"] = Polygon(vertices=vertices) result = Volume3D(**kwargs) + if "meters_up" in translation and translation.meters_up: if result.altitude_lower: if result.altitude_lower.units == DistanceUnits.M: @@ -411,16 +442,34 @@ def translate_absolute(self, translation: AbsoluteTranslation) -> Volume3D: new_center = LatLngPoint( lat=translation.new_latitude, lng=translation.new_longitude ) + + if self.outline_polygon is not None: + src_center = self.outline_polygon.vertex_average() + elif self.outline_circle is not None: + src_center = self.outline_circle.center + else: + raise ValueError("Neither outline_circle nor outline_polygon specified") + + r_matrix = make_rotation_matrix( + src_center.as_s2sphere(), new_center.as_s2sphere() + ) + kwargs = {k: v for k, v in self.items() if v is not None} if self.outline_circle is not None: + new_circle_center = LatLngPoint.from_s2( + apply_rotation(r_matrix, self.outline_circle.center.as_s2sphere()) + ) kwargs["outline_circle"] = Circle( - center=new_center, radius=self.outline_circle.radius + center=new_circle_center, radius=self.outline_circle.radius ) - if self.outline_polygon is not None: - ref0 = self.outline_polygon.vertex_average().as_s2sphere() - xy = [flatten(ref0, p.as_s2sphere()) for p in self.outline_polygon.vertices] - ref1 = new_center.as_s2sphere() - vertices = [LatLngPoint.from_s2(unflatten(ref1, p)) for p in xy] + if ( + self.outline_polygon is not None + and self.outline_polygon.vertices is not None + ): + vertices = [ + LatLngPoint.from_s2(apply_rotation(r_matrix, p.as_s2sphere())) + for p in self.outline_polygon.vertices + ] kwargs["outline_polygon"] = Polygon(vertices=vertices) return Volume3D(**kwargs) @@ -552,7 +601,7 @@ def make_latlng_rect(area) -> s2sphere.LatLngRect: lng_max = max(v.lng for v in area.outline_polygon.vertices) elif "outline_circle" in area and area.outline_circle: p0 = s2sphere.LatLng.from_degrees( - area.outline_circle.center.lng, area.outline_circle.center.lat + area.outline_circle.center.lat, area.outline_circle.center.lng ) lat_min = ( unflatten(p0, (0, -area.outline_circle.radius.value)).lat().degrees @@ -605,6 +654,50 @@ def validate_lng(lng: str | float) -> float: return lng +def make_rotation_matrix(src: s2sphere.LatLng, dst: s2sphere.LatLng) -> np.ndarray: + """Computes the 3D rotation matrix that rotates vector src to vector dst on a unit sphere.""" + p_src = src.to_point() + p_dst = dst.to_point() + + a = np.array([p_src[0], p_src[1], p_src[2]]) + b = np.array([p_dst[0], p_dst[1], p_dst[2]]) + + v = np.cross(a, b) + c = np.dot(a, b) + + if np.allclose(v, 0): + if c > 0: + return np.eye(3) + else: + # Antipodal rotation. Choose an arbitrary perpendicular axis. + if not np.allclose(a[1:], 0): + axis = np.cross(a, [1, 0, 0]) + else: + axis = np.cross(a, [0, 1, 0]) + axis = axis / np.linalg.norm(axis) + kmat = np.array( + [[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]] + ) + return np.eye(3) + 2 * np.dot(kmat, kmat) + + kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) + + return np.eye(3) + kmat + np.dot(kmat, kmat) * (1.0 / (1.0 + c)) + + +def apply_rotation(r_matrix: np.ndarray, point: s2sphere.LatLng) -> s2sphere.LatLng: + """Applies a 3D rotation matrix to a LatLng point on the sphere.""" + pt = point.to_point() + v_arr = np.array([pt[0], pt[1], pt[2]]) + rotated_arr = r_matrix.dot(v_arr) + # Re-normalize to ensure the point is on the unit sphere + norm = np.linalg.norm(rotated_arr) + if norm > 0: + rotated_arr = rotated_arr / norm + rotated_pt = s2sphere.Point(rotated_arr[0], rotated_arr[1], rotated_arr[2]) + return s2sphere.LatLng.from_point(rotated_pt) + + def flatten(reference: s2sphere.LatLng, point: s2sphere.LatLng) -> tuple[float, float]: """Locally flatten a lat-lng point to (dx, dy) in meters from reference.""" return ( diff --git a/monitoring/prober/monitorlib/test_geo.py b/monitoring/prober/monitorlib/test_geo.py index 8797789c41..edc1203639 100644 --- a/monitoring/prober/monitorlib/test_geo.py +++ b/monitoring/prober/monitorlib/test_geo.py @@ -15,3 +15,58 @@ def test_flatten_unflatten(): p1 = geo.unflatten(ref, xy) assert abs(p.lat().degrees - p1.lat().degrees) < 1e-9 assert abs(p.lng().degrees - p1.lng().degrees) < 1e-9 + + +def test_rotation_translation(): + p_ref = s2sphere.LatLng.from_degrees(45.0, 9.0) + p_tgt = s2sphere.LatLng.from_degrees(46.0, 10.0) + + r_matrix = geo.make_rotation_matrix(p_ref, p_tgt) + + p_ref_rotated = geo.apply_rotation(r_matrix, p_ref) + assert abs(p_ref_rotated.lat().degrees - p_tgt.lat().degrees) < 1e-9 + assert abs(p_ref_rotated.lng().degrees - p_tgt.lng().degrees) < 1e-9 + + from monitoring.monitorlib.geo import LatLngPoint, Polygon, Volume3D + from monitoring.monitorlib.transformations import ( + AbsoluteTranslation, + RelativeTranslation, + ) + + poly = Polygon( + vertices=[ + LatLngPoint(lat=45.0, lng=9.0), + LatLngPoint(lat=45.1, lng=9.0), + LatLngPoint(lat=45.1, lng=9.1), + LatLngPoint(lat=45.0, lng=9.1), + ] + ) + vol = Volume3D(outline_polygon=poly) + + # Translate relative + rel_trans = RelativeTranslation(meters_east=1000, meters_north=2000) + vol_rel = vol.translate_relative(rel_trans) + + # Translate absolute + abs_trans = AbsoluteTranslation(new_latitude=46.0, new_longitude=10.0) + vol_abs = vol.translate_absolute(abs_trans) + + # Verify that the average vertex is moved correctly + assert vol_rel.outline_polygon is not None + avg_rel = vol_rel.outline_polygon.vertex_average() + expected_rel = poly.vertex_average().offset(1000, 2000) + assert abs(avg_rel.lat - expected_rel.lat) < 1e-6 + assert abs(avg_rel.lng - expected_rel.lng) < 1e-6 + + assert vol_abs.outline_polygon is not None + avg_abs = vol_abs.outline_polygon.vertex_average() + assert abs(avg_abs.lat - 46.0) < 1e-6 + assert abs(avg_abs.lng - 10.0) < 1e-6 + + +def test_make_latlng_rect(): + from monitoring.monitorlib.geo import Circle, Volume3D, make_latlng_rect + + circle_vol = Volume3D(outline_circle=Circle.from_meters(34.0, -118.0, 1000.0)) + rect = make_latlng_rect(circle_vol) + assert rect.contains(s2sphere.LatLng.from_degrees(34.0, -118.0)) From 9d2bc1e4c1fe87079c6105b11228a737e34205b6 Mon Sep 17 00:00:00 2001 From: Benjamin Pelletier Date: Mon, 15 Jun 2026 00:07:03 +0000 Subject: [PATCH 2/3] Fix transformation batching on FlightIntentsCollection --- .basedpyright/baseline.json | 120 -------------- .../flight_planning/flight_info_template.py | 2 +- monitoring/monitorlib/geo.py | 136 ++++++++++++++-- monitoring/monitorlib/geo_test.py | 69 ++++++++ monitoring/monitorlib/geotemporal.py | 2 +- monitoring/monitorlib/transformations.py | 38 ----- monitoring/prober/monitorlib/__init__.py | 0 monitoring/prober/monitorlib/test_geo.py | 72 --------- .../flight_planning/flight_intent.py | 36 ++++- .../flight_planning/flight_intent_test.py | 153 ++++++++++++++++++ .../FlightInfoTemplate.json | 2 +- .../AbsoluteTranslation.json | 15 +- .../monitoring/monitorlib/geo/Polygon.json | 8 +- .../RelativeTranslation.json | 15 +- .../Transformation.json | 4 +- .../geotemporal/Volume4DTemplate.json | 2 +- .../flight_intent/FlightIntentCollection.json | 2 +- .../FlightIntentsSpecification.json | 2 +- 18 files changed, 414 insertions(+), 264 deletions(-) delete mode 100644 monitoring/monitorlib/transformations.py delete mode 100644 monitoring/prober/monitorlib/__init__.py delete mode 100644 monitoring/prober/monitorlib/test_geo.py create mode 100644 monitoring/uss_qualifier/resources/flight_planning/flight_intent_test.py rename schemas/monitoring/monitorlib/{transformations => geo}/AbsoluteTranslation.json (63%) rename schemas/monitoring/monitorlib/{transformations => geo}/RelativeTranslation.json (73%) rename schemas/monitoring/monitorlib/{transformations => geo}/Transformation.json (82%) diff --git a/.basedpyright/baseline.json b/.basedpyright/baseline.json index fcffc813fd..fbebc2a460 100644 --- a/.basedpyright/baseline.json +++ b/.basedpyright/baseline.json @@ -4045,38 +4045,6 @@ } ], "./monitoring/monitorlib/geo.py": [ - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 33, - "endColumn": 46, - "lineCount": 1 - } - }, - { - "code": "reportArgumentType", - "range": { - "startColumn": 54, - "endColumn": 67, - "lineCount": 1 - } - }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 33, - "endColumn": 46, - "lineCount": 1 - } - }, - { - "code": "reportArgumentType", - "range": { - "startColumn": 54, - "endColumn": 67, - "lineCount": 1 - } - }, { "code": "reportAttributeAccessIssue", "range": { @@ -4149,30 +4117,6 @@ "lineCount": 1 } }, - { - "code": "reportOptionalSubscript", - "range": { - "startColumn": 16, - "endColumn": 47, - "lineCount": 1 - } - }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 25, - "endColumn": 56, - "lineCount": 1 - } - }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 25, - "endColumn": 56, - "lineCount": 1 - } - }, { "code": "reportOptionalMemberAccess", "range": { @@ -4181,14 +4125,6 @@ "lineCount": 1 } }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 56, - "endColumn": 85, - "lineCount": 1 - } - }, { "code": "reportAttributeAccessIssue", "range": { @@ -4253,46 +4189,6 @@ "lineCount": 1 } }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 41, - "endColumn": 70, - "lineCount": 1 - } - }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 41, - "endColumn": 70, - "lineCount": 1 - } - }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 41, - "endColumn": 70, - "lineCount": 1 - } - }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 41, - "endColumn": 70, - "lineCount": 1 - } - }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 54, - "endColumn": 67, - "lineCount": 1 - } - }, { "code": "reportReturnType", "range": { @@ -4375,14 +4271,6 @@ "lineCount": 1 } }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 21, - "endColumn": 57, - "lineCount": 1 - } - }, { "code": "reportArgumentType", "range": { @@ -4495,14 +4383,6 @@ "lineCount": 1 } }, - { - "code": "reportOptionalIterable", - "range": { - "startColumn": 25, - "endColumn": 61, - "lineCount": 1 - } - }, { "code": "reportArgumentType", "range": { diff --git a/monitoring/monitorlib/clients/flight_planning/flight_info_template.py b/monitoring/monitorlib/clients/flight_planning/flight_info_template.py index 51c8aa16e8..fbd6360d35 100644 --- a/monitoring/monitorlib/clients/flight_planning/flight_info_template.py +++ b/monitoring/monitorlib/clients/flight_planning/flight_info_template.py @@ -10,12 +10,12 @@ RPAS26FlightDetails, UasState, ) +from monitoring.monitorlib.geo import Transformation from monitoring.monitorlib.geotemporal import ( Volume4DCollection, Volume4DTemplateCollection, ) from monitoring.monitorlib.temporal import TestTimeContext -from monitoring.monitorlib.transformations import Transformation class BasicFlightPlanInformationTemplate(ImplicitDict): diff --git a/monitoring/monitorlib/geo.py b/monitoring/monitorlib/geo.py index b6dc8e3cde..ac0f0cfa81 100644 --- a/monitoring/monitorlib/geo.py +++ b/monitoring/monitorlib/geo.py @@ -2,6 +2,7 @@ import math import os +from collections.abc import Iterable from enum import StrEnum import numpy as np @@ -22,12 +23,6 @@ injection as f3411testing_injection, ) -from monitoring.monitorlib.transformations import ( - AbsoluteTranslation, - RelativeTranslation, - Transformation, -) - EARTH_CIRCUMFERENCE_KM = 40075 EARTH_CIRCUMFERENCE_M = EARTH_CIRCUMFERENCE_KM * 1000 EARTH_RADIUS_M = 40075 * 1000 / (2 * math.pi) @@ -110,7 +105,7 @@ def in_meters(self) -> float: class Polygon(ImplicitDict): - vertices: Optional[list[LatLngPoint]] + vertices: list[LatLngPoint] def vertex_average(self) -> LatLngPoint: lat = sum(p.lat for p in self.vertices) / len(self.vertices) @@ -264,6 +259,49 @@ def is_equivalent(self, other: Altitude) -> bool: ) +class RelativeTranslation(ImplicitDict): + """Offset a geo feature by a particular amount.""" + + meters_east: Optional[float] + """Number of meters east to translate.""" + + meters_north: Optional[float] + """Number of meters north to translate.""" + + meters_up: Optional[float] + """Number of meters upward to translate.""" + + degrees_east: Optional[float] + """Number of degrees of longitude east to translate.""" + + degrees_north: Optional[float] + """Number of degrees of latitude north to translate.""" + + reference_center: Optional[LatLngPoint] + """The center around which the translation is defined/rotated.""" + + +class AbsoluteTranslation(ImplicitDict): + """Move a geo feature to a specified location.""" + + new_latitude: float + """The new latitude at which the feature should be located (degrees).""" + + new_longitude: float + """The new longitude at which the feature should be located (degrees).""" + + reference_center: Optional[LatLngPoint] + """The center around which the translation is defined/rotated.""" + + +class Transformation(ImplicitDict): + """A transformation to apply to a geotemporal feature. Exactly one field must be specified.""" + + relative_translation: Optional[RelativeTranslation] + + absolute_translation: Optional[AbsoluteTranslation] + + class Volume3D(ImplicitDict): outline_circle: Optional[Circle] = None outline_polygon: Optional[Polygon] = None @@ -365,12 +403,18 @@ def transform(self, transformation: Transformation) -> Volume3D: ) def translate_relative(self, translation: RelativeTranslation) -> Volume3D: - if self.outline_polygon is not None: - src_center = self.outline_polygon.vertex_average() - elif self.outline_circle is not None: - src_center = self.outline_circle.center + if ( + "reference_center" in translation + and translation.reference_center is not None + ): + src_center = translation.reference_center else: - raise ValueError("Neither outline_circle nor outline_polygon specified") + if self.outline_polygon is not None: + src_center = self.outline_polygon.vertex_average() + elif self.outline_circle is not None: + src_center = self.outline_circle.center + else: + raise ValueError("Neither outline_circle nor outline_polygon specified") meters_east = ( translation.meters_east @@ -443,12 +487,18 @@ def translate_absolute(self, translation: AbsoluteTranslation) -> Volume3D: lat=translation.new_latitude, lng=translation.new_longitude ) - if self.outline_polygon is not None: - src_center = self.outline_polygon.vertex_average() - elif self.outline_circle is not None: - src_center = self.outline_circle.center + if ( + "reference_center" in translation + and translation.reference_center is not None + ): + src_center = translation.reference_center else: - raise ValueError("Neither outline_circle nor outline_polygon specified") + if self.outline_polygon is not None: + src_center = self.outline_polygon.vertex_average() + elif self.outline_circle is not None: + src_center = self.outline_circle.center + else: + raise ValueError("Neither outline_circle nor outline_polygon specified") r_matrix = make_rotation_matrix( src_center.as_s2sphere(), new_center.as_s2sphere() @@ -698,6 +748,58 @@ def apply_rotation(r_matrix: np.ndarray, point: s2sphere.LatLng) -> s2sphere.Lat return s2sphere.LatLng.from_point(rotated_pt) +def bind_transformations( + transformations: Iterable[Transformation], src_center: LatLngPoint +) -> None: + """Binds a single explicit reference center for each transformation in a sequence + so all elements in the collection are transformed rigidly relative to that same center. + """ + + for xform in transformations: + if "relative_translation" in xform and xform.relative_translation: + translation = xform.relative_translation + meters_east = ( + translation.meters_east + if "meters_east" in translation and translation.meters_east is not None + else 0.0 + ) + meters_north = ( + translation.meters_north + if "meters_north" in translation + and translation.meters_north is not None + else 0.0 + ) + degrees_east = ( + translation.degrees_east + if "degrees_east" in translation + and translation.degrees_east is not None + else 0.0 + ) + degrees_north = ( + translation.degrees_north + if "degrees_north" in translation + and translation.degrees_north is not None + else 0.0 + ) + + translation.reference_center = src_center + + dst_center = src_center.offset(meters_east, meters_north) + dst_center.lng += degrees_east + dst_center.lat += degrees_north + src_center = dst_center + + elif "absolute_translation" in xform and xform.absolute_translation: + translation = xform.absolute_translation + + translation.reference_center = src_center + + dst_center = LatLngPoint( + lat=translation.new_latitude, lng=translation.new_longitude + ) + src_center = dst_center + + def flatten(reference: s2sphere.LatLng, point: s2sphere.LatLng) -> tuple[float, float]: """Locally flatten a lat-lng point to (dx, dy) in meters from reference.""" return ( diff --git a/monitoring/monitorlib/geo_test.py b/monitoring/monitorlib/geo_test.py index 58938ed25c..9185206816 100644 --- a/monitoring/monitorlib/geo_test.py +++ b/monitoring/monitorlib/geo_test.py @@ -3,6 +3,7 @@ from s2sphere import LatLng from monitoring.monitorlib.geo import ( + AbsoluteTranslation, Altitude, AltitudeDatum, Circle, @@ -10,14 +11,82 @@ LatLngPoint, Polygon, Radius, + RelativeTranslation, Volume3D, + apply_rotation, + flatten, generate_area_in_vicinity, generate_slight_overlap_area, + make_rotation_matrix, + unflatten, ) MAX_DIFFERENCE = 0.001 +def test_flatten_unflatten(): + pts = [(34, -118), (-70, -150), (45, 9), (-10, 80), (0, 0), (1, 1), (-1, -1)] + deltas = [(0, 0), (1e-6, 1e-6), (1e-3, 1e-3), (-1e-2, 1e-2), (1e-4, -1e-4)] + + for pt in pts: + ref = LatLng.from_degrees(pt[0], pt[1]) + for delta in deltas: + p = LatLng.from_degrees(pt[0] + delta[0], pt[1] + delta[1]) + xy = flatten(ref, p) + p1 = unflatten(ref, xy) + assert abs(p.lat().degrees - p1.lat().degrees) < 1e-9 + assert abs(p.lng().degrees - p1.lng().degrees) < 1e-9 + + +def test_rotation_translation(): + p_ref = LatLng.from_degrees(45.0, 9.0) + p_tgt = LatLng.from_degrees(46.0, 10.0) + + r_matrix = make_rotation_matrix(p_ref, p_tgt) + + p_ref_rotated = apply_rotation(r_matrix, p_ref) + assert abs(p_ref_rotated.lat().degrees - p_tgt.lat().degrees) < 1e-9 + assert abs(p_ref_rotated.lng().degrees - p_tgt.lng().degrees) < 1e-9 + + poly = Polygon( + vertices=[ + LatLngPoint(lat=45.0, lng=9.0), + LatLngPoint(lat=45.1, lng=9.0), + LatLngPoint(lat=45.1, lng=9.1), + LatLngPoint(lat=45.0, lng=9.1), + ] + ) + vol = Volume3D(outline_polygon=poly) + + # Translate relative + rel_trans = RelativeTranslation(meters_east=1000, meters_north=2000) + vol_rel = vol.translate_relative(rel_trans) + + # Translate absolute + abs_trans = AbsoluteTranslation(new_latitude=46.0, new_longitude=10.0) + vol_abs = vol.translate_absolute(abs_trans) + + # Verify that the average vertex is moved correctly + assert vol_rel.outline_polygon is not None + avg_rel = vol_rel.outline_polygon.vertex_average() + expected_rel = poly.vertex_average().offset(1000, 2000) + assert abs(avg_rel.lat - expected_rel.lat) < 1e-6 + assert abs(avg_rel.lng - expected_rel.lng) < 1e-6 + + assert vol_abs.outline_polygon is not None + avg_abs = vol_abs.outline_polygon.vertex_average() + assert abs(avg_abs.lat - 46.0) < 1e-6 + assert abs(avg_abs.lng - 10.0) < 1e-6 + + +def test_make_latlng_rect(): + from monitoring.monitorlib.geo import Circle, Volume3D, make_latlng_rect + + circle_vol = Volume3D(outline_circle=Circle.from_meters(34.0, -118.0, 1000.0)) + rect = make_latlng_rect(circle_vol) + assert rect.contains(LatLng.from_degrees(34.0, -118.0)) + + def _points(in_points: list[tuple[float, float]]) -> list[LatLng]: return [LatLng.from_degrees(*p) for p in in_points] diff --git a/monitoring/monitorlib/geotemporal.py b/monitoring/monitorlib/geotemporal.py index 66e24138ae..4671884ee5 100644 --- a/monitoring/monitorlib/geotemporal.py +++ b/monitoring/monitorlib/geotemporal.py @@ -20,6 +20,7 @@ DistanceUnits, LatLngPoint, Polygon, + Transformation, Volume3D, ) from monitoring.monitorlib.temporal import ( @@ -27,7 +28,6 @@ TestTimeContext, Time, ) -from monitoring.monitorlib.transformations import Transformation TIME_TOLERANCE = timedelta(milliseconds=10) diff --git a/monitoring/monitorlib/transformations.py b/monitoring/monitorlib/transformations.py deleted file mode 100644 index 7d4e67fde4..0000000000 --- a/monitoring/monitorlib/transformations.py +++ /dev/null @@ -1,38 +0,0 @@ -from implicitdict import ImplicitDict, Optional - - -class RelativeTranslation(ImplicitDict): - """Offset a geo feature by a particular amount.""" - - meters_east: Optional[float] - """Number of meters east to translate.""" - - meters_north: Optional[float] - """Number of meters north to translate.""" - - meters_up: Optional[float] - """Number of meters upward to translate.""" - - degrees_east: Optional[float] - """Number of degrees of longitude east to translate.""" - - degrees_north: Optional[float] - """Number of degrees of latitude north to translate.""" - - -class AbsoluteTranslation(ImplicitDict): - """Move a geo feature to a specified location.""" - - new_latitude: float - """The new latitude at which the feature should be located (degrees).""" - - new_longitude: float - """The new longitude at which the feature should be located (degrees).""" - - -class Transformation(ImplicitDict): - """A transformation to apply to a geotemporal feature. Exactly one field must be specified.""" - - relative_translation: Optional[RelativeTranslation] - - absolute_translation: Optional[AbsoluteTranslation] diff --git a/monitoring/prober/monitorlib/__init__.py b/monitoring/prober/monitorlib/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/monitoring/prober/monitorlib/test_geo.py b/monitoring/prober/monitorlib/test_geo.py deleted file mode 100644 index edc1203639..0000000000 --- a/monitoring/prober/monitorlib/test_geo.py +++ /dev/null @@ -1,72 +0,0 @@ -import s2sphere - -from monitoring.monitorlib import geo - - -def test_flatten_unflatten(): - pts = [(34, -118), (-70, -150), (45, 9), (-10, 80), (0, 0), (1, 1), (-1, -1)] - deltas = [(0, 0), (1e-6, 1e-6), (1e-3, 1e-3), (-1e-2, 1e-2), (1e-4, -1e-4)] - - for pt in pts: - ref = s2sphere.LatLng.from_degrees(pt[0], pt[1]) - for delta in deltas: - p = s2sphere.LatLng.from_degrees(pt[0] + delta[0], pt[1] + delta[1]) - xy = geo.flatten(ref, p) - p1 = geo.unflatten(ref, xy) - assert abs(p.lat().degrees - p1.lat().degrees) < 1e-9 - assert abs(p.lng().degrees - p1.lng().degrees) < 1e-9 - - -def test_rotation_translation(): - p_ref = s2sphere.LatLng.from_degrees(45.0, 9.0) - p_tgt = s2sphere.LatLng.from_degrees(46.0, 10.0) - - r_matrix = geo.make_rotation_matrix(p_ref, p_tgt) - - p_ref_rotated = geo.apply_rotation(r_matrix, p_ref) - assert abs(p_ref_rotated.lat().degrees - p_tgt.lat().degrees) < 1e-9 - assert abs(p_ref_rotated.lng().degrees - p_tgt.lng().degrees) < 1e-9 - - from monitoring.monitorlib.geo import LatLngPoint, Polygon, Volume3D - from monitoring.monitorlib.transformations import ( - AbsoluteTranslation, - RelativeTranslation, - ) - - poly = Polygon( - vertices=[ - LatLngPoint(lat=45.0, lng=9.0), - LatLngPoint(lat=45.1, lng=9.0), - LatLngPoint(lat=45.1, lng=9.1), - LatLngPoint(lat=45.0, lng=9.1), - ] - ) - vol = Volume3D(outline_polygon=poly) - - # Translate relative - rel_trans = RelativeTranslation(meters_east=1000, meters_north=2000) - vol_rel = vol.translate_relative(rel_trans) - - # Translate absolute - abs_trans = AbsoluteTranslation(new_latitude=46.0, new_longitude=10.0) - vol_abs = vol.translate_absolute(abs_trans) - - # Verify that the average vertex is moved correctly - assert vol_rel.outline_polygon is not None - avg_rel = vol_rel.outline_polygon.vertex_average() - expected_rel = poly.vertex_average().offset(1000, 2000) - assert abs(avg_rel.lat - expected_rel.lat) < 1e-6 - assert abs(avg_rel.lng - expected_rel.lng) < 1e-6 - - assert vol_abs.outline_polygon is not None - avg_abs = vol_abs.outline_polygon.vertex_average() - assert abs(avg_abs.lat - 46.0) < 1e-6 - assert abs(avg_abs.lng - 10.0) < 1e-6 - - -def test_make_latlng_rect(): - from monitoring.monitorlib.geo import Circle, Volume3D, make_latlng_rect - - circle_vol = Volume3D(outline_circle=Circle.from_meters(34.0, -118.0, 1000.0)) - rect = make_latlng_rect(circle_vol) - assert rect.contains(s2sphere.LatLng.from_degrees(34.0, -118.0)) diff --git a/monitoring/uss_qualifier/resources/flight_planning/flight_intent.py b/monitoring/uss_qualifier/resources/flight_planning/flight_intent.py index d046730d98..71f89224f8 100644 --- a/monitoring/uss_qualifier/resources/flight_planning/flight_intent.py +++ b/monitoring/uss_qualifier/resources/flight_planning/flight_intent.py @@ -1,13 +1,14 @@ from __future__ import annotations import json +from collections.abc import Iterable from implicitdict import ImplicitDict, Optional from monitoring.monitorlib.clients.flight_planning.flight_info_template import ( FlightInfoTemplate, ) -from monitoring.monitorlib.transformations import Transformation +from monitoring.monitorlib.geo import LatLngPoint, Transformation, bind_transformations from monitoring.uss_qualifier.resources.files import ExternalFile from monitoring.uss_qualifier.resources.overrides import apply_overrides @@ -39,6 +40,35 @@ class FlightIntentCollectionElement(ImplicitDict): """If specified, a flight planning intent based on another flight intent, but with some changes.""" +def flight_info_templates_center( + templates: Iterable[FlightInfoTemplate], +) -> LatLngPoint: + lats = [] + lngs = [] + for template in templates: + if ( + "basic_information" in template + and template.basic_information + and "area" in template.basic_information + and template.basic_information.area + ): + for v4d_template in template.basic_information.area: + if "outline_polygon" in v4d_template and v4d_template.outline_polygon: + for vertex in v4d_template.outline_polygon.vertices: + lats.append(vertex.lat) + lngs.append(vertex.lng) + elif "outline_circle" in v4d_template and v4d_template.outline_circle: + lats.append(v4d_template.outline_circle.center.lat) + lngs.append(v4d_template.outline_circle.center.lng) + + if not lats: + raise ValueError( + "Cannot compute center of FlightInfoTemplates without any geographical information" + ) + + return LatLngPoint(lat=sum(lats) / len(lats), lng=sum(lngs) / len(lngs)) + + class FlightIntentCollection(ImplicitDict): """Specification for a collection of flight intents, each identified by a FlightIntentID.""" @@ -101,6 +131,10 @@ def resolve(self) -> dict[FlightIntentID, FlightInfoTemplate]: xforms.extend(self.transformations) v.transformations = xforms + # Transform intent elements as a group rather than independently + center = flight_info_templates_center(processed_intents.values()) + bind_transformations(self.transformations, center) + return processed_intents diff --git a/monitoring/uss_qualifier/resources/flight_planning/flight_intent_test.py b/monitoring/uss_qualifier/resources/flight_planning/flight_intent_test.py new file mode 100644 index 0000000000..08ab3365ae --- /dev/null +++ b/monitoring/uss_qualifier/resources/flight_planning/flight_intent_test.py @@ -0,0 +1,153 @@ +import arrow + +from monitoring.monitorlib.clients.flight_planning.flight_info_template import ( + BasicFlightPlanInformationTemplate, + FlightInfoTemplate, +) +from monitoring.monitorlib.geo import ( + LatLngPoint, + Polygon, + RelativeTranslation, + Transformation, + Volume3D, +) +from monitoring.monitorlib.temporal import TestTimeContext, Time +from monitoring.uss_qualifier.resources.flight_planning.flight_intent import ( + FlightIntentCollection, + FlightIntentCollectionElement, +) + + +def test_flight_intent_relative_transformations(): + # Two flight intents defined near origin that are very close but not overlapping. + # Flight 1 polygon + poly1 = Polygon( + vertices=[ + LatLngPoint(lat=0.00000, lng=0.00000), + LatLngPoint(lat=0.00100, lng=0.00000), + LatLngPoint(lat=0.00100, lng=0.00100), + LatLngPoint(lat=0.00000, lng=0.00100), + ] + ) + + # Flight 2 polygon, shifted slightly to the east of poly1, very close but not overlapping + poly2 = Polygon( + vertices=[ + LatLngPoint(lat=0.00000, lng=0.001001), + LatLngPoint(lat=0.00100, lng=0.001001), + LatLngPoint(lat=0.00100, lng=0.002), + LatLngPoint(lat=0.00000, lng=0.002), + ] + ) + + template1 = FlightInfoTemplate( + basic_information=BasicFlightPlanInformationTemplate( + usage_state="Planned", + uas_state="Nominal", + area=[ + Volume3D( + outline_polygon=poly1, + altitude_lower={"value": 0, "units": "M", "reference": "W84"}, + altitude_upper={"value": 100, "units": "M", "reference": "W84"}, + ) + ], + ) + ) + + template2 = FlightInfoTemplate( + basic_information=BasicFlightPlanInformationTemplate( + usage_state="Planned", + uas_state="Nominal", + area=[ + Volume3D( + outline_polygon=poly2, + altitude_lower={"value": 0, "units": "M", "reference": "W84"}, + altitude_upper={"value": 100, "units": "M", "reference": "W84"}, + ) + ], + ) + ) + + transformations = [ + Transformation( + relative_translation=RelativeTranslation( + meters_east=500.0, + meters_north=-300.0, + ) + ), + Transformation( + relative_translation=RelativeTranslation( + degrees_north=32.7181, + degrees_east=-96.7587, + ) + ), + Transformation( + relative_translation=RelativeTranslation( + meters_east=-100.0, + meters_north=5000.0, + ) + ), + ] + + for n_transformations in range(len(transformations) + 1): + collection = FlightIntentCollection( + intents={ + "flight_1": FlightIntentCollectionElement(full=template1), + "flight_2": FlightIntentCollectionElement(full=template2), + }, + transformations=transformations[0:n_transformations], + ) + + resolved = collection.resolve() + + # Resolve the final FlightInfo using a dummy context + t = Time(arrow.utcnow().datetime) + context = TestTimeContext.all_times_are(t) + info1 = resolved["flight_1"].resolve(context) + info2 = resolved["flight_2"].resolve(context) + + vol1 = info1.basic_information.area[0].volume + vol2 = info2.basic_information.area[0].volume + + # Verify that distance between any pair of vertices remains unchanged (rigid transformation) + assert ( + vol1.outline_polygon is not None + and vol1.outline_polygon.vertices is not None + ) + assert ( + vol2.outline_polygon is not None + and vol2.outline_polygon.vertices is not None + ) + assert poly1.vertices is not None + assert poly2.vertices is not None + orig_vertices = poly1.vertices + poly2.vertices + trans_vertices = vol1.outline_polygon.vertices + vol2.outline_polygon.vertices + assert len(orig_vertices) == len(trans_vertices) + for i in range(len(orig_vertices)): + for j in range(i + 1, len(orig_vertices)): + d_orig = ( + orig_vertices[i] + .as_s2sphere() + .get_distance(orig_vertices[j].as_s2sphere()) + .radians + ) + d_trans = ( + trans_vertices[i] + .as_s2sphere() + .get_distance(trans_vertices[j].as_s2sphere()) + .radians + ) + assert abs(d_orig - d_trans) < 1e-12 + + # Verify they still do not overlap + assert not vol1.intersects_vol3(vol2) + + # If they were shifted slightly closer to overlap, they would intersect + # Let's verify that a small additional overlap translation makes them intersect + overlap_trans = Transformation( + relative_translation=RelativeTranslation( + degrees_east=-0.000002 # Shift flight 2 slightly west to overlap flight 1 + ) + ) + vol2_overlapped = vol2.transform(overlap_trans) + assert vol1.intersects_vol3(vol2_overlapped) diff --git a/schemas/monitoring/monitorlib/clients/flight_planning/flight_info_template/FlightInfoTemplate.json b/schemas/monitoring/monitorlib/clients/flight_planning/flight_info_template/FlightInfoTemplate.json index 76a714d1e9..52f055a0fc 100644 --- a/schemas/monitoring/monitorlib/clients/flight_planning/flight_info_template/FlightInfoTemplate.json +++ b/schemas/monitoring/monitorlib/clients/flight_planning/flight_info_template/FlightInfoTemplate.json @@ -40,7 +40,7 @@ "transformations": { "description": "If specified, transform this flight according to these transformations in order (after all templates are resolved).", "items": { - "$ref": "../../../transformations/Transformation.json" + "$ref": "../../../geo/Transformation.json" }, "type": [ "array", diff --git a/schemas/monitoring/monitorlib/transformations/AbsoluteTranslation.json b/schemas/monitoring/monitorlib/geo/AbsoluteTranslation.json similarity index 63% rename from schemas/monitoring/monitorlib/transformations/AbsoluteTranslation.json rename to schemas/monitoring/monitorlib/geo/AbsoluteTranslation.json index 5bc444a7be..5eca5bf8a2 100644 --- a/schemas/monitoring/monitorlib/transformations/AbsoluteTranslation.json +++ b/schemas/monitoring/monitorlib/geo/AbsoluteTranslation.json @@ -1,7 +1,7 @@ { - "$id": "https://github.com/interuss/monitoring/blob/main/schemas/monitoring/monitorlib/transformations/AbsoluteTranslation.json", + "$id": "https://github.com/interuss/monitoring/blob/main/schemas/monitoring/monitorlib/geo/AbsoluteTranslation.json", "$schema": "https://json-schema.org/draft/2020-12/schema", - "description": "Move a geo feature to a specified location.\n\nmonitoring.monitorlib.transformations.AbsoluteTranslation, as defined in monitoring/monitorlib/transformations.py", + "description": "Move a geo feature to a specified location.\n\nmonitoring.monitorlib.geo.AbsoluteTranslation, as defined in monitoring/monitorlib/geo.py", "properties": { "$ref": { "description": "Path to content that replaces the $ref", @@ -14,6 +14,17 @@ "new_longitude": { "description": "The new longitude at which the feature should be located (degrees).", "type": "number" + }, + "reference_center": { + "description": "The center around which the translation is defined/rotated.", + "oneOf": [ + { + "type": "null" + }, + { + "$ref": "LatLngPoint.json" + } + ] } }, "required": [ diff --git a/schemas/monitoring/monitorlib/geo/Polygon.json b/schemas/monitoring/monitorlib/geo/Polygon.json index ae8e2e6629..c7bb037d14 100644 --- a/schemas/monitoring/monitorlib/geo/Polygon.json +++ b/schemas/monitoring/monitorlib/geo/Polygon.json @@ -11,11 +11,11 @@ "items": { "$ref": "LatLngPoint.json" }, - "type": [ - "array", - "null" - ] + "type": "array" } }, + "required": [ + "vertices" + ], "type": "object" } \ No newline at end of file diff --git a/schemas/monitoring/monitorlib/transformations/RelativeTranslation.json b/schemas/monitoring/monitorlib/geo/RelativeTranslation.json similarity index 73% rename from schemas/monitoring/monitorlib/transformations/RelativeTranslation.json rename to schemas/monitoring/monitorlib/geo/RelativeTranslation.json index 79473b962b..1b2187f295 100644 --- a/schemas/monitoring/monitorlib/transformations/RelativeTranslation.json +++ b/schemas/monitoring/monitorlib/geo/RelativeTranslation.json @@ -1,7 +1,7 @@ { - "$id": "https://github.com/interuss/monitoring/blob/main/schemas/monitoring/monitorlib/transformations/RelativeTranslation.json", + "$id": "https://github.com/interuss/monitoring/blob/main/schemas/monitoring/monitorlib/geo/RelativeTranslation.json", "$schema": "https://json-schema.org/draft/2020-12/schema", - "description": "Offset a geo feature by a particular amount.\n\nmonitoring.monitorlib.transformations.RelativeTranslation, as defined in monitoring/monitorlib/transformations.py", + "description": "Offset a geo feature by a particular amount.\n\nmonitoring.monitorlib.geo.RelativeTranslation, as defined in monitoring/monitorlib/geo.py", "properties": { "$ref": { "description": "Path to content that replaces the $ref", @@ -41,6 +41,17 @@ "number", "null" ] + }, + "reference_center": { + "description": "The center around which the translation is defined/rotated.", + "oneOf": [ + { + "type": "null" + }, + { + "$ref": "LatLngPoint.json" + } + ] } }, "type": "object" diff --git a/schemas/monitoring/monitorlib/transformations/Transformation.json b/schemas/monitoring/monitorlib/geo/Transformation.json similarity index 82% rename from schemas/monitoring/monitorlib/transformations/Transformation.json rename to schemas/monitoring/monitorlib/geo/Transformation.json index 87966c40a9..13be15308c 100644 --- a/schemas/monitoring/monitorlib/transformations/Transformation.json +++ b/schemas/monitoring/monitorlib/geo/Transformation.json @@ -1,7 +1,7 @@ { - "$id": "https://github.com/interuss/monitoring/blob/main/schemas/monitoring/monitorlib/transformations/Transformation.json", + "$id": "https://github.com/interuss/monitoring/blob/main/schemas/monitoring/monitorlib/geo/Transformation.json", "$schema": "https://json-schema.org/draft/2020-12/schema", - "description": "A transformation to apply to a geotemporal feature. Exactly one field must be specified.\n\nmonitoring.monitorlib.transformations.Transformation, as defined in monitoring/monitorlib/transformations.py", + "description": "A transformation to apply to a geotemporal feature. Exactly one field must be specified.\n\nmonitoring.monitorlib.geo.Transformation, as defined in monitoring/monitorlib/geo.py", "properties": { "$ref": { "description": "Path to content that replaces the $ref", diff --git a/schemas/monitoring/monitorlib/geotemporal/Volume4DTemplate.json b/schemas/monitoring/monitorlib/geotemporal/Volume4DTemplate.json index f867b1969e..863e1e13f7 100644 --- a/schemas/monitoring/monitorlib/geotemporal/Volume4DTemplate.json +++ b/schemas/monitoring/monitorlib/geotemporal/Volume4DTemplate.json @@ -84,7 +84,7 @@ "transformations": { "description": "If specified, transform this volume according to these transformations in order.", "items": { - "$ref": "../transformations/Transformation.json" + "$ref": "../geo/Transformation.json" }, "type": [ "array", diff --git a/schemas/monitoring/uss_qualifier/resources/flight_planning/flight_intent/FlightIntentCollection.json b/schemas/monitoring/uss_qualifier/resources/flight_planning/flight_intent/FlightIntentCollection.json index 0606503812..24c46d50c1 100644 --- a/schemas/monitoring/uss_qualifier/resources/flight_planning/flight_intent/FlightIntentCollection.json +++ b/schemas/monitoring/uss_qualifier/resources/flight_planning/flight_intent/FlightIntentCollection.json @@ -23,7 +23,7 @@ "transformations": { "description": "Transformations to append to all FlightInfoTemplates.", "items": { - "$ref": "../../../../monitorlib/transformations/Transformation.json" + "$ref": "../../../../monitorlib/geo/Transformation.json" }, "type": [ "array", diff --git a/schemas/monitoring/uss_qualifier/resources/flight_planning/flight_intent/FlightIntentsSpecification.json b/schemas/monitoring/uss_qualifier/resources/flight_planning/flight_intent/FlightIntentsSpecification.json index b39df5ee4d..2e27bab592 100644 --- a/schemas/monitoring/uss_qualifier/resources/flight_planning/flight_intent/FlightIntentsSpecification.json +++ b/schemas/monitoring/uss_qualifier/resources/flight_planning/flight_intent/FlightIntentsSpecification.json @@ -32,7 +32,7 @@ "transformations": { "description": "Transformations to apply to all flight intents' 4D volumes after resolution (if specified)", "items": { - "$ref": "../../../../monitorlib/transformations/Transformation.json" + "$ref": "../../../../monitorlib/geo/Transformation.json" }, "type": [ "array", From f2f0ce42531df3ad46e082d4c4b203e45be6008b Mon Sep 17 00:00:00 2001 From: Benjamin Pelletier Date: Mon, 15 Jun 2026 15:43:03 +0000 Subject: [PATCH 3/3] Add Volume3d.center_2d --- monitoring/monitorlib/geo.py | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/monitoring/monitorlib/geo.py b/monitoring/monitorlib/geo.py index ac0f0cfa81..1a037afebf 100644 --- a/monitoring/monitorlib/geo.py +++ b/monitoring/monitorlib/geo.py @@ -402,6 +402,14 @@ def transform(self, transformation: Transformation) -> Volume3D: f"No supported transformation defined (keys: {', '.join(transformation)})" ) + def center_2d(self) -> LatLngPoint: + if self.outline_polygon is not None: + return self.outline_polygon.vertex_average() + elif self.outline_circle is not None: + return self.outline_circle.center + else: + raise ValueError("Neither outline_circle nor outline_polygon specified") + def translate_relative(self, translation: RelativeTranslation) -> Volume3D: if ( "reference_center" in translation @@ -409,12 +417,7 @@ def translate_relative(self, translation: RelativeTranslation) -> Volume3D: ): src_center = translation.reference_center else: - if self.outline_polygon is not None: - src_center = self.outline_polygon.vertex_average() - elif self.outline_circle is not None: - src_center = self.outline_circle.center - else: - raise ValueError("Neither outline_circle nor outline_polygon specified") + src_center = self.center_2d() meters_east = ( translation.meters_east @@ -493,12 +496,7 @@ def translate_absolute(self, translation: AbsoluteTranslation) -> Volume3D: ): src_center = translation.reference_center else: - if self.outline_polygon is not None: - src_center = self.outline_polygon.vertex_average() - elif self.outline_circle is not None: - src_center = self.outline_circle.center - else: - raise ValueError("Neither outline_circle nor outline_polygon specified") + src_center = self.center_2d() r_matrix = make_rotation_matrix( src_center.as_s2sphere(), new_center.as_s2sphere()