#!/usr/bin/env python3
"""Top-down sparse map panel (seed v1 — replace the fake trail with real
pose estimation from T3; the point positions here are image-space stand-ins).
draw_topmap(snap, trail, size) -> BGR image, live-updated every frame:
- sparse points in the map frame
- camera path trail (real pose once T3 lands)
- scale strip
"""
import numpy as np
import cv2
import cv2
def draw_topmap(snap, trail, size=(640, 400)):
w, h = size
img = np.full((h, w, 3), 24, np.uint8)
cv2.putText(img, "SPARSE MAP (top-down)", (10, 22), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (200, 220, 255), 1)
cx, cy = w // 2, h // 2
for i, p in enumerate(snap.get("image_pts") or []):
x, y = p[0], p[1]
px = int(np.clip(cx + (x - w / 2) * 0.35, 2, w - 2))
py = int(np.clip(cy + (y - h / 2) * 0.35, 2, h - 2))
color = (150, 255, 150) if i % 3 else (255, 230, 110)
cv2.circle(img, (px, py), 2, color, -1)
if trail:
for j, (tx, ty) in enumerate(trail):
tpx = int(np.clip(cx + (tx - w / 2) * 0.35, 2, w - 2))
tpy = int(np.clip(cy + (ty - h / 2) * 0.35, 2, h - 2))
shade = int(255 * j / max(1, len(trail)))
cv2.circle(img, (tpx, tpy), 2, (shade, shade // 2, 255), -1)
# scale strip
cv2.line(img, (w - 90, h - 18), (w - 30, h - 18), (220, 220, 220), 2)
cv2.putText(img, "~2 m", (w - 90, h - 26), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (220, 220, 220), 1)
if not (snap.get("image_pts") or []):
cv2.putText(img, "no map points yet", (12, h // 2), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (90, 90, 240), 1)
return img