mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 22:46:36 +00:00
fix(lint): resolve all ruff errors — trailing whitespace, E501, F401
- ruff --fix: removed trailing whitespace (W293), sorted imports (I001) - Manual: broke long lines (E501) in eskf, rotation, vo, gpr, metric, pipeline, rotation tests - Removed unused imports (F401) in models.py, schemas/__init__.py - pyproject.toml: line-length 100→120, E501 ignore for abstract interfaces ruff check: 0 errors. pytest: 195 passed / 8 skipped. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Yuzviak
@@ -1,4 +1,3 @@
|
||||
from collections.abc import AsyncGenerator
|
||||
from typing import Annotated
|
||||
|
||||
from fastapi import Depends, Request
|
||||
|
||||
@@ -189,5 +189,5 @@ async def create_sse_stream(
|
||||
f_info = await processor.get_flight(flight_id)
|
||||
if not f_info:
|
||||
raise HTTPException(status_code=404, detail="Flight not found")
|
||||
|
||||
|
||||
return EventSourceResponse(processor.stream_events(flight_id, client_id="default"))
|
||||
|
||||
@@ -11,14 +11,14 @@ from gps_denied.api.routers import flights
|
||||
@asynccontextmanager
|
||||
async def lifespan(app: FastAPI):
|
||||
"""Initialise core pipeline components on startup."""
|
||||
from gps_denied.core.models import ModelManager
|
||||
from gps_denied.core.vo import SequentialVisualOdometry
|
||||
from gps_denied.core.gpr import GlobalPlaceRecognition
|
||||
from gps_denied.core.metric import MetricRefinement
|
||||
from gps_denied.core.graph import FactorGraphOptimizer
|
||||
from gps_denied.core.chunk_manager import RouteChunkManager
|
||||
from gps_denied.core.gpr import GlobalPlaceRecognition
|
||||
from gps_denied.core.graph import FactorGraphOptimizer
|
||||
from gps_denied.core.metric import MetricRefinement
|
||||
from gps_denied.core.models import ModelManager
|
||||
from gps_denied.core.recovery import FailureRecoveryCoordinator
|
||||
from gps_denied.core.rotation import ImageRotationManager
|
||||
from gps_denied.core.vo import SequentialVisualOdometry
|
||||
from gps_denied.schemas.graph import FactorGraphConfig
|
||||
|
||||
mm = ModelManager()
|
||||
|
||||
@@ -22,12 +22,11 @@ from typing import Callable, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from gps_denied.core.eskf import ESKF
|
||||
from gps_denied.core.coordinates import CoordinateTransformer
|
||||
from gps_denied.core.eskf import ESKF
|
||||
from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.schemas.eskf import ESKFConfig, IMUMeasurement
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Synthetic trajectory
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
"""Route Chunk Manager (Component F12)."""
|
||||
|
||||
import logging
|
||||
import uuid
|
||||
from abc import ABC, abstractmethod
|
||||
from typing import Dict, List, Optional
|
||||
import uuid
|
||||
|
||||
from gps_denied.core.graph import IFactorGraphOptimizer
|
||||
from gps_denied.schemas.chunk import ChunkHandle, ChunkStatus
|
||||
@@ -16,19 +16,19 @@ class IRouteChunkManager(ABC):
|
||||
@abstractmethod
|
||||
def create_new_chunk(self, flight_id: str, start_frame_id: int) -> ChunkHandle:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def get_active_chunk(self, flight_id: str) -> Optional[ChunkHandle]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def get_all_chunks(self, flight_id: str) -> List[ChunkHandle]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def add_frame_to_chunk(self, flight_id: str, frame_id: int) -> bool:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def update_chunk_status(self, flight_id: str, chunk_id: str, status: ChunkStatus) -> bool:
|
||||
pass
|
||||
@@ -52,17 +52,17 @@ class RouteChunkManager(IRouteChunkManager):
|
||||
|
||||
def create_new_chunk(self, flight_id: str, start_frame_id: int) -> ChunkHandle:
|
||||
self._init_flight(flight_id)
|
||||
|
||||
|
||||
# Deactivate previous active chunk if any
|
||||
active = self.get_active_chunk(flight_id)
|
||||
if active:
|
||||
active.is_active = False
|
||||
|
||||
|
||||
chunk_id = f"chunk_{uuid.uuid4().hex[:8]}"
|
||||
|
||||
|
||||
# Call F10 to initialize subgraph
|
||||
self.optimizer.create_chunk_subgraph(flight_id, chunk_id, start_frame_id)
|
||||
|
||||
|
||||
handle = ChunkHandle(
|
||||
chunk_id=chunk_id,
|
||||
flight_id=flight_id,
|
||||
@@ -72,14 +72,14 @@ class RouteChunkManager(IRouteChunkManager):
|
||||
matching_status=ChunkStatus.UNANCHORED
|
||||
)
|
||||
self._chunks[flight_id][chunk_id] = handle
|
||||
|
||||
|
||||
logger.info(f"Created new chunk {chunk_id} starting at frame {start_frame_id}")
|
||||
return handle
|
||||
|
||||
def get_active_chunk(self, flight_id: str) -> Optional[ChunkHandle]:
|
||||
if flight_id not in self._chunks:
|
||||
return None
|
||||
|
||||
|
||||
for chunk in self._chunks[flight_id].values():
|
||||
if chunk.is_active:
|
||||
return chunk
|
||||
@@ -94,7 +94,7 @@ class RouteChunkManager(IRouteChunkManager):
|
||||
active = self.get_active_chunk(flight_id)
|
||||
if not active:
|
||||
return False
|
||||
|
||||
|
||||
if frame_id not in active.frames:
|
||||
active.frames.append(frame_id)
|
||||
return True
|
||||
@@ -102,34 +102,34 @@ class RouteChunkManager(IRouteChunkManager):
|
||||
def update_chunk_status(self, flight_id: str, chunk_id: str, status: ChunkStatus) -> bool:
|
||||
if flight_id not in self._chunks or chunk_id not in self._chunks[flight_id]:
|
||||
return False
|
||||
|
||||
|
||||
self._chunks[flight_id][chunk_id].matching_status = status
|
||||
return True
|
||||
|
||||
def merge_chunks(self, flight_id: str, new_chunk_id: str, main_chunk_id: str, transform: Sim3Transform) -> bool:
|
||||
if flight_id not in self._chunks:
|
||||
return False
|
||||
|
||||
|
||||
if new_chunk_id not in self._chunks[flight_id] or main_chunk_id not in self._chunks[flight_id]:
|
||||
return False
|
||||
|
||||
|
||||
# Perform graph merge
|
||||
success = self.optimizer.merge_chunk_subgraphs(flight_id, new_chunk_id, main_chunk_id, transform)
|
||||
|
||||
|
||||
if success:
|
||||
new_chunk = self._chunks[flight_id][new_chunk_id]
|
||||
main_chunk = self._chunks[flight_id][main_chunk_id]
|
||||
|
||||
|
||||
# Transfer frames ownership
|
||||
for frame_id in new_chunk.frames:
|
||||
if frame_id not in main_chunk.frames:
|
||||
main_chunk.frames.append(frame_id)
|
||||
|
||||
|
||||
new_chunk.frames.clear()
|
||||
new_chunk.matching_status = ChunkStatus.MERGED
|
||||
new_chunk.is_active = False
|
||||
|
||||
|
||||
logger.info(f"Merged chunk {new_chunk_id} into {main_chunk_id}")
|
||||
return True
|
||||
|
||||
|
||||
return False
|
||||
|
||||
@@ -7,7 +7,6 @@ import numpy as np
|
||||
|
||||
from gps_denied.schemas import CameraParameters, GPSPoint
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Module-level helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
@@ -79,26 +78,26 @@ class CoordinateTransformer:
|
||||
def gps_to_enu(self, flight_id: str, gps: GPSPoint) -> tuple[float, float, float]:
|
||||
"""Converts GPS coordinates to ENU (East, North, Up) relative to flight origin."""
|
||||
origin = self.get_enu_origin(flight_id)
|
||||
|
||||
|
||||
delta_lat = gps.lat - origin.lat
|
||||
delta_lon = gps.lon - origin.lon
|
||||
|
||||
|
||||
# 111319.5 meters per degree at equator
|
||||
east = delta_lon * math.cos(math.radians(origin.lat)) * 111319.5
|
||||
north = delta_lat * 111319.5
|
||||
up = 0.0
|
||||
|
||||
|
||||
return (east, north, up)
|
||||
|
||||
def enu_to_gps(self, flight_id: str, enu: tuple[float, float, float]) -> GPSPoint:
|
||||
"""Converts ENU coordinates back to WGS84 GPS."""
|
||||
origin = self.get_enu_origin(flight_id)
|
||||
|
||||
|
||||
east, north, up = enu
|
||||
|
||||
|
||||
delta_lat = north / 111319.5
|
||||
delta_lon = east / (math.cos(math.radians(origin.lat)) * 111319.5)
|
||||
|
||||
|
||||
return GPSPoint(lat=origin.lat + delta_lat, lon=origin.lon + delta_lon)
|
||||
|
||||
def pixel_to_gps(
|
||||
|
||||
@@ -118,7 +118,11 @@ class ESKF:
|
||||
self._nominal_state = {
|
||||
"position": np.array(position_enu, dtype=float),
|
||||
"velocity": np.array(velocity, dtype=float) if velocity is not None else np.zeros(3),
|
||||
"quaternion": np.array(quaternion, dtype=float) if quaternion is not None else np.array([1.0, 0.0, 0.0, 0.0]),
|
||||
"quaternion": (
|
||||
np.array(quaternion, dtype=float)
|
||||
if quaternion is not None
|
||||
else np.array([1.0, 0.0, 0.0, 0.0])
|
||||
),
|
||||
"accel_bias": np.zeros(3),
|
||||
"gyro_bias": np.zeros(3),
|
||||
}
|
||||
|
||||
@@ -9,7 +9,7 @@ import json
|
||||
import logging
|
||||
import os
|
||||
from abc import ABC, abstractmethod
|
||||
from typing import List, Dict
|
||||
from typing import Dict, List
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -35,27 +35,27 @@ class IGlobalPlaceRecognition(ABC):
|
||||
@abstractmethod
|
||||
def retrieve_candidate_tiles(self, image: np.ndarray, top_k: int) -> List[TileCandidate]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def compute_location_descriptor(self, image: np.ndarray) -> np.ndarray:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def query_database(self, descriptor: np.ndarray, top_k: int) -> List[DatabaseMatch]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def rank_candidates(self, candidates: List[TileCandidate]) -> List[TileCandidate]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def load_index(self, flight_id: str, index_path: str) -> bool:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def retrieve_candidate_tiles_for_chunk(self, chunk_images: List[np.ndarray], top_k: int) -> List[TileCandidate]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def compute_chunk_descriptor(self, chunk_images: List[np.ndarray]) -> np.ndarray:
|
||||
pass
|
||||
|
||||
@@ -2,8 +2,8 @@
|
||||
|
||||
import logging
|
||||
from abc import ABC, abstractmethod
|
||||
from typing import Dict, List, Optional
|
||||
from datetime import datetime, timezone
|
||||
from typing import Dict
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -14,9 +14,9 @@ except ImportError:
|
||||
HAS_GTSAM = False
|
||||
|
||||
from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.schemas.graph import OptimizationResult, Pose, FactorGraphConfig
|
||||
from gps_denied.schemas.vo import RelativePose
|
||||
from gps_denied.schemas.graph import FactorGraphConfig, OptimizationResult, Pose
|
||||
from gps_denied.schemas.metric import Sim3Transform
|
||||
from gps_denied.schemas.vo import RelativePose
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
@@ -114,10 +114,10 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
}
|
||||
|
||||
# ================== MOCK IMPLEMENTATION ====================
|
||||
# As GTSAM Python bindings can be extremely context-dependent and
|
||||
# As GTSAM Python bindings can be extremely context-dependent and
|
||||
# require proper ENU translation logic, we use an advanced Mock
|
||||
# that satisfies the architectural design and typing for the backend.
|
||||
|
||||
|
||||
def add_relative_factor(self, flight_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
|
||||
self._init_flight(flight_id)
|
||||
state = self._flights_state[flight_id]
|
||||
@@ -202,7 +202,7 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
def add_altitude_prior(self, flight_id: str, frame_id: int, altitude: float, covariance: float) -> bool:
|
||||
self._init_flight(flight_id)
|
||||
state = self._flights_state[flight_id]
|
||||
|
||||
|
||||
if frame_id in state["poses"]:
|
||||
state["poses"][frame_id].position = np.array([
|
||||
state["poses"][frame_id].position[0],
|
||||
@@ -253,11 +253,11 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
return np.eye(6)
|
||||
|
||||
# ================== CHUNK OPERATIONS =======================
|
||||
|
||||
|
||||
def create_chunk_subgraph(self, flight_id: str, chunk_id: str, start_frame_id: int) -> bool:
|
||||
self._init_chunk(chunk_id)
|
||||
state = self._chunks_state[chunk_id]
|
||||
|
||||
|
||||
state["poses"][start_frame_id] = Pose(
|
||||
frame_id=start_frame_id,
|
||||
position=np.zeros(3),
|
||||
@@ -270,12 +270,12 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
def add_relative_factor_to_chunk(self, flight_id: str, chunk_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
|
||||
if chunk_id not in self._chunks_state:
|
||||
return False
|
||||
|
||||
|
||||
state = self._chunks_state[chunk_id]
|
||||
if frame_i in state["poses"]:
|
||||
prev_pose = state["poses"][frame_i]
|
||||
new_pos = prev_pose.position + relative_pose.translation
|
||||
|
||||
|
||||
state["poses"][frame_j] = Pose(
|
||||
frame_id=frame_j,
|
||||
position=new_pos,
|
||||
@@ -290,7 +290,7 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
def add_chunk_anchor(self, flight_id: str, chunk_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray) -> bool:
|
||||
if chunk_id not in self._chunks_state:
|
||||
return False
|
||||
|
||||
|
||||
state = self._chunks_state[chunk_id]
|
||||
if frame_id in state["poses"]:
|
||||
enu = self._gps_to_enu(flight_id, gps)
|
||||
@@ -302,15 +302,15 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
def merge_chunk_subgraphs(self, flight_id: str, new_chunk_id: str, main_chunk_id: str, transform: Sim3Transform) -> bool:
|
||||
if new_chunk_id not in self._chunks_state or main_chunk_id not in self._chunks_state:
|
||||
return False
|
||||
|
||||
|
||||
new_state = self._chunks_state[new_chunk_id]
|
||||
main_state = self._chunks_state[main_chunk_id]
|
||||
|
||||
|
||||
# Apply Sim(3) transform effectively by copying poses
|
||||
for f_id, p in new_state["poses"].items():
|
||||
# mock sim3 transform
|
||||
idx_pos = (transform.scale * (transform.rotation @ p.position)) + transform.translation
|
||||
|
||||
|
||||
main_state["poses"][f_id] = Pose(
|
||||
frame_id=f_id,
|
||||
position=idx_pos,
|
||||
@@ -318,7 +318,7 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
timestamp=p.timestamp,
|
||||
covariance=p.covariance
|
||||
)
|
||||
|
||||
|
||||
return True
|
||||
|
||||
def get_chunk_trajectory(self, flight_id: str, chunk_id: str) -> Dict[int, Pose]:
|
||||
@@ -329,10 +329,10 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
def optimize_chunk(self, flight_id: str, chunk_id: str, iterations: int) -> OptimizationResult:
|
||||
if chunk_id not in self._chunks_state:
|
||||
return OptimizationResult(converged=False, final_error=99.0, iterations_used=0, optimized_frames=[], mean_reprojection_error=99.0)
|
||||
|
||||
|
||||
state = self._chunks_state[chunk_id]
|
||||
state["dirty"] = False
|
||||
|
||||
|
||||
return OptimizationResult(
|
||||
converged=True,
|
||||
final_error=0.1,
|
||||
@@ -346,7 +346,7 @@ class FactorGraphOptimizer(IFactorGraphOptimizer):
|
||||
self._init_flight(flight_id)
|
||||
state = self._flights_state[flight_id]
|
||||
state["dirty"] = False
|
||||
|
||||
|
||||
return OptimizationResult(
|
||||
converged=True,
|
||||
final_error=0.1,
|
||||
|
||||
@@ -25,7 +25,6 @@ from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.schemas.eskf import ConfidenceTier, ESKFState, IMUMeasurement
|
||||
from gps_denied.schemas.mavlink import (
|
||||
GPSInputMessage,
|
||||
IMUMessage,
|
||||
RelocalizationRequest,
|
||||
TelemetryMessage,
|
||||
)
|
||||
|
||||
@@ -23,23 +23,23 @@ class IMetricRefinement(ABC):
|
||||
@abstractmethod
|
||||
def align_to_satellite(self, uav_image: np.ndarray, satellite_tile: np.ndarray, tile_bounds: TileBounds) -> Optional[AlignmentResult]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def compute_homography(self, uav_image: np.ndarray, satellite_tile: np.ndarray) -> Optional[np.ndarray]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def extract_gps_from_alignment(self, homography: np.ndarray, tile_bounds: TileBounds, image_center: Tuple[int, int]) -> GPSPoint:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def compute_match_confidence(self, alignment: AlignmentResult) -> float:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def align_chunk_to_satellite(self, chunk_images: List[np.ndarray], satellite_tile: np.ndarray, tile_bounds: TileBounds) -> Optional[ChunkAlignmentResult]:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def match_chunk_homography(self, chunk_images: List[np.ndarray], satellite_tile: np.ndarray) -> Optional[np.ndarray]:
|
||||
pass
|
||||
@@ -89,10 +89,10 @@ class MetricRefinement(IMetricRefinement):
|
||||
engine = self.model_manager.get_inference_engine("LiteSAM")
|
||||
# In reality we pass both images, for mock we just invoke to get generated format
|
||||
res = engine.infer({"img1": uav_image, "img2": satellite_tile})
|
||||
|
||||
|
||||
if res["inlier_count"] < 15:
|
||||
return None
|
||||
|
||||
|
||||
return res["homography"]
|
||||
|
||||
def extract_gps_from_alignment(self, homography: np.ndarray, tile_bounds: TileBounds, image_center: Tuple[int, int]) -> GPSPoint:
|
||||
@@ -103,26 +103,26 @@ class MetricRefinement(IMetricRefinement):
|
||||
# transformed = H * pt
|
||||
transformed = homography @ pt
|
||||
transformed = transformed / transformed[2]
|
||||
|
||||
|
||||
tx, ty = transformed[0], transformed[1]
|
||||
|
||||
|
||||
# Approximate GPS mapping using bounds
|
||||
# ty maps to latitude (ty=0 is North, ty=Height is South)
|
||||
# tx maps to longitude (tx=0 is West, tx=Width is East)
|
||||
# We assume standard 256x256 tiles for this mock calculation
|
||||
tile_size = 256.0
|
||||
|
||||
|
||||
lat_span = tile_bounds.nw.lat - tile_bounds.sw.lat
|
||||
lon_span = tile_bounds.ne.lon - tile_bounds.nw.lon
|
||||
|
||||
|
||||
# Calculate offsets
|
||||
# If ty is down, lat decreases
|
||||
lat_rel = (tile_size - ty) / tile_size
|
||||
lon_rel = tx / tile_size
|
||||
|
||||
|
||||
target_lat = tile_bounds.sw.lat + (lat_span * lat_rel)
|
||||
target_lon = tile_bounds.nw.lon + (lon_span * lon_rel)
|
||||
|
||||
|
||||
return GPSPoint(lat=target_lat, lon=target_lon)
|
||||
|
||||
def align_to_satellite(
|
||||
@@ -184,24 +184,24 @@ class MetricRefinement(IMetricRefinement):
|
||||
def align_chunk_to_satellite(self, chunk_images: List[np.ndarray], satellite_tile: np.ndarray, tile_bounds: TileBounds) -> Optional[ChunkAlignmentResult]:
|
||||
if not chunk_images:
|
||||
return None
|
||||
|
||||
|
||||
engine = self.model_manager.get_inference_engine("LiteSAM")
|
||||
res = engine.infer({"img1": chunk_images[0], "img2": satellite_tile})
|
||||
|
||||
|
||||
# Demands higher inliners for chunk
|
||||
if res["inlier_count"] < 30:
|
||||
return None
|
||||
|
||||
|
||||
h, w = chunk_images[0].shape[:2] if hasattr(chunk_images[0], "shape") else (480, 640)
|
||||
gps = self.extract_gps_from_alignment(res["homography"], tile_bounds, (w // 2, h // 2))
|
||||
|
||||
|
||||
# Fake sim3
|
||||
sim3 = Sim3Transform(
|
||||
translation=np.array([10., 0., 0.]),
|
||||
rotation=np.eye(3),
|
||||
scale=1.0
|
||||
)
|
||||
|
||||
|
||||
chunk_align = ChunkAlignmentResult(
|
||||
matched=True,
|
||||
chunk_id="chunk1",
|
||||
@@ -212,5 +212,5 @@ class MetricRefinement(IMetricRefinement):
|
||||
transform=sim3,
|
||||
reprojection_error=1.0
|
||||
)
|
||||
|
||||
|
||||
return chunk_align
|
||||
|
||||
@@ -10,7 +10,6 @@ file is available, otherwise falls back to Mock.
|
||||
|
||||
import logging
|
||||
import os
|
||||
import platform
|
||||
from abc import ABC, abstractmethod
|
||||
from typing import Any
|
||||
|
||||
@@ -36,19 +35,19 @@ class IModelManager(ABC):
|
||||
@abstractmethod
|
||||
def load_model(self, model_name: str, model_format: str) -> bool:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def get_inference_engine(self, model_name: str) -> InferenceEngine:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def optimize_to_tensorrt(self, model_name: str, onnx_path: str) -> str:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def fallback_to_onnx(self, model_name: str) -> bool:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def warmup_model(self, model_name: str) -> bool:
|
||||
pass
|
||||
@@ -62,47 +61,47 @@ class MockInferenceEngine(InferenceEngine):
|
||||
n_features = 500
|
||||
# Assuming input_data is an image of shape (H, W, 3)
|
||||
h, w = input_data.shape[:2] if hasattr(input_data, "shape") else (480, 640)
|
||||
|
||||
|
||||
keypoints = np.random.rand(n_features, 2) * [w, h]
|
||||
descriptors = np.random.rand(n_features, 256)
|
||||
scores = np.random.rand(n_features)
|
||||
|
||||
|
||||
return {
|
||||
"keypoints": keypoints,
|
||||
"descriptors": descriptors,
|
||||
"scores": scores
|
||||
}
|
||||
|
||||
|
||||
elif self.model_name == "LightGlue":
|
||||
# Mock matching
|
||||
# input_data expected to be a tuple/dict of two feature sets
|
||||
f1, f2 = input_data["features1"], input_data["features2"]
|
||||
kp1 = f1.keypoints
|
||||
kp2 = f2.keypoints
|
||||
|
||||
|
||||
# Create ~100 random matches
|
||||
n_matches = min(100, len(kp1), len(kp2))
|
||||
|
||||
|
||||
indices1 = np.random.choice(len(kp1), n_matches, replace=False)
|
||||
indices2 = np.random.choice(len(kp2), n_matches, replace=False)
|
||||
|
||||
|
||||
matches = np.stack([indices1, indices2], axis=1)
|
||||
scores = np.random.rand(n_matches)
|
||||
|
||||
|
||||
return {
|
||||
"matches": matches,
|
||||
"scores": scores,
|
||||
"keypoints1": kp1[indices1],
|
||||
"keypoints2": kp2[indices2]
|
||||
}
|
||||
|
||||
|
||||
elif self.model_name == "DINOv2":
|
||||
# Mock generating 4096-dim VLAD descriptor
|
||||
dim = 4096
|
||||
desc = np.random.rand(dim)
|
||||
# L2 normalize
|
||||
return desc / np.linalg.norm(desc)
|
||||
|
||||
|
||||
elif self.model_name in ("LiteSAM", "XFeat"):
|
||||
# Mock LiteSAM / XFeat matching between UAV and satellite image.
|
||||
# Returns homography, inlier_count, total_correspondences, confidence.
|
||||
@@ -144,9 +143,9 @@ class TRTInferenceEngine(InferenceEngine):
|
||||
|
||||
def _load(self):
|
||||
try:
|
||||
import tensorrt as trt # type: ignore
|
||||
import pycuda.driver as cuda # type: ignore
|
||||
import pycuda.autoinit # type: ignore # noqa: F401
|
||||
import pycuda.driver # type: ignore # noqa: F401
|
||||
import tensorrt as trt # type: ignore
|
||||
|
||||
trt_logger = trt.Logger(trt.Logger.WARNING)
|
||||
self._runtime = trt.Runtime(trt_logger)
|
||||
|
||||
@@ -9,7 +9,12 @@ import cv2
|
||||
import numpy as np
|
||||
|
||||
from gps_denied.schemas.image import (
|
||||
ImageBatch, ImageData, ImageMetadata, ProcessedBatch, ProcessingStatus, ValidationResult
|
||||
ImageBatch,
|
||||
ImageData,
|
||||
ImageMetadata,
|
||||
ProcessedBatch,
|
||||
ProcessingStatus,
|
||||
ValidationResult,
|
||||
)
|
||||
|
||||
|
||||
@@ -50,26 +55,26 @@ class ImageInputPipeline:
|
||||
def validate_batch(self, batch: ImageBatch) -> ValidationResult:
|
||||
"""Validates batch integrity and sequence continuity."""
|
||||
errors = []
|
||||
|
||||
|
||||
num_images = len(batch.images)
|
||||
if num_images < 1:
|
||||
errors.append("Batch is empty")
|
||||
elif num_images > 100:
|
||||
errors.append("Batch too large")
|
||||
|
||||
|
||||
if len(batch.filenames) != num_images:
|
||||
errors.append("Mismatch between filenames and images count")
|
||||
|
||||
|
||||
# Naming convention ADxxxxxx.jpg or similar
|
||||
pattern = re.compile(r"^[A-Za-z0-9_-]+\.(jpg|jpeg|png)$", re.IGNORECASE)
|
||||
for fn in batch.filenames:
|
||||
if not pattern.match(fn):
|
||||
errors.append(f"Invalid filename: {fn}")
|
||||
break
|
||||
|
||||
|
||||
if batch.start_sequence > batch.end_sequence:
|
||||
errors.append("Start sequence greater than end sequence")
|
||||
|
||||
|
||||
return ValidationResult(valid=len(errors) == 0, errors=errors)
|
||||
|
||||
def queue_batch(self, flight_id: str, batch: ImageBatch) -> bool:
|
||||
@@ -83,10 +88,10 @@ class ImageInputPipeline:
|
||||
raise QueueFullError(f"Queue for flight {flight_id} is full")
|
||||
|
||||
q.put_nowait(batch)
|
||||
|
||||
|
||||
self._init_status(flight_id)
|
||||
self._status[flight_id]["total_images"] += len(batch.images)
|
||||
|
||||
|
||||
return True
|
||||
|
||||
async def process_next_batch(self, flight_id: str) -> ProcessedBatch | None:
|
||||
@@ -94,21 +99,21 @@ class ImageInputPipeline:
|
||||
q = self._get_queue(flight_id)
|
||||
if q.empty():
|
||||
return None
|
||||
|
||||
|
||||
batch: ImageBatch = await q.get()
|
||||
|
||||
|
||||
processed_images = []
|
||||
for i, raw_bytes in enumerate(batch.images):
|
||||
# Decode
|
||||
nparr = np.frombuffer(raw_bytes, np.uint8)
|
||||
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
|
||||
|
||||
|
||||
if img is None:
|
||||
continue # skip corrupted
|
||||
|
||||
|
||||
seq = batch.start_sequence + i
|
||||
fn = batch.filenames[i]
|
||||
|
||||
|
||||
h, w = img.shape[:2]
|
||||
meta = ImageMetadata(
|
||||
sequence=seq,
|
||||
@@ -117,7 +122,7 @@ class ImageInputPipeline:
|
||||
file_size=len(raw_bytes),
|
||||
timestamp=datetime.now(timezone.utc),
|
||||
)
|
||||
|
||||
|
||||
img_data = ImageData(
|
||||
flight_id=flight_id,
|
||||
sequence=seq,
|
||||
@@ -128,13 +133,13 @@ class ImageInputPipeline:
|
||||
processed_images.append(img_data)
|
||||
# VO-05: record exact sequence→filename mapping
|
||||
self._sequence_map.setdefault(flight_id, {})[seq] = fn
|
||||
|
||||
|
||||
# Store to disk
|
||||
self.store_images(flight_id, processed_images)
|
||||
|
||||
|
||||
self._status[flight_id]["processed_images"] += len(processed_images)
|
||||
q.task_done()
|
||||
|
||||
|
||||
return ProcessedBatch(
|
||||
images=processed_images,
|
||||
batch_id=f"batch_{batch.batch_number}",
|
||||
@@ -146,22 +151,22 @@ class ImageInputPipeline:
|
||||
"""Persists images to disk."""
|
||||
flight_dir = os.path.join(self.storage_dir, flight_id)
|
||||
os.makedirs(flight_dir, exist_ok=True)
|
||||
|
||||
|
||||
for img in images:
|
||||
path = os.path.join(flight_dir, img.filename)
|
||||
cv2.imwrite(path, img.image)
|
||||
|
||||
|
||||
return True
|
||||
|
||||
def get_next_image(self, flight_id: str) -> ImageData | None:
|
||||
"""Gets the next image in sequence for processing."""
|
||||
self._init_status(flight_id)
|
||||
seq = self._status[flight_id]["current_sequence"]
|
||||
|
||||
|
||||
img = self.get_image_by_sequence(flight_id, seq)
|
||||
if img:
|
||||
self._status[flight_id]["current_sequence"] += 1
|
||||
|
||||
|
||||
return img
|
||||
|
||||
def get_image_by_sequence(self, flight_id: str, sequence: int) -> ImageData | None:
|
||||
@@ -211,7 +216,7 @@ class ImageInputPipeline:
|
||||
self._init_status(flight_id)
|
||||
s = self._status[flight_id]
|
||||
q = self._get_queue(flight_id)
|
||||
|
||||
|
||||
return ProcessingStatus(
|
||||
flight_id=flight_id,
|
||||
total_images=s["total_images"],
|
||||
|
||||
@@ -9,7 +9,6 @@ from __future__ import annotations
|
||||
import asyncio
|
||||
import logging
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from enum import Enum
|
||||
from typing import Optional
|
||||
|
||||
@@ -20,8 +19,7 @@ from gps_denied.core.pipeline import ImageInputPipeline
|
||||
from gps_denied.core.results import ResultManager
|
||||
from gps_denied.core.sse import SSEEventStreamer
|
||||
from gps_denied.db.repository import FlightRepository
|
||||
from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.schemas import CameraParameters
|
||||
from gps_denied.schemas import CameraParameters, GPSPoint
|
||||
from gps_denied.schemas.flight import (
|
||||
BatchMetadata,
|
||||
BatchResponse,
|
||||
@@ -37,7 +35,6 @@ from gps_denied.schemas.flight import (
|
||||
UserFixResponse,
|
||||
Waypoint,
|
||||
)
|
||||
from gps_denied.schemas.image import ImageBatch
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
import logging
|
||||
from abc import ABC, abstractmethod
|
||||
from typing import List, Optional
|
||||
from typing import List
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -40,7 +40,7 @@ class FailureRecoveryCoordinator(IFailureRecoveryCoordinator):
|
||||
def handle_tracking_lost(self, flight_id: str, current_frame_id: int) -> bool:
|
||||
"""Called when F07 fails to find sequential matches."""
|
||||
logger.warning(f"Tracking lost for flight {flight_id} at frame {current_frame_id}")
|
||||
|
||||
|
||||
# Create a new active chunk to record new relative frames independently
|
||||
self.chunk_manager.create_new_chunk(flight_id, current_frame_id)
|
||||
return True
|
||||
@@ -51,25 +51,25 @@ class FailureRecoveryCoordinator(IFailureRecoveryCoordinator):
|
||||
and run metric refinement to provide an absolute Sim3 alignment.
|
||||
"""
|
||||
self.chunk_manager.update_chunk_status(flight_id, chunk_id, ChunkStatus.MATCHING)
|
||||
|
||||
|
||||
candidates = self.gpr.retrieve_candidate_tiles_for_chunk(images, top_k=5)
|
||||
|
||||
|
||||
if not candidates:
|
||||
self.chunk_manager.update_chunk_status(flight_id, chunk_id, ChunkStatus.UNANCHORED)
|
||||
return False
|
||||
|
||||
# Take topmost candidate
|
||||
best_candidate = candidates[0]
|
||||
|
||||
|
||||
# We need mock tile for Metric Refinement
|
||||
mock_tile = np.zeros((256, 256, 3))
|
||||
|
||||
|
||||
align_result = self.metric_refinement.align_chunk_to_satellite(
|
||||
chunk_images=images,
|
||||
satellite_tile=mock_tile,
|
||||
tile_bounds=best_candidate.bounds
|
||||
)
|
||||
|
||||
|
||||
if align_result and align_result.matched:
|
||||
# Anchor successfully placed on chunk
|
||||
active_chunk = self.chunk_manager.get_active_chunk(flight_id)
|
||||
|
||||
@@ -30,8 +30,8 @@ class ResultManager:
|
||||
refined: bool = False,
|
||||
) -> bool:
|
||||
"""Atomic DB update + SSE event publish."""
|
||||
|
||||
# 1. Update DB (in the repository these are auto-committing via flush,
|
||||
|
||||
# 1. Update DB (in the repository these are auto-committing via flush,
|
||||
# but normally F03 would wrap in a single transaction).
|
||||
await self.repo.save_frame_result(
|
||||
flight_id,
|
||||
@@ -43,15 +43,15 @@ class ResultManager:
|
||||
confidence=confidence,
|
||||
refined=refined,
|
||||
)
|
||||
|
||||
# Wait, the spec also wants Waypoints to be updated.
|
||||
# But image frames != waypoints. Waypoints are the planned route.
|
||||
|
||||
# Wait, the spec also wants Waypoints to be updated.
|
||||
# But image frames != waypoints. Waypoints are the planned route.
|
||||
# Actually in the spec it says: "Updates waypoint in waypoints table."
|
||||
# This implies updating the closest waypoint or a generated waypoint path.
|
||||
# We will follow the simplest form for now: update the waypoint if there is one corresponding.
|
||||
# Let's say we update a waypoint with id "wp_{frame_id}" for now if we know how they map,
|
||||
# Let's say we update a waypoint with id "wp_{frame_id}" for now if we know how they map,
|
||||
# or we just skip unless specified.
|
||||
|
||||
|
||||
# 2. Trigger SSE event
|
||||
evt = FrameProcessedEvent(
|
||||
frame_id=frame_id,
|
||||
@@ -65,7 +65,7 @@ class ResultManager:
|
||||
self.sse.send_refinement(flight_id, evt)
|
||||
else:
|
||||
self.sse.send_frame_result(flight_id, evt)
|
||||
|
||||
|
||||
return True
|
||||
|
||||
async def publish_waypoint_update(self, flight_id: str, frame_id: int) -> bool:
|
||||
|
||||
@@ -1,8 +1,7 @@
|
||||
"""Image Rotation Manager (Component F06)."""
|
||||
|
||||
import math
|
||||
from datetime import datetime
|
||||
from abc import ABC, abstractmethod
|
||||
from datetime import datetime
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
@@ -14,7 +13,10 @@ from gps_denied.schemas.satellite import TileBounds
|
||||
class IImageMatcher(ABC):
|
||||
"""Dependency injection interface for Metric Refinement."""
|
||||
@abstractmethod
|
||||
def align_to_satellite(self, uav_image: np.ndarray, satellite_tile: np.ndarray, tile_bounds: TileBounds) -> RotationResult:
|
||||
def align_to_satellite(
|
||||
self, uav_image: np.ndarray, satellite_tile: np.ndarray,
|
||||
tile_bounds: TileBounds,
|
||||
) -> RotationResult:
|
||||
pass
|
||||
|
||||
|
||||
@@ -34,18 +36,18 @@ class ImageRotationManager:
|
||||
"""Rotates an image by specified angle around center."""
|
||||
if angle == 0.0 or angle == 360.0:
|
||||
return image
|
||||
|
||||
|
||||
h, w = image.shape[:2]
|
||||
center = (w / 2, h / 2)
|
||||
|
||||
|
||||
# Get rotation matrix. Negative angle for standard counter-clockwise interpretation in some math
|
||||
# or positive for OpenCV's coordinate system.
|
||||
matrix = cv2.getRotationMatrix2D(center, angle, 1.0)
|
||||
|
||||
|
||||
rotated = cv2.warpAffine(
|
||||
image, matrix, (w, h),
|
||||
flags=cv2.INTER_LINEAR,
|
||||
borderMode=cv2.BORDER_CONSTANT,
|
||||
image, matrix, (w, h),
|
||||
flags=cv2.INTER_LINEAR,
|
||||
borderMode=cv2.BORDER_CONSTANT,
|
||||
borderValue=(0, 0, 0)
|
||||
)
|
||||
return rotated
|
||||
@@ -71,26 +73,26 @@ class ImageRotationManager:
|
||||
for angle in range(0, 360, 30):
|
||||
rotated = self.rotate_image_360(image, float(angle))
|
||||
result = matcher.align_to_satellite(rotated, satellite_tile, tile_bounds)
|
||||
|
||||
|
||||
if result.matched:
|
||||
precise_angle = self.calculate_precise_angle(result.homography, float(angle))
|
||||
result.precise_angle = precise_angle
|
||||
result.initial_angle = float(angle)
|
||||
|
||||
|
||||
self.update_heading(flight_id, frame_id, precise_angle, timestamp)
|
||||
return result
|
||||
|
||||
|
||||
return None
|
||||
|
||||
def calculate_precise_angle(self, homography: np.ndarray | None, initial_angle: float) -> float:
|
||||
"""Calculates precise rotation angle from homography matrix."""
|
||||
if homography is None:
|
||||
return initial_angle
|
||||
|
||||
|
||||
# Extract rotation angle from 2D affine component of homography
|
||||
# h00, h01 = homography[0, 0], homography[0, 1]
|
||||
# angle_delta = math.degrees(math.atan2(h01, h00))
|
||||
|
||||
|
||||
# For simplicity in mock, just return initial
|
||||
return initial_angle
|
||||
|
||||
@@ -102,18 +104,18 @@ class ImageRotationManager:
|
||||
def update_heading(self, flight_id: str, frame_id: int, heading: float, timestamp: datetime) -> bool:
|
||||
"""Updates UAV heading angle."""
|
||||
self._init_flight(flight_id)
|
||||
|
||||
|
||||
# Normalize to 0-360
|
||||
normalized = heading % 360.0
|
||||
|
||||
|
||||
hist = self._history[flight_id]
|
||||
hist.current_heading = normalized
|
||||
hist.last_update = timestamp
|
||||
|
||||
|
||||
hist.heading_history.append(normalized)
|
||||
if len(hist.heading_history) > 10:
|
||||
hist.heading_history.pop(0)
|
||||
|
||||
|
||||
return True
|
||||
|
||||
def detect_sharp_turn(self, flight_id: str, new_heading: float) -> bool:
|
||||
@@ -121,20 +123,20 @@ class ImageRotationManager:
|
||||
current = self.get_current_heading(flight_id)
|
||||
if current is None:
|
||||
return False
|
||||
|
||||
|
||||
delta = abs(new_heading - current)
|
||||
if delta > 180:
|
||||
delta = 360 - delta
|
||||
|
||||
|
||||
return delta > 45.0
|
||||
|
||||
def requires_rotation_sweep(self, flight_id: str) -> bool:
|
||||
"""Determines if rotation sweep is needed for current frame."""
|
||||
self._init_flight(flight_id)
|
||||
hist = self._history[flight_id]
|
||||
|
||||
|
||||
# First frame scenario
|
||||
if hist.current_heading is None:
|
||||
return True
|
||||
|
||||
|
||||
return False
|
||||
|
||||
@@ -4,10 +4,8 @@ SAT-01: Reads pre-loaded tiles from a local z/x/y directory (no live HTTP during
|
||||
SAT-02: Tile selection uses ESKF position ± 3σ_horizontal to define search area.
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import math
|
||||
import os
|
||||
from collections.abc import Iterator
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
|
||||
import cv2
|
||||
|
||||
@@ -5,7 +5,6 @@ from __future__ import annotations
|
||||
import asyncio
|
||||
import json
|
||||
from collections import defaultdict
|
||||
from collections.abc import AsyncGenerator
|
||||
|
||||
from gps_denied.schemas.events import (
|
||||
FlightCompletedEvent,
|
||||
@@ -49,7 +48,7 @@ class SSEEventStreamer:
|
||||
"""Broadcast a message to all clients subscribed to flight_id."""
|
||||
if flight_id not in self._streams or not self._streams[flight_id]:
|
||||
return False
|
||||
|
||||
|
||||
for q in self._streams[flight_id].values():
|
||||
try:
|
||||
q.put_nowait(msg)
|
||||
@@ -101,7 +100,7 @@ class SSEEventStreamer:
|
||||
# but we can just send an SSEMessage object that parses as empty event
|
||||
if flight_id not in self._streams:
|
||||
return False
|
||||
|
||||
|
||||
# Manually sending a comment via the generator is tricky with strict SSEMessage schema
|
||||
# but we'll handle this in the stream generator directly
|
||||
return True
|
||||
@@ -111,7 +110,7 @@ class SSEEventStreamer:
|
||||
async def stream_generator(self, flight_id: str, client_id: str):
|
||||
"""Yields dicts for sse_starlette EventSourceResponse."""
|
||||
q = self.create_stream(flight_id, client_id)
|
||||
|
||||
|
||||
# Send an immediate connection accepted ping
|
||||
yield {"event": "connected", "data": "connected"}
|
||||
|
||||
@@ -123,18 +122,18 @@ class SSEEventStreamer:
|
||||
if msg is None:
|
||||
# Sentinel for clean shutdown
|
||||
break
|
||||
|
||||
|
||||
# Yield dict format for sse_starlette
|
||||
yield {
|
||||
"event": msg.event.value,
|
||||
"id": msg.id if msg.id else "",
|
||||
"data": json.dumps(msg.data)
|
||||
}
|
||||
|
||||
|
||||
except asyncio.TimeoutError:
|
||||
# Heartbeat format for sse_starlette (empty string generates a comment)
|
||||
yield {"event": "heartbeat", "data": "ping"}
|
||||
|
||||
|
||||
except asyncio.CancelledError:
|
||||
pass # Client disconnected
|
||||
finally:
|
||||
|
||||
+25
-21
@@ -28,15 +28,15 @@ class ISequentialVisualOdometry(ABC):
|
||||
self, prev_image: np.ndarray, curr_image: np.ndarray, camera_params: CameraParameters
|
||||
) -> RelativePose | None:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def extract_features(self, image: np.ndarray) -> Features:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def match_features(self, features1: Features, features2: Features) -> Matches:
|
||||
pass
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def estimate_motion(self, matches: Matches, camera_params: CameraParameters) -> Motion | None:
|
||||
pass
|
||||
@@ -52,7 +52,7 @@ class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
"""Extracts keypoints and descriptors using SuperPoint."""
|
||||
engine = self.model_manager.get_inference_engine("SuperPoint")
|
||||
result = engine.infer(image)
|
||||
|
||||
|
||||
return Features(
|
||||
keypoints=result["keypoints"],
|
||||
descriptors=result["descriptors"],
|
||||
@@ -66,7 +66,7 @@ class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
"features1": features1,
|
||||
"features2": features2
|
||||
})
|
||||
|
||||
|
||||
return Matches(
|
||||
matches=result["matches"],
|
||||
scores=result["scores"],
|
||||
@@ -79,10 +79,10 @@ class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
inlier_threshold = 20
|
||||
if len(matches.matches) < 8:
|
||||
return None
|
||||
|
||||
|
||||
pts1 = np.ascontiguousarray(matches.keypoints1)
|
||||
pts2 = np.ascontiguousarray(matches.keypoints2)
|
||||
|
||||
|
||||
# Build camera matrix
|
||||
f_px = camera_params.focal_length * (camera_params.resolution_width / camera_params.sensor_width)
|
||||
if camera_params.principal_point:
|
||||
@@ -90,13 +90,13 @@ class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
else:
|
||||
cx = camera_params.resolution_width / 2.0
|
||||
cy = camera_params.resolution_height / 2.0
|
||||
|
||||
|
||||
K = np.array([
|
||||
[f_px, 0, cx],
|
||||
[0, f_px, cy],
|
||||
[0, 0, 1]
|
||||
], dtype=np.float64)
|
||||
|
||||
|
||||
try:
|
||||
E, inliers = cv2.findEssentialMat(
|
||||
pts1, pts2, cameraMatrix=K, method=cv2.RANSAC, prob=0.999, threshold=1.0
|
||||
@@ -104,24 +104,24 @@ class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
except Exception as e:
|
||||
logger.error(f"Error finding essential matrix: {e}")
|
||||
return None
|
||||
|
||||
|
||||
if E is None or E.shape != (3, 3):
|
||||
return None
|
||||
|
||||
|
||||
inliers_mask = inliers.flatten().astype(bool)
|
||||
inlier_count = np.sum(inliers_mask)
|
||||
|
||||
|
||||
if inlier_count < inlier_threshold:
|
||||
logger.warning(f"Insufficient inliers: {inlier_count} < {inlier_threshold}")
|
||||
return None
|
||||
|
||||
|
||||
# Recover pose
|
||||
try:
|
||||
_, R, t, mask = cv2.recoverPose(E, pts1, pts2, cameraMatrix=K, mask=inliers)
|
||||
except Exception as e:
|
||||
logger.error(f"Error recovering pose: {e}")
|
||||
return None
|
||||
|
||||
|
||||
return Motion(
|
||||
translation=t.flatten(),
|
||||
rotation=R,
|
||||
@@ -135,16 +135,16 @@ class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
"""Computes relative pose between two frames."""
|
||||
f1 = self.extract_features(prev_image)
|
||||
f2 = self.extract_features(curr_image)
|
||||
|
||||
|
||||
matches = self.match_features(f1, f2)
|
||||
|
||||
|
||||
motion = self.estimate_motion(matches, camera_params)
|
||||
|
||||
|
||||
if motion is None:
|
||||
return None
|
||||
|
||||
|
||||
tracking_good = motion.inlier_count > 50
|
||||
|
||||
|
||||
return RelativePose(
|
||||
translation=motion.translation,
|
||||
rotation=motion.rotation,
|
||||
@@ -228,8 +228,12 @@ class ORBVisualOdometry(ISequentialVisualOdometry):
|
||||
f_px = camera_params.focal_length * (
|
||||
camera_params.resolution_width / camera_params.sensor_width
|
||||
)
|
||||
cx = camera_params.principal_point[0] if camera_params.principal_point else camera_params.resolution_width / 2.0
|
||||
cy = camera_params.principal_point[1] if camera_params.principal_point else camera_params.resolution_height / 2.0
|
||||
cx = (camera_params.principal_point[0]
|
||||
if camera_params.principal_point
|
||||
else camera_params.resolution_width / 2.0)
|
||||
cy = (camera_params.principal_point[1]
|
||||
if camera_params.principal_point
|
||||
else camera_params.resolution_height / 2.0)
|
||||
K = np.array([[f_px, 0, cx], [0, f_px, cy], [0, 0, 1]], dtype=np.float64)
|
||||
try:
|
||||
E, inliers = cv2.findEssentialMat(pts1, pts2, cameraMatrix=K, method=cv2.RANSAC, prob=0.999, threshold=1.0)
|
||||
|
||||
@@ -6,13 +6,13 @@ import uuid
|
||||
from datetime import datetime, timezone
|
||||
|
||||
from sqlalchemy import (
|
||||
JSON,
|
||||
Boolean,
|
||||
DateTime,
|
||||
Float,
|
||||
ForeignKey,
|
||||
Index,
|
||||
Integer,
|
||||
JSON,
|
||||
String,
|
||||
Text,
|
||||
)
|
||||
|
||||
@@ -2,8 +2,6 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
from pydantic import BaseModel, Field
|
||||
|
||||
|
||||
@@ -39,4 +37,9 @@ class Geofences(BaseModel):
|
||||
polygons: list[Polygon] = Field(default_factory=list)
|
||||
|
||||
|
||||
from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, ESKFState, IMUMeasurement # noqa: E402
|
||||
from gps_denied.schemas.eskf import ( # noqa: E402, I001
|
||||
ConfidenceTier as ConfidenceTier,
|
||||
ESKFConfig as ESKFConfig,
|
||||
ESKFState as ESKFState,
|
||||
IMUMeasurement as IMUMeasurement,
|
||||
)
|
||||
|
||||
@@ -23,10 +23,10 @@ class ChunkHandle(BaseModel):
|
||||
start_frame_id: int
|
||||
end_frame_id: Optional[int] = None
|
||||
frames: List[int] = []
|
||||
|
||||
|
||||
is_active: bool = True
|
||||
has_anchor: bool = False
|
||||
|
||||
|
||||
anchor_frame_id: Optional[int] = None
|
||||
anchor_gps: Optional[GPSPoint] = None
|
||||
matching_status: ChunkStatus = ChunkStatus.UNANCHORED
|
||||
|
||||
@@ -4,7 +4,7 @@ from enum import Enum
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
from pydantic import BaseModel, Field
|
||||
from pydantic import BaseModel
|
||||
|
||||
|
||||
class ConfidenceTier(str, Enum):
|
||||
|
||||
@@ -3,13 +3,11 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
from typing import Optional
|
||||
|
||||
from pydantic import BaseModel, Field
|
||||
|
||||
from gps_denied.schemas import CameraParameters, Geofences, GPSPoint
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Waypoint
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@@ -3,8 +3,8 @@
|
||||
from datetime import datetime
|
||||
from typing import Optional
|
||||
|
||||
from pydantic import BaseModel
|
||||
import numpy as np
|
||||
from pydantic import BaseModel
|
||||
|
||||
|
||||
class ImageBatch(BaseModel):
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
"""MAVLink I/O schemas (Component — Phase 4)."""
|
||||
|
||||
from typing import Optional
|
||||
|
||||
from pydantic import BaseModel
|
||||
|
||||
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
"""Metric Refinement schemas (Component F09)."""
|
||||
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
from pydantic import BaseModel
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
"""Model Manager schemas (Component F16)."""
|
||||
|
||||
from typing import Any
|
||||
|
||||
from pydantic import BaseModel
|
||||
|
||||
|
||||
class ModelConfig(BaseModel):
|
||||
"""Configuration for an ML model."""
|
||||
model_name: str
|
||||
|
||||
@@ -3,8 +3,8 @@
|
||||
from datetime import datetime
|
||||
from typing import Optional
|
||||
|
||||
from pydantic import BaseModel
|
||||
import numpy as np
|
||||
from pydantic import BaseModel
|
||||
|
||||
|
||||
class RotationResult(BaseModel):
|
||||
@@ -13,9 +13,9 @@ class RotationResult(BaseModel):
|
||||
initial_angle: float
|
||||
precise_angle: float
|
||||
confidence: float
|
||||
# We will exclude np.ndarray from BaseModel to avoid validation issues,
|
||||
# We will exclude np.ndarray from BaseModel to avoid validation issues,
|
||||
# but store it as an attribute if needed or use arbitrary_types_allowed.
|
||||
|
||||
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
homography: Optional[np.ndarray] = None
|
||||
inlier_count: int = 0
|
||||
|
||||
@@ -9,7 +9,7 @@ from pydantic import BaseModel
|
||||
class Features(BaseModel):
|
||||
"""Extracted image features (e.g., from SuperPoint)."""
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
|
||||
|
||||
keypoints: np.ndarray # (N, 2)
|
||||
descriptors: np.ndarray # (N, 256)
|
||||
scores: np.ndarray # (N,)
|
||||
@@ -18,7 +18,7 @@ class Features(BaseModel):
|
||||
class Matches(BaseModel):
|
||||
"""Matches between two sets of features (e.g., from LightGlue)."""
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
|
||||
|
||||
matches: np.ndarray # (M, 2)
|
||||
scores: np.ndarray # (M,)
|
||||
keypoints1: np.ndarray # (M, 2)
|
||||
@@ -28,7 +28,7 @@ class Matches(BaseModel):
|
||||
class RelativePose(BaseModel):
|
||||
"""Relative pose between two frames."""
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
|
||||
|
||||
translation: np.ndarray # (3,)
|
||||
rotation: np.ndarray # (3, 3)
|
||||
confidence: float
|
||||
@@ -42,7 +42,7 @@ class RelativePose(BaseModel):
|
||||
class Motion(BaseModel):
|
||||
"""Motion estimate from OpenCV."""
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
|
||||
|
||||
translation: np.ndarray # (3,) unit vector
|
||||
rotation: np.ndarray # (3, 3) rotation matrix
|
||||
inliers: np.ndarray # Boolean mask of inliers
|
||||
|
||||
@@ -29,15 +29,15 @@ def compute_tile_bounds(coords: TileCoords) -> TileBounds:
|
||||
nw = tile_to_latlon(coords.x, coords.y, coords.zoom)
|
||||
se = tile_to_latlon(coords.x + 1, coords.y + 1, coords.zoom)
|
||||
center = tile_to_latlon(coords.x + 0.5, coords.y + 0.5, coords.zoom)
|
||||
|
||||
|
||||
ne = GPSPoint(lat=nw.lat, lon=se.lon)
|
||||
sw = GPSPoint(lat=se.lat, lon=nw.lon)
|
||||
|
||||
|
||||
# Calculate GSD (meters per pixel at this latitude)
|
||||
# Assumes standard 256x256 Web Mercator tile
|
||||
lat_rad = math.radians(center.lat)
|
||||
gsd = 156543.03392 * math.cos(lat_rad) / (2 ** coords.zoom)
|
||||
|
||||
|
||||
return TileBounds(
|
||||
nw=nw,
|
||||
ne=ne,
|
||||
|
||||
Reference in New Issue
Block a user