mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 08:56:37 +00:00
Yuzviak
094895b21b
Phase 2 — Visual Odometry: - ORBVisualOdometry (dev/CI), CuVSLAMVisualOdometry (Jetson) - TRTInferenceEngine (TensorRT FP16, conditional import) - create_vo_backend() factory Phase 3 — Satellite Matching + GPR: - SatelliteDataManager: local z/x/y tiles, ESKF ±3σ tile selection - GSD normalization (SAT-03), RANSAC inlier-ratio confidence (SAT-04) - GlobalPlaceRecognition: Faiss index + numpy fallback Phase 4 — MAVLink I/O: - MAVLinkBridge: GPS_INPUT 15+ fields, IMU callback, 1Hz telemetry - 3-consecutive-failure reloc request - MockMAVConnection for CI Phase 5 — Pipeline Wiring: - ESKF wired into process_frame: VO update → satellite update - CoordinateTransformer + SatelliteDataManager via DI - MAVLink state push per frame (PIPE-07) - Real pixel_to_gps via ray-ground projection (PIPE-06) - GTSAM ISAM2 update when available (PIPE-03) Phase 6 — Docker + CI: - Multi-stage Dockerfile (python:3.11-slim) - docker-compose.yml (dev), docker-compose.sitl.yml (ArduPilot SITL) - GitHub Actions: ci.yml (lint+pytest+docker smoke), sitl.yml (nightly) - tests/test_sitl_integration.py (8 tests, skip without SITL) Phase 7 — Accuracy Validation: - AccuracyBenchmark + SyntheticTrajectory - AC-PERF-1: 80% within 50m ✅ - AC-PERF-2: 60% within 20m ✅ - AC-PERF-3: p95 latency < 400ms ✅ - AC-PERF-4: VO drift 1km < 100m ✅ (actual ~11m) - scripts/benchmark_accuracy.py CLI Tests: 195 passed / 8 skipped Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
576 lines
23 KiB
Python
576 lines
23 KiB
Python
"""Core Flight Processor — Full Processing Pipeline (Stage 10).
|
||
|
||
Orchestrates: ImageInputPipeline → VO → MetricRefinement → FactorGraph → SSE.
|
||
State Machine: NORMAL → LOST → RECOVERY → NORMAL.
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import asyncio
|
||
import logging
|
||
import time
|
||
from datetime import datetime, timezone
|
||
from enum import Enum
|
||
from typing import Optional
|
||
|
||
import numpy as np
|
||
|
||
from gps_denied.core.eskf import ESKF
|
||
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.flight import (
|
||
BatchMetadata,
|
||
BatchResponse,
|
||
BatchUpdateResponse,
|
||
DeleteResponse,
|
||
FlightCreateRequest,
|
||
FlightDetailResponse,
|
||
FlightResponse,
|
||
FlightStatusResponse,
|
||
ObjectGPSResponse,
|
||
UpdateResponse,
|
||
UserFixRequest,
|
||
UserFixResponse,
|
||
Waypoint,
|
||
)
|
||
from gps_denied.schemas.image import ImageBatch
|
||
|
||
logger = logging.getLogger(__name__)
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# State Machine
|
||
# ---------------------------------------------------------------------------
|
||
class TrackingState(str, Enum):
|
||
"""Processing state for a flight."""
|
||
NORMAL = "normal"
|
||
LOST = "lost"
|
||
RECOVERY = "recovery"
|
||
|
||
|
||
class FrameResult:
|
||
"""Intermediate result of processing a single frame."""
|
||
|
||
def __init__(self, frame_id: int):
|
||
self.frame_id = frame_id
|
||
self.gps: Optional[GPSPoint] = None
|
||
self.confidence: float = 0.0
|
||
self.tracking_state: TrackingState = TrackingState.NORMAL
|
||
self.vo_success: bool = False
|
||
self.alignment_success: bool = False
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# FlightProcessor
|
||
# ---------------------------------------------------------------------------
|
||
class FlightProcessor:
|
||
"""Manages business logic, background processing, and frame orchestration."""
|
||
|
||
def __init__(self, repository: FlightRepository, streamer: SSEEventStreamer) -> None:
|
||
self.repository = repository
|
||
self.streamer = streamer
|
||
self.result_manager = ResultManager(repository, streamer)
|
||
self.pipeline = ImageInputPipeline(storage_dir=".image_storage", max_queue_size=50)
|
||
|
||
# Per-flight processing state
|
||
self._flight_states: dict[str, TrackingState] = {}
|
||
self._prev_images: dict[str, np.ndarray] = {} # previous frame cache
|
||
self._flight_cameras: dict[str, CameraParameters] = {} # per-flight camera
|
||
self._altitudes: dict[str, float] = {} # per-flight altitude (m)
|
||
self._failure_counts: dict[str, int] = {} # per-flight consecutive failure counter
|
||
|
||
# Per-flight ESKF instances (PIPE-01/07)
|
||
self._eskf: dict[str, ESKF] = {}
|
||
|
||
# Lazy-initialised component references (set via `attach_components`)
|
||
self._vo = None # ISequentialVisualOdometry
|
||
self._gpr = None # IGlobalPlaceRecognition
|
||
self._metric = None # IMetricRefinement
|
||
self._graph = None # IFactorGraphOptimizer
|
||
self._recovery = None # IFailureRecoveryCoordinator
|
||
self._chunk_mgr = None # IRouteChunkManager
|
||
self._rotation = None # ImageRotationManager
|
||
self._satellite = None # SatelliteDataManager (PIPE-02)
|
||
self._coord = None # CoordinateTransformer (PIPE-02/06)
|
||
self._mavlink = None # MAVLinkBridge (PIPE-07)
|
||
|
||
# ------ Dependency injection for core components ---------
|
||
def attach_components(
|
||
self,
|
||
vo=None,
|
||
gpr=None,
|
||
metric=None,
|
||
graph=None,
|
||
recovery=None,
|
||
chunk_mgr=None,
|
||
rotation=None,
|
||
satellite=None,
|
||
coord=None,
|
||
mavlink=None,
|
||
):
|
||
"""Attach pipeline components after construction (avoids circular deps)."""
|
||
self._vo = vo
|
||
self._gpr = gpr
|
||
self._metric = metric
|
||
self._graph = graph
|
||
self._recovery = recovery
|
||
self._chunk_mgr = chunk_mgr
|
||
self._rotation = rotation
|
||
self._satellite = satellite # PIPE-02: SatelliteDataManager
|
||
self._coord = coord # PIPE-02/06: CoordinateTransformer
|
||
self._mavlink = mavlink # PIPE-07: MAVLinkBridge
|
||
|
||
# ------ ESKF lifecycle helpers ----------------------------
|
||
def _init_eskf_for_flight(
|
||
self, flight_id: str, start_gps: GPSPoint, altitude: float
|
||
) -> None:
|
||
"""Create and initialize a per-flight ESKF instance."""
|
||
if flight_id in self._eskf:
|
||
return
|
||
eskf = ESKF()
|
||
if self._coord:
|
||
try:
|
||
e, n, _ = self._coord.gps_to_enu(flight_id, start_gps)
|
||
eskf.initialize(np.array([e, n, altitude]), time.time())
|
||
except Exception:
|
||
eskf.initialize(np.zeros(3), time.time())
|
||
else:
|
||
eskf.initialize(np.zeros(3), time.time())
|
||
self._eskf[flight_id] = eskf
|
||
|
||
def _eskf_to_gps(self, flight_id: str, eskf: ESKF) -> Optional[GPSPoint]:
|
||
"""Convert current ESKF ENU position to WGS84 GPS."""
|
||
if not eskf.initialized or eskf._nominal_state is None or self._coord is None:
|
||
return None
|
||
try:
|
||
pos = eskf._nominal_state["position"]
|
||
return self._coord.enu_to_gps(flight_id, (float(pos[0]), float(pos[1]), float(pos[2])))
|
||
except Exception:
|
||
return None
|
||
|
||
# =========================================================
|
||
# process_frame — central orchestration
|
||
# =========================================================
|
||
async def process_frame(
|
||
self,
|
||
flight_id: str,
|
||
frame_id: int,
|
||
image: np.ndarray,
|
||
) -> FrameResult:
|
||
"""
|
||
Process a single UAV frame through the full pipeline.
|
||
|
||
State transitions:
|
||
NORMAL — VO succeeds → ESKF VO update, attempt satellite fix
|
||
LOST — VO failed → create new chunk, enter RECOVERY
|
||
RECOVERY— try GPR + MetricRefinement → if anchored, merge & return to NORMAL
|
||
|
||
PIPE-01: VO result → eskf.update_vo → satellite match → eskf.update_satellite → MAVLink GPS_INPUT
|
||
PIPE-02: SatelliteDataManager + CoordinateTransformer wired for tile selection
|
||
PIPE-04: Consecutive failure counter wired to FailureRecoveryCoordinator
|
||
PIPE-05: ImageRotationManager initialised on first frame
|
||
PIPE-07: ESKF confidence → MAVLink fix_type via bridge.update_state
|
||
"""
|
||
result = FrameResult(frame_id)
|
||
state = self._flight_states.get(flight_id, TrackingState.NORMAL)
|
||
eskf = self._eskf.get(flight_id)
|
||
|
||
_default_cam = CameraParameters(
|
||
focal_length=4.5, sensor_width=6.17, sensor_height=4.55,
|
||
resolution_width=640, resolution_height=480,
|
||
)
|
||
|
||
# ---- PIPE-05: Initialise heading tracking on first frame ----
|
||
if self._rotation and frame_id == 0:
|
||
self._rotation.requires_rotation_sweep(flight_id) # seeds HeadingHistory
|
||
|
||
# ---- 1. Visual Odometry (frame-to-frame) ----
|
||
vo_ok = False
|
||
if self._vo and flight_id in self._prev_images:
|
||
try:
|
||
cam = self._flight_cameras.get(flight_id, _default_cam)
|
||
rel_pose = self._vo.compute_relative_pose(
|
||
self._prev_images[flight_id], image, cam
|
||
)
|
||
if rel_pose and rel_pose.tracking_good:
|
||
vo_ok = True
|
||
result.vo_success = True
|
||
|
||
if self._graph:
|
||
self._graph.add_relative_factor(
|
||
flight_id, frame_id - 1, frame_id, rel_pose, np.eye(6)
|
||
)
|
||
|
||
# PIPE-01: Feed VO relative displacement into ESKF
|
||
if eskf and eskf.initialized:
|
||
now = time.time()
|
||
dt_vo = max(0.01, now - (eskf._last_timestamp or now))
|
||
eskf.update_vo(rel_pose.translation, dt_vo)
|
||
except Exception as exc:
|
||
logger.warning("VO failed for frame %d: %s", frame_id, exc)
|
||
|
||
# Store current image for next frame
|
||
self._prev_images[flight_id] = image
|
||
|
||
# ---- PIPE-04: Consecutive failure counter ----
|
||
if not vo_ok and frame_id > 0:
|
||
self._failure_counts[flight_id] = self._failure_counts.get(flight_id, 0) + 1
|
||
else:
|
||
self._failure_counts[flight_id] = 0
|
||
|
||
# ---- 2. State Machine transitions ----
|
||
if state == TrackingState.NORMAL:
|
||
if not vo_ok and frame_id > 0:
|
||
state = TrackingState.LOST
|
||
logger.info("Flight %s → LOST at frame %d", flight_id, frame_id)
|
||
if self._recovery:
|
||
self._recovery.handle_tracking_lost(flight_id, frame_id)
|
||
|
||
if state == TrackingState.LOST:
|
||
state = TrackingState.RECOVERY
|
||
|
||
if state == TrackingState.RECOVERY:
|
||
recovered = False
|
||
if self._recovery and self._chunk_mgr:
|
||
active_chunk = self._chunk_mgr.get_active_chunk(flight_id)
|
||
if active_chunk:
|
||
recovered = self._recovery.process_chunk_recovery(
|
||
flight_id, active_chunk.chunk_id, [image]
|
||
)
|
||
if recovered:
|
||
state = TrackingState.NORMAL
|
||
result.alignment_success = True
|
||
# PIPE-04: Reset failure count on successful recovery
|
||
self._failure_counts[flight_id] = 0
|
||
logger.info("Flight %s recovered → NORMAL at frame %d", flight_id, frame_id)
|
||
|
||
# ---- 3. Satellite position fix (PIPE-01/02) ----
|
||
if state == TrackingState.NORMAL and self._metric:
|
||
sat_tile: Optional[np.ndarray] = None
|
||
tile_bounds = None
|
||
|
||
# PIPE-02: Prefer real SatelliteDataManager tiles (ESKF ±3σ selection)
|
||
if self._satellite and eskf and eskf.initialized:
|
||
gps_est = self._eskf_to_gps(flight_id, eskf)
|
||
if gps_est:
|
||
sigma_h = float(
|
||
np.sqrt(np.trace(eskf._P[0:3, 0:3]) / 3.0)
|
||
) if eskf._P is not None else 30.0
|
||
sigma_h = max(sigma_h, 5.0)
|
||
try:
|
||
tile_result = await asyncio.get_event_loop().run_in_executor(
|
||
None,
|
||
self._satellite.fetch_tiles_for_position,
|
||
gps_est, sigma_h, 18,
|
||
)
|
||
if tile_result:
|
||
sat_tile, tile_bounds = tile_result
|
||
except Exception as exc:
|
||
logger.debug("Satellite tile fetch failed: %s", exc)
|
||
|
||
# Fallback: GPR candidate tile (mock image, real bounds)
|
||
if sat_tile is None and self._gpr:
|
||
try:
|
||
candidates = self._gpr.retrieve_candidate_tiles(image, top_k=1)
|
||
if candidates:
|
||
sat_tile = np.zeros((256, 256, 3), dtype=np.uint8)
|
||
tile_bounds = candidates[0].bounds
|
||
except Exception as exc:
|
||
logger.debug("GPR tile fallback failed: %s", exc)
|
||
|
||
if sat_tile is not None and tile_bounds is not None:
|
||
try:
|
||
align = self._metric.align_to_satellite(image, sat_tile, tile_bounds)
|
||
if align and align.matched:
|
||
result.gps = align.gps_center
|
||
result.confidence = align.confidence
|
||
result.alignment_success = True
|
||
|
||
if self._graph:
|
||
self._graph.add_absolute_factor(
|
||
flight_id, frame_id,
|
||
align.gps_center, np.eye(6),
|
||
is_user_anchor=False,
|
||
)
|
||
|
||
# PIPE-01: ESKF satellite update — noise from RANSAC confidence
|
||
if eskf and eskf.initialized and self._coord:
|
||
try:
|
||
e, n, _ = self._coord.gps_to_enu(flight_id, align.gps_center)
|
||
alt = self._altitudes.get(flight_id, 100.0)
|
||
pos_enu = np.array([e, n, alt])
|
||
noise_m = 5.0 + 15.0 * (1.0 - float(align.confidence))
|
||
eskf.update_satellite(pos_enu, noise_m)
|
||
except Exception as exc:
|
||
logger.debug("ESKF satellite update failed: %s", exc)
|
||
except Exception as exc:
|
||
logger.warning("Metric alignment failed at frame %d: %s", frame_id, exc)
|
||
|
||
# ---- 4. Graph optimization (incremental) ----
|
||
if self._graph:
|
||
opt_result = self._graph.optimize(flight_id, iterations=5)
|
||
logger.debug(
|
||
"Optimization: converged=%s, error=%.4f",
|
||
opt_result.converged, opt_result.final_error,
|
||
)
|
||
|
||
# ---- PIPE-07: Push ESKF state → MAVLink GPS_INPUT ----
|
||
if self._mavlink and eskf and eskf.initialized:
|
||
try:
|
||
eskf_state = eskf.get_state()
|
||
alt = self._altitudes.get(flight_id, 100.0)
|
||
self._mavlink.update_state(eskf_state, altitude_m=alt)
|
||
except Exception as exc:
|
||
logger.debug("MAVLink state push failed: %s", exc)
|
||
|
||
# ---- 5. Publish via SSE ----
|
||
result.tracking_state = state
|
||
self._flight_states[flight_id] = state
|
||
await self._publish_frame_result(flight_id, result)
|
||
return result
|
||
|
||
async def _publish_frame_result(self, flight_id: str, result: FrameResult):
|
||
"""Emit SSE event for processed frame."""
|
||
event_data = {
|
||
"frame_id": result.frame_id,
|
||
"tracking_state": result.tracking_state.value,
|
||
"vo_success": result.vo_success,
|
||
"alignment_success": result.alignment_success,
|
||
"confidence": result.confidence,
|
||
}
|
||
if result.gps:
|
||
event_data["lat"] = result.gps.lat
|
||
event_data["lon"] = result.gps.lon
|
||
|
||
await self.streamer.push_event(
|
||
flight_id, event_type="frame_result", data=event_data
|
||
)
|
||
|
||
# =========================================================
|
||
# Existing CRUD / REST helpers (unchanged from Stage 3-4)
|
||
# =========================================================
|
||
async def create_flight(self, req: FlightCreateRequest) -> FlightResponse:
|
||
flight = await self.repository.insert_flight(
|
||
name=req.name,
|
||
description=req.description,
|
||
start_lat=req.start_gps.lat,
|
||
start_lon=req.start_gps.lon,
|
||
altitude=req.altitude,
|
||
camera_params=req.camera_params.model_dump(),
|
||
)
|
||
# P0#2: Store camera params for process_frame VO calls
|
||
self._flight_cameras[flight.id] = req.camera_params
|
||
|
||
for poly in req.geofences.polygons:
|
||
await self.repository.insert_geofence(
|
||
flight.id,
|
||
nw_lat=poly.north_west.lat,
|
||
nw_lon=poly.north_west.lon,
|
||
se_lat=poly.south_east.lat,
|
||
se_lon=poly.south_east.lon,
|
||
)
|
||
for w in req.rough_waypoints:
|
||
await self.repository.insert_waypoint(flight.id, lat=w.lat, lon=w.lon)
|
||
|
||
# Store per-flight altitude for ESKF/pixel projection
|
||
self._altitudes[flight.id] = req.altitude or 100.0
|
||
|
||
# PIPE-02: Set ENU origin and initialise ESKF for this flight
|
||
if self._coord:
|
||
self._coord.set_enu_origin(flight.id, req.start_gps)
|
||
self._init_eskf_for_flight(flight.id, req.start_gps, req.altitude or 100.0)
|
||
|
||
return FlightResponse(
|
||
flight_id=flight.id,
|
||
status="prefetching",
|
||
message="Flight created and prefetching started.",
|
||
created_at=flight.created_at,
|
||
)
|
||
|
||
async def get_flight(self, flight_id: str) -> FlightDetailResponse | None:
|
||
flight = await self.repository.get_flight(flight_id)
|
||
if not flight:
|
||
return None
|
||
wps = await self.repository.get_waypoints(flight_id)
|
||
state = await self.repository.load_flight_state(flight_id)
|
||
|
||
waypoints = [
|
||
Waypoint(
|
||
id=w.id,
|
||
lat=w.lat,
|
||
lon=w.lon,
|
||
altitude=w.altitude,
|
||
confidence=w.confidence,
|
||
timestamp=w.timestamp,
|
||
refined=w.refined,
|
||
)
|
||
for w in wps
|
||
]
|
||
|
||
status = state.status if state else "unknown"
|
||
frames_processed = state.frames_processed if state else 0
|
||
frames_total = state.frames_total if state else 0
|
||
|
||
from gps_denied.schemas import Geofences
|
||
|
||
return FlightDetailResponse(
|
||
flight_id=flight.id,
|
||
name=flight.name,
|
||
description=flight.description,
|
||
start_gps=GPSPoint(lat=flight.start_lat, lon=flight.start_lon),
|
||
waypoints=waypoints,
|
||
geofences=Geofences(polygons=[]),
|
||
camera_params=flight.camera_params,
|
||
altitude=flight.altitude,
|
||
status=status,
|
||
frames_processed=frames_processed,
|
||
frames_total=frames_total,
|
||
created_at=flight.created_at,
|
||
updated_at=flight.updated_at,
|
||
)
|
||
|
||
async def delete_flight(self, flight_id: str) -> DeleteResponse:
|
||
deleted = await self.repository.delete_flight(flight_id)
|
||
# P0#1: Cleanup in-memory state to prevent memory leaks
|
||
self._cleanup_flight(flight_id)
|
||
return DeleteResponse(deleted=deleted, flight_id=flight_id)
|
||
|
||
def _cleanup_flight(self, flight_id: str) -> None:
|
||
"""Remove all in-memory state for a flight (prevents memory leaks)."""
|
||
self._prev_images.pop(flight_id, None)
|
||
self._flight_states.pop(flight_id, None)
|
||
self._flight_cameras.pop(flight_id, None)
|
||
self._altitudes.pop(flight_id, None)
|
||
self._failure_counts.pop(flight_id, None)
|
||
self._eskf.pop(flight_id, None)
|
||
if self._graph:
|
||
self._graph.delete_flight_graph(flight_id)
|
||
|
||
async def update_waypoint(
|
||
self, flight_id: str, waypoint_id: str, waypoint: Waypoint
|
||
) -> UpdateResponse:
|
||
ok = await self.repository.update_waypoint(
|
||
flight_id,
|
||
waypoint_id,
|
||
lat=waypoint.lat,
|
||
lon=waypoint.lon,
|
||
altitude=waypoint.altitude,
|
||
confidence=waypoint.confidence,
|
||
refined=waypoint.refined,
|
||
)
|
||
return UpdateResponse(updated=ok, waypoint_id=waypoint_id)
|
||
|
||
async def batch_update_waypoints(
|
||
self, flight_id: str, waypoints: list[Waypoint]
|
||
) -> BatchUpdateResponse:
|
||
failed = []
|
||
updated = 0
|
||
for wp in waypoints:
|
||
ok = await self.repository.update_waypoint(
|
||
flight_id,
|
||
wp.id,
|
||
lat=wp.lat,
|
||
lon=wp.lon,
|
||
altitude=wp.altitude,
|
||
confidence=wp.confidence,
|
||
refined=wp.refined,
|
||
)
|
||
if ok:
|
||
updated += 1
|
||
else:
|
||
failed.append(wp.id)
|
||
return BatchUpdateResponse(
|
||
success=(len(failed) == 0), updated_count=updated, failed_ids=failed
|
||
)
|
||
|
||
async def queue_images(
|
||
self, flight_id: str, metadata: BatchMetadata, file_count: int
|
||
) -> BatchResponse:
|
||
state = await self.repository.load_flight_state(flight_id)
|
||
if state:
|
||
total = state.frames_total + file_count
|
||
await self.repository.save_flight_state(
|
||
flight_id, frames_total=total, status="processing"
|
||
)
|
||
|
||
next_seq = metadata.end_sequence + 1
|
||
seqs = list(range(metadata.start_sequence, metadata.end_sequence + 1))
|
||
return BatchResponse(
|
||
accepted=True,
|
||
sequences=seqs,
|
||
next_expected=next_seq,
|
||
message=f"Queued {file_count} images.",
|
||
)
|
||
|
||
async def handle_user_fix(
|
||
self, flight_id: str, req: UserFixRequest
|
||
) -> UserFixResponse:
|
||
await self.repository.save_flight_state(
|
||
flight_id, blocked=False, status="processing"
|
||
)
|
||
return UserFixResponse(
|
||
accepted=True, processing_resumed=True, message="Fix applied."
|
||
)
|
||
|
||
async def get_flight_status(self, flight_id: str) -> FlightStatusResponse | None:
|
||
state = await self.repository.load_flight_state(flight_id)
|
||
if not state:
|
||
return None
|
||
return FlightStatusResponse(
|
||
status=state.status,
|
||
frames_processed=state.frames_processed,
|
||
frames_total=state.frames_total,
|
||
current_frame=state.current_frame,
|
||
current_heading=None,
|
||
blocked=state.blocked,
|
||
search_grid_size=state.search_grid_size,
|
||
created_at=state.created_at,
|
||
updated_at=state.updated_at,
|
||
)
|
||
|
||
async def convert_object_to_gps(
|
||
self, flight_id: str, frame_id: int, pixel: tuple[float, float]
|
||
) -> ObjectGPSResponse:
|
||
# PIPE-06: Use real CoordinateTransformer + ESKF pose for ray-ground projection
|
||
gps: Optional[GPSPoint] = None
|
||
eskf = self._eskf.get(flight_id)
|
||
if self._coord and eskf and eskf.initialized and eskf._nominal_state is not None:
|
||
pos = eskf._nominal_state["position"]
|
||
quat = eskf._nominal_state["quaternion"]
|
||
cam = self._flight_cameras.get(flight_id, CameraParameters(
|
||
focal_length=4.5, sensor_width=6.17, sensor_height=4.55,
|
||
resolution_width=640, resolution_height=480,
|
||
))
|
||
alt = self._altitudes.get(flight_id, 100.0)
|
||
try:
|
||
gps = self._coord.pixel_to_gps(
|
||
flight_id,
|
||
pixel,
|
||
frame_pose={"position": pos},
|
||
camera_params=cam,
|
||
altitude=float(alt),
|
||
quaternion=quat,
|
||
)
|
||
except Exception as exc:
|
||
logger.debug("pixel_to_gps failed: %s", exc)
|
||
|
||
# Fallback: return ESKF position projected to ground (no pixel shift)
|
||
if gps is None and eskf:
|
||
gps = self._eskf_to_gps(flight_id, eskf)
|
||
|
||
return ObjectGPSResponse(
|
||
gps=gps or GPSPoint(lat=0.0, lon=0.0),
|
||
accuracy_meters=5.0,
|
||
frame_id=frame_id,
|
||
pixel=pixel,
|
||
)
|
||
|
||
async def stream_events(self, flight_id: str, client_id: str):
|
||
"""Async generator for SSE stream."""
|
||
async for event in self.streamer.stream_generator(flight_id, client_id):
|
||
yield event
|