#! /usr/bin/python
# pip install fastapi uvicorn numpy scipy pyproj opencv-python
# app.py
# NEXT STEPS:
# Replace the in memory CAMERAS, OBSERVATIONS, and LANDMARKS with Postgres/PostGIS or a vector DB for persistence and scale.
# Swap the SciPy BA prototype for a production BA (Ceres/g2o) for robust global optimization.
# Integrate Grounding DINO / GLIP and MiDaS in a separate inference worker that calls /observe with pixel coordinates and timestamps.
# Add authentication, request validation, and provenance logging (signed observation receipts) for commercial use.
# Add unit tests for linear_triangulation, reprojection error, and the API endpoints.
"""
Minimal geometry-engine microservice (FastAPI) illustrating:
- register_camera
- observe
- triangulate
- solve_pnp
- bundle_adjust
- georeference
- query_landmarks
- explain
This is a prototype scaffold using in-memory stores and simple numeric routines.
Replace or extend numeric solvers and storage for production use.
"""
from typing import List, Dict, Any, Optional
from fastapi import FastAPI, HTTPException
from pydantic import BaseModel
import numpy as np
import uuid
import math
# Optional imports (wrap to provide clear error if missing)
try:
import cv2
except Exception:
cv2 = None
try:
from scipy.optimize import least_squares
except Exception:
least_squares = None
try:
from pyproj import Transformer
except Exception:
Transformer = None
app = FastAPI(title="Geometry Engine Prototype")
# -------------------------
# Data models and stores
# -------------------------
class CameraSpec(BaseModel):
camera_id: str
intrinsics: List[float] # fx, fy, cx, cy
# extrinsics: rotation (3x3 flattened) and translation (3)
rotation: Optional[List[float]] = None
translation: Optional[List[float]] = None
class Observation(BaseModel):
camera_id: str
frame_id: str
timestamp: float
bbox: Optional[List[float]] = None # [x1,y1,x2,y2]
pixel: Optional[List[float]] = None # [u, v] single point
label: Optional[str] = None
confidence: Optional[float] = None
# In-memory stores
CAMERAS: Dict[str, Dict[str, Any]] = {}
OBSERVATIONS: Dict[str, Dict[str, Any]] = {}
LANDMARKS: Dict[str, Dict[str, Any]] = {}
# -------------------------
# Utilities: linear algebra
# -------------------------
def intrinsics_matrix(fx, fy, cx, cy):
K = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]], dtype=float)
return K
def rt_to_proj(R: np.ndarray, t: np.ndarray, K: np.ndarray):
"""Return 3x4 projection matrix P = K [R | t]"""
Rt = np.hstack((R, t.reshape(3,1)))
return K @ Rt
def linear_triangulation(proj_mats: List[np.ndarray], pixels: List[np.ndarray]):
"""
Linear triangulation (DLT) for N views.
proj_mats: list of 3x4 projection matrices
pixels: list of [u, v] pixel coordinates
Returns 3D point in homogeneous coordinates (x, y, z)
"""
A = []
for P, (u, v) in zip(proj_mats, pixels):
# rows: u * P[2,:] - P[0,:]; v * P[2,:] - P[1,:]
A.append(u * P[2,:] - P[0,:])
A.append(v * P[2,:] - P[1,:])
A = np.vstack(A)
# Solve A X = 0 via SVD
_, _, Vt = np.linalg.svd(A)
X = Vt[-1]
X = X / X[3]
return X[:3]
def reprojection_error_point(point3d, proj_mats, pixels):
errs = []
Xh = np.hstack((point3d, 1.0))
for P, (u, v) in zip(proj_mats, pixels):
proj = P @ Xh
proj = proj / proj[2]
errs.append(proj[0] - u)
errs.append(proj[1] - v)
return np.array(errs)
# -------------------------
# API endpoints
# -------------------------
@app.post("/register_camera")
def register_camera(spec: CameraSpec):
"""
Register a camera with intrinsics and optional extrinsics.
rotation: 9 floats (row-major) for 3x3 R
translation: 3 floats for t (meters)
"""
cam_id = spec.camera_id
fx, fy, cx, cy = spec.intrinsics
K = intrinsics_matrix(fx, fy, cx, cy)
if spec.rotation is not None and spec.translation is not None:
R = np.array(spec.rotation, dtype=float).reshape(3,3)
t = np.array(spec.translation, dtype=float)
else:
R = np.eye(3)
t = np.zeros(3)
CAMERAS[cam_id] = {"K": K, "R": R, "t": t}
return {"camera_id": cam_id, "status": "registered"}
@app.post("/observe")
def observe(obs: Observation):
"""
Ingest an observation (pixel or bbox) from a camera frame.
Returns an observation_id.
"""
if obs.camera_id not in CAMERAS:
raise HTTPException(status_code=400, detail="camera not registered")
obs_id = str(uuid.uuid4())
OBSERVATIONS[obs_id] = obs.dict()
OBSERVATIONS[obs_id]["obs_id"] = obs_id
return {"obs_id": obs_id}
@app.post("/triangulate")
def triangulate(observation_ids: List[str]):
"""
Triangulate a landmark from multiple observations.
Requires at least two observations with pixel coordinates.
Returns landmark_id, local_position (x,y,z), and reprojection error.
"""
if len(observation_ids) < 2:
raise HTTPException(status_code=400, detail="need at least two observations")
proj_mats = []
pixels = []
for oid in observation_ids:
if oid not in OBSERVATIONS:
raise HTTPException(status_code=404, detail=f"observation {oid} not found")
o = OBSERVATIONS[oid]
cam = CAMERAS.get(o["camera_id"])
if cam is None:
raise HTTPException(status_code=400, detail=f"camera {o['camera_id']} not registered")
if o.get("pixel") is None:
# fallback: use bbox center if pixel not provided
if o.get("bbox") is None:
raise HTTPException(status_code=400, detail=f"observation {oid} has no pixel or bbox")
x1,y1,x2,y2 = o["bbox"]
u = (x1 + x2) / 2.0
v = (y1 + y2) / 2.0
else:
u, v = o["pixel"]
K = cam["K"]
R = cam["R"]
t = cam["t"]
P = rt_to_proj(R, t, K)
proj_mats.append(P)
pixels.append((u, v))
# Linear triangulation
point3d = linear_triangulation(proj_mats, pixels)
# Optionally refine with small BA if scipy available
if least_squares is not None:
def fun(x):
return reprojection_error_point(x, proj_mats, pixels)
res = least_squares(fun, point3d, verbose=0)
point3d = res.x
reproj_err = float(np.linalg.norm(res.fun) / math.sqrt(len(res.fun)/2))
else:
reproj_err = float(np.linalg.norm(reprojection_error_point(point3d, proj_mats, pixels)))
landmark_id = str(uuid.uuid4())
LANDMARKS[landmark_id] = {
"landmark_id": landmark_id,
"position": point3d.tolist(),
"observations": observation_ids,
"reprojection_error": reproj_err
}
return {"landmark_id": landmark_id, "position": point3d.tolist(), "reprojection_error": reproj_err}
@app.post("/solve_pnp")
def solve_pnp(payload: Dict[str, Any]):
"""
Solve PnP for a known 3D landmark and pixel correspondences.
payload: { camera_id, object_points: [[x,y,z],...], image_points: [[u,v],...], use_extrinsic_guess: bool }
"""
if cv2 is None:
raise HTTPException(status_code=500, detail="OpenCV not available for solvePnP")
cam_id = payload.get("camera_id")
cam = CAMERAS.get(cam_id)
if cam is None:
raise HTTPException(status_code=400, detail="camera not registered")
obj_pts = np.array(payload.get("object_points", []), dtype=float)
img_pts = np.array(payload.get("image_points", []), dtype=float)
if obj_pts.shape[0] < 4:
# PnP can work with 4+ points for robust solve
pass
K = cam["K"]
dist = np.zeros(5)
success, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, dist, flags=cv2.SOLVEPNP_ITERATIVE)
if not success:
raise HTTPException(status_code=500, detail="solvePnP failed")
R, _ = cv2.Rodrigues(rvec)
t = tvec.flatten()
return {"rotation": R.tolist(), "translation": t.tolist()}
@app.post("/bundle_adjust")
def bundle_adjust(payload: Dict[str, Any]):
"""
Small bundle-adjustment prototype.
payload: { landmarks: {id: [x,y,z]}, poses: {cam_id: {R_flat, t}}, observations: [{landmark_id, cam_id, pixel: [u,v]}] }
Returns optimized landmarks and poses (local).
"""
if least_squares is None:
raise HTTPException(status_code=500, detail="scipy least_squares not available")
landmarks = payload.get("landmarks", {})
poses = payload.get("poses", {})
observations = payload.get("observations", [])
# Flatten variables: landmark positions only (simple prototype)
lm_ids = list(landmarks.keys())
x0 = []
for lid in lm_ids:
x0.extend(landmarks[lid])
x0 = np.array(x0, dtype=float)
# Build projection matrices from poses dict (assume intrinsics known per camera)
proj_mats = {}
for cam_id, p in poses.items():
cam = CAMERAS.get(cam_id)
if cam is None:
raise HTTPException(status_code=400, detail=f"camera {cam_id} not registered")
R = np.array(p["R_flat"], dtype=float).reshape(3,3)
t = np.array(p["t"], dtype=float)
K = cam["K"]
proj_mats[cam_id] = rt_to_proj(R, t, K)
def residuals(x):
res = []
# unpack landmarks
pts = x.reshape(-1,3)
for obs in observations:
lid = obs["landmark_id"]
cam_id = obs["cam_id"]
pixel = obs["pixel"]
idx = lm_ids.index(lid)
P = proj_mats[cam_id]
Xh = np.hstack((pts[idx], 1.0))
proj = P @ Xh
proj = proj / proj[2]
res.append(proj[0] - pixel[0])
res.append(proj[1] - pixel[1])
return np.array(res)
res = least_squares(residuals, x0, verbose=0)
optimized = res.x.reshape(-1,3)
out = {}
for i, lid in enumerate(lm_ids):
out[lid] = optimized[i].tolist()
return {"optimized_landmarks": out, "cost": float(res.cost)}
@app.post("/georeference")
def georeference(payload: Dict[str, Any]):
"""
Georeference a local landmark position using a known reference.
payload: { landmark_id, reference_local: [x,y,z], reference_latlon: [lat, lon, alt] }
Returns lat/lon/alt for the landmark.
Note: This uses a simple similarity transform (2D Procrustes on X,Y) and pyproj for conversions.
"""
if Transformer is None:
raise HTTPException(status_code=500, detail="pyproj not available for georeference")
lid = payload.get("landmark_id")
ref_local = np.array(payload.get("reference_local"), dtype=float)
ref_latlon = payload.get("reference_latlon") # [lat, lon, alt]
if lid not in LANDMARKS:
raise HTTPException(status_code=404, detail="landmark not found")
# Convert reference latlon to local meters using pyproj (WGS84 -> local Easting/Northing via azimuthal equidistant)
lat0, lon0, alt0 = ref_latlon[0], ref_latlon[1], ref_latlon[2] if len(ref_latlon) > 2 else 0.0
# Use an azimuthal equidistant projection centered at reference
proj_to_m = Transformer.from_crs("EPSG:4326", f"+proj=aeqd +lat_0={lat0} +lon_0={lon0} +units=m +datum=WGS84", always_xy=True)
# Landmark local position (our engine's local coords)
local_pos = np.array(LANDMARKS[lid]["position"], dtype=float)
# We assume ref_local corresponds to (0,0) in projected meters; compute offset between local_pos and ref_local
dx_local = local_pos[:2] - ref_local[:2]
# Convert ref_latlon to meters (should be 0,0 by construction) and add dx_local
ref_mx, ref_my = proj_to_m.transform(lon0, lat0)
# Add offset
target_mx = ref_mx + dx_local[0]
target_my = ref_my + dx_local[1]
# Convert back to latlon
proj_from_m = Transformer.from_crs(f"+proj=aeqd +lat_0={lat0} +lon_0={lon0} +units=m +datum=WGS84", "EPSG:4326", always_xy=True)
lon_t, lat_t = proj_from_m.transform(target_mx, target_my)
alt_t = float(local_pos[2]) + (ref_latlon[2] if len(ref_latlon) > 2 else 0.0)
return {"lat": lat_t, "lon": lon_t, "alt": alt_t}
@app.get("/query_landmarks")
def query_landmarks():
"""Return all landmarks (simple query)."""
return {"landmarks": list(LANDMARKS.values())}
@app.get("/explain/{landmark_id}")
def explain(landmark_id: str):
"""Return supporting observations and reprojection error for a landmark."""
lm = LANDMARKS.get(landmark_id)
if lm is None:
raise HTTPException(status_code=404, detail="landmark not found")
obs = [OBSERVATIONS[oid] for oid in lm["observations"] if oid in OBSERVATIONS]
return {"landmark": lm, "observations": obs}
# -------------------------
# Example quick-run (if executed directly)
# -------------------------
if __name__ == "__main__":
import uvicorn
print("Run with: uvicorn app:app --reload --port 8000")
uvicorn.run(app, host="0.0.0.0", port=8000)
