dory-processor-sdk 0.0.1__py3-none-any.whl

dory/geo/geolocalizer.py

@@ -0,0 +1,1034 @@
+"""
+Geolocation estimation from a single fixed camera.
+
+Maps detected object positions (pixel coordinates) to geographic
+coordinates (lat/lng) using one of two strategies:
+
+**Homography** (``GeoMethod.HOMOGRAPHY``):
+    Computes a 3×3 planar perspective transform (DLT) from reference
+    correspondences.  Best when the ground is truly flat and a full
+    perspective mapping is needed.
+
+**Trilateration + linear interpolation** (``GeoMethod.TRILATERATION``):
+    1. Inside the calibrated region: finds the enclosing triangle of
+       reference points and uses barycentric (linear) interpolation.
+    2. Outside the calibrated region: estimates pixel-to-metre scale
+       from reference pairs and solves linearised trilateration.
+    Better when reference points are scattered and the ground may not
+    be perfectly flat.
+
+Both methods:
+    1. Convert reference geo-coordinates to a local Cartesian frame (metres).
+    2. Perform the computation in metre-space.
+    3. Convert results back to lat/lng.
+
+Pure-Python implementation — no numpy/scipy dependency.
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from enum import Enum
+from typing import Sequence
+
+# ---------------------------------------------------------------------------
+# Constants
+# ---------------------------------------------------------------------------
+
+# Metres per degree of latitude (WGS-84 mean)
+_METRES_PER_DEG_LAT = 111_319.5
+
+
+# ---------------------------------------------------------------------------
+# Enums
+# ---------------------------------------------------------------------------
+
+
+class GeoMethod(Enum):
+    """Strategy for converting pixel coordinates to geographic coordinates.
+
+    Attributes:
+        HOMOGRAPHY: Planar perspective transform via DLT.  Requires ≥ 4
+            non-collinear reference points.  Best for flat ground planes.
+        TRILATERATION: Trilateration (outside) + barycentric linear
+            interpolation (inside the reference mesh).  Works well with
+            scattered reference points on uneven surfaces.
+    """
+
+    HOMOGRAPHY = "homography"
+    TRILATERATION = "trilateration"
+
+
+# ---------------------------------------------------------------------------
+# Data classes
+# ---------------------------------------------------------------------------
+
+
+@dataclass(frozen=True, slots=True)
+class GeoPoint:
+    """Geographic coordinate."""
+
+    lat: float
+    lng: float
+
+
+@dataclass(frozen=True, slots=True)
+class ReferencePoint:
+    """A calibration point linking a pixel position to a geographic location.
+
+    Attributes:
+        image_x: Horizontal pixel coordinate in the camera frame.
+        image_y: Vertical pixel coordinate in the camera frame.
+        lat: Latitude in decimal degrees.
+        lng: Longitude in decimal degrees.
+    """
+
+    image_x: float
+    image_y: float
+    lat: float
+    lng: float
+
+
+@dataclass(frozen=True, slots=True)
+class BoundingBox:
+    """Axis-aligned bounding box from an object detector.
+
+    Attributes:
+        left: X-coordinate of the left edge (pixels).
+        top: Y-coordinate of the top edge (pixels).
+        width: Box width (pixels, must be >= 0).
+        height: Box height (pixels, must be >= 0).
+    """
+
+    left: float
+    top: float
+    width: float
+    height: float
+
+    def __post_init__(self) -> None:
+        if self.width < 0:
+            raise ValueError(f"width must be >= 0, got {self.width}")
+        if self.height < 0:
+            raise ValueError(f"height must be >= 0, got {self.height}")
+
+    @property
+    def ground_point(self) -> tuple[float, float]:
+        """Bottom-centre of the box — assumed ground-contact point.
+
+        Returns:
+            (x, y) pixel coordinates.
+        """
+        return (self.left + self.width / 2.0, self.top + self.height)
+
+
+# ---------------------------------------------------------------------------
+# Coordinate conversion helpers
+# ---------------------------------------------------------------------------
+
+
+def _geo_to_local(
+    lat: float,
+    lng: float,
+    origin_lat: float,
+    origin_lng: float,
+    cos_origin: float,
+) -> tuple[float, float]:
+    """Convert lat/lng to local Cartesian metres relative to *origin*.
+
+    Uses an equirectangular projection which is accurate for the small
+    areas covered by a single camera's field of view (typically < 1 km).
+
+    Args:
+        lat: Point latitude.
+        lng: Point longitude.
+        origin_lat: Origin latitude (used only for documentation here).
+        origin_lng: Origin longitude.
+        cos_origin: ``cos(radians(origin_lat))`` — precomputed.
+
+    Returns:
+        (x_metres, y_metres) east/north offsets from origin.
+    """
+    x = (lng - origin_lng) * cos_origin * _METRES_PER_DEG_LAT
+    y = (lat - origin_lat) * _METRES_PER_DEG_LAT
+    return (x, y)
+
+
+def _local_to_geo(
+    x: float,
+    y: float,
+    origin_lat: float,
+    origin_lng: float,
+    cos_origin: float,
+) -> GeoPoint:
+    """Convert local Cartesian metres back to lat/lng.
+
+    Inverse of :func:`_geo_to_local`.
+    """
+    lat = origin_lat + y / _METRES_PER_DEG_LAT
+    lng = origin_lng + x / (cos_origin * _METRES_PER_DEG_LAT)
+    return GeoPoint(lat=lat, lng=lng)
+
+
+# ---------------------------------------------------------------------------
+# Linear-algebra helpers (pure Python, no numpy)
+# ---------------------------------------------------------------------------
+
+# A "matrix" is a list of rows, each row a list of floats.
+_Matrix = list[list[float]]
+_Vector = list[float]
+
+
+def _mat_mul(a: _Matrix, b: _Matrix) -> _Matrix:
+    """Multiply two matrices."""
+    rows_a, cols_a = len(a), len(a[0])
+    cols_b = len(b[0])
+    result: _Matrix = [[0.0] * cols_b for _ in range(rows_a)]
+    for i in range(rows_a):
+        for k in range(cols_a):
+            a_ik = a[i][k]
+            if a_ik == 0.0:
+                continue
+            for j in range(cols_b):
+                result[i][j] += a_ik * b[k][j]
+    return result
+
+
+def _transpose(m: _Matrix) -> _Matrix:
+    """Transpose a matrix."""
+    rows, cols = len(m), len(m[0])
+    return [[m[i][j] for i in range(rows)] for j in range(cols)]
+
+
+def _solve_linear(a: _Matrix, b: _Vector) -> _Vector:
+    """Solve Ax = b via Gaussian elimination with partial pivoting.
+
+    Args:
+        a: n×n coefficient matrix (modified in place).
+        b: Right-hand side vector of length n (modified in place).
+
+    Returns:
+        Solution vector x of length n.
+
+    Raises:
+        ValueError: If the system is singular or near-singular.
+    """
+    n = len(b)
+
+    # Forward elimination with partial pivoting
+    for col in range(n):
+        # Find pivot row
+        max_val = abs(a[col][col])
+        max_row = col
+        for row in range(col + 1, n):
+            val = abs(a[row][col])
+            if val > max_val:
+                max_val = val
+                max_row = row
+
+        if max_val < 1e-12:
+            raise ValueError(
+                "Singular or near-singular matrix encountered. "
+                "Check that reference points are not collinear."
+            )
+
+        # Swap rows
+        if max_row != col:
+            a[col], a[max_row] = a[max_row], a[col]
+            b[col], b[max_row] = b[max_row], b[col]
+
+        # Eliminate below
+        pivot = a[col][col]
+        for row in range(col + 1, n):
+            factor = a[row][col] / pivot
+            for j in range(col, n):
+                a[row][j] -= factor * a[col][j]
+            b[row] -= factor * b[col]
+
+    # Back substitution
+    x = [0.0] * n
+    for i in range(n - 1, -1, -1):
+        s = b[i]
+        for j in range(i + 1, n):
+            s -= a[i][j] * x[j]
+        x[i] = s / a[i][i]
+
+    return x
+
+
+def _solve_least_squares(a: _Matrix, b: _Vector) -> _Vector:
+    """Solve overdetermined Ax = b via normal equations (A^T A) x = A^T b.
+
+    Args:
+        a: m×n matrix (m >= n).
+        b: Right-hand side vector of length m.
+
+    Returns:
+        Least-squares solution vector of length n.
+    """
+    at = _transpose(a)
+    # A^T b
+    atb_col = _mat_mul(at, [[v] for v in b])
+    atb: _Vector = [atb_col[i][0] for i in range(len(at))]
+    # A^T A
+    ata = _mat_mul(at, a)
+    return _solve_linear(ata, atb)
+
+
+# ---------------------------------------------------------------------------
+# Hartley normalisation
+# ---------------------------------------------------------------------------
+
+
+def _normalise_points(
+    pts: list[tuple[float, float]],
+) -> tuple[list[tuple[float, float]], _Matrix]:
+    """Apply Hartley normalisation: translate centroid to origin, scale so
+    that the mean distance from origin is sqrt(2).
+
+    This dramatically improves numerical conditioning of the DLT system.
+
+    Args:
+        pts: List of (x, y) points.
+
+    Returns:
+        (normalised_points, 3×3 normalisation matrix T).
+    """
+    n = len(pts)
+    cx = sum(p[0] for p in pts) / n
+    cy = sum(p[1] for p in pts) / n
+
+    mean_dist = sum(math.hypot(p[0] - cx, p[1] - cy) for p in pts) / n
+
+    if mean_dist < 1e-12:
+        # All points coincide — return identity transform
+        t: _Matrix = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
+        return pts, t
+
+    scale = math.sqrt(2.0) / mean_dist
+
+    normed = [(scale * (p[0] - cx), scale * (p[1] - cy)) for p in pts]
+
+    t = [
+        [scale, 0.0, -scale * cx],
+        [0.0, scale, -scale * cy],
+        [0.0, 0.0, 1.0],
+    ]
+    return normed, t
+
+
+def _invert_3x3(m: _Matrix) -> _Matrix:
+    """Invert a 3×3 matrix analytically."""
+    a, b, c = m[0]
+    d, e, f = m[1]
+    g, h, i = m[2]
+
+    det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g)
+    if abs(det) < 1e-15:
+        raise ValueError("3x3 matrix is singular, cannot invert.")
+
+    inv_det = 1.0 / det
+    return [
+        [
+            (e * i - f * h) * inv_det,
+            (c * h - b * i) * inv_det,
+            (b * f - c * e) * inv_det,
+        ],
+        [
+            (f * g - d * i) * inv_det,
+            (a * i - c * g) * inv_det,
+            (c * d - a * f) * inv_det,
+        ],
+        [
+            (d * h - e * g) * inv_det,
+            (b * g - a * h) * inv_det,
+            (a * e - b * d) * inv_det,
+        ],
+    ]
+
+
+# ---------------------------------------------------------------------------
+# Homography computation (DLT)
+# ---------------------------------------------------------------------------
+
+
+def _compute_homography(
+    src: list[tuple[float, float]],
+    dst: list[tuple[float, float]],
+) -> _Matrix:
+    """Compute the 3×3 homography mapping *src* → *dst* using normalised DLT.
+
+    Each correspondence ``(x, y) → (X, Y)`` satisfies::
+
+        [X']   [h11 h12 h13] [x]
+        [Y'] = [h21 h22 h23] [y]
+        [W']   [h31 h32 h33] [1]
+
+    where ``X = X'/W'`` and ``Y = Y'/W'``.
+
+    We fix ``h33 = 1`` (valid when the origin is not mapped to infinity)
+    and solve the resulting 2n × 8 system via least squares (normal
+    equations) when n > 4, or direct solve when n = 4.
+
+    Args:
+        src: Image points (at least 4).
+        dst: World points (same length as *src*).
+
+    Returns:
+        3×3 homography matrix as nested lists.
+
+    Raises:
+        ValueError: If fewer than 4 correspondences or system is singular.
+    """
+    n = len(src)
+    if n != len(dst):
+        raise ValueError(
+            f"src and dst must have the same length, got {n} and {len(dst)}."
+        )
+    if n < 4:
+        raise ValueError(f"Need >= 4 point correspondences, got {n}.")
+
+    # Step 1: Hartley normalisation
+    src_norm, t_src = _normalise_points(src)
+    dst_norm, t_dst = _normalise_points(dst)
+
+    # Step 2: Build the DLT system with h33 = 1
+    # For each correspondence (x,y) → (X,Y):
+    #   h11*x + h12*y + h13 - h31*x*X - h32*y*X = X
+    #   h21*x + h22*y + h23 - h31*x*Y - h32*y*Y = Y
+    # Unknowns: [h11, h12, h13, h21, h22, h23, h31, h32]
+
+    num_unknowns = 8
+    a_rows: _Matrix = []
+    b_vec: _Vector = []
+
+    for i in range(n):
+        x, y = src_norm[i]
+        cap_x, cap_y = dst_norm[i]
+
+        a_rows.append([x, y, 1.0, 0.0, 0.0, 0.0, -x * cap_x, -y * cap_x])
+        b_vec.append(cap_x)
+
+        a_rows.append([0.0, 0.0, 0.0, x, y, 1.0, -x * cap_y, -y * cap_y])
+        b_vec.append(cap_y)
+
+    # Step 3: Solve
+    if n == 4:
+        # Exact 8×8 system
+        h_vec = _solve_linear(a_rows, b_vec)
+    else:
+        # Overdetermined → least squares via normal equations
+        h_vec = _solve_least_squares(a_rows, b_vec)
+
+    # Reconstruct normalised 3×3 H (h33 = 1)
+    h_norm: _Matrix = [
+        [h_vec[0], h_vec[1], h_vec[2]],
+        [h_vec[3], h_vec[4], h_vec[5]],
+        [h_vec[6], h_vec[7], 1.0],
+    ]
+
+    # Step 4: Denormalise — H = T_dst^{-1} · H_norm · T_src
+    t_dst_inv = _invert_3x3(t_dst)
+    h_final = _mat_mul(t_dst_inv, _mat_mul(h_norm, t_src))
+
+    return h_final
+
+
+def _apply_homography(h: _Matrix, x: float, y: float) -> tuple[float, float]:
+    """Apply homography H to a single point (x, y).
+
+    Returns:
+        Projected (X, Y) in the destination coordinate system.
+
+    Raises:
+        ValueError: If the projected w-coordinate is near zero.
+    """
+    w = h[2][0] * x + h[2][1] * y + h[2][2]
+    if abs(w) < 1e-12:
+        raise ValueError(
+            f"Homography maps point ({x}, {y}) to infinity (w ≈ 0). "
+            "This usually means the point is outside the valid ground plane."
+        )
+    inv_w = 1.0 / w
+    out_x = (h[0][0] * x + h[0][1] * y + h[0][2]) * inv_w
+    out_y = (h[1][0] * x + h[1][1] * y + h[1][2]) * inv_w
+    return (out_x, out_y)
+
+
+# ---------------------------------------------------------------------------
+# Trilateration + linear interpolation
+# ---------------------------------------------------------------------------
+
+
+def _barycentric(
+    px: float,
+    py: float,
+    ax: float,
+    ay: float,
+    bx: float,
+    by: float,
+    cx: float,
+    cy: float,
+) -> tuple[float, float, float] | None:
+    """Compute barycentric coordinates (u, v, w) of point P in triangle ABC.
+
+    Returns:
+        (u, v, w) where ``P = u*A + v*B + w*C`` and ``u + v + w = 1``.
+        Returns None if the triangle is degenerate (area ≈ 0).
+    """
+    # Twice the signed area of triangle ABC
+    denom = (by - cy) * (ax - cx) + (cx - bx) * (ay - cy)
+    if abs(denom) < 1e-12:
+        return None
+
+    u = ((by - cy) * (px - cx) + (cx - bx) * (py - cy)) / denom
+    v = ((cy - ay) * (px - cx) + (ax - cx) * (py - cy)) / denom
+    w = 1.0 - u - v
+    return (u, v, w)
+
+
+def _find_enclosing_triangle(
+    px: float,
+    py: float,
+    image_pts: list[tuple[float, float]],
+) -> tuple[int, int, int, float, float, float] | None:
+    """Find a triangle from *image_pts* that contains (px, py).
+
+    Tries all combinations of 3 reference points and returns the first
+    triangle where all barycentric coordinates are in [0, 1].
+
+    Returns:
+        (i, j, k, u, v, w) — indices and barycentric coords.
+        None if no enclosing triangle is found (point is outside the
+        convex hull of the reference points).
+    """
+    n = len(image_pts)
+    # Try triangles formed by nearest points first (more likely to enclose)
+    dists = sorted(range(n), key=lambda i: math.hypot(
+        image_pts[i][0] - px, image_pts[i][1] - py
+    ))
+
+    for ii in range(len(dists)):
+        for jj in range(ii + 1, len(dists)):
+            for kk in range(jj + 1, len(dists)):
+                i, j, k = dists[ii], dists[jj], dists[kk]
+                bary = _barycentric(
+                    px, py,
+                    image_pts[i][0], image_pts[i][1],
+                    image_pts[j][0], image_pts[j][1],
+                    image_pts[k][0], image_pts[k][1],
+                )
+                if bary is None:
+                    continue
+                u, v, w = bary
+                # Allow a small tolerance for points on edges
+                if u >= -1e-9 and v >= -1e-9 and w >= -1e-9:
+                    return (i, j, k, u, v, w)
+    return None
+
+
+def _compute_pixel_to_metre_scale(
+    image_pts: list[tuple[float, float]],
+    world_pts: list[tuple[float, float]],
+) -> float:
+    """Compute a robust pixel-to-metre scale factor from reference pairs.
+
+    Uses the median of all pairwise ratios (real_distance / pixel_distance)
+    for robustness against outlier pairs.
+
+    Args:
+        image_pts: Pixel positions of reference points.
+        world_pts: Local-XY positions (metres) of reference points.
+
+    Returns:
+        Median scale factor (metres per pixel).
+
+    Raises:
+        ValueError: If scale cannot be computed (e.g. coincident points).
+    """
+    scales: list[float] = []
+    n = len(image_pts)
+    for i in range(n):
+        for j in range(i + 1, n):
+            pixel_dist = math.hypot(
+                image_pts[i][0] - image_pts[j][0],
+                image_pts[i][1] - image_pts[j][1],
+            )
+            world_dist = math.hypot(
+                world_pts[i][0] - world_pts[j][0],
+                world_pts[i][1] - world_pts[j][1],
+            )
+            if pixel_dist > 1e-6:
+                scales.append(world_dist / pixel_dist)
+
+    if not scales:
+        raise ValueError(
+            "Cannot compute pixel-to-metre scale: all reference points "
+            "have identical pixel positions."
+        )
+
+    # Median is robust to outlier pairs
+    scales.sort()
+    mid = len(scales) // 2
+    if len(scales) % 2 == 0:
+        return (scales[mid - 1] + scales[mid]) / 2.0
+    return scales[mid]
+
+
+def _trilaterate(
+    world_pts: list[tuple[float, float]],
+    distances: list[float],
+) -> tuple[float, float]:
+    """Estimate position from known positions and estimated distances.
+
+    Linearises the trilateration equations by subtracting the first
+    equation from all others::
+
+        |P - P_i|^2 = d_i^2    for i = 1..n
+
+    Subtracting equation 1 from equation i gives::
+
+        2(x_1 - x_i) * x + 2(y_1 - y_i) * y =
+            d_i^2 - d_1^2 - x_i^2 + x_1^2 - y_i^2 + y_1^2
+
+    This yields (n-1) linear equations for 2 unknowns, solved via
+    least squares when n > 3.
+
+    Args:
+        world_pts: Known positions (x, y) in metres — at least 3.
+        distances: Estimated distances from each position to the unknown
+            point — same length as *world_pts*.
+
+    Returns:
+        (x, y) estimated position in metres.
+
+    Raises:
+        ValueError: If the system is degenerate.
+    """
+    n = len(world_pts)
+    x1, y1 = world_pts[0]
+    d1_sq = distances[0] ** 2
+
+    # Build the (n-1) × 2 linear system Ah = b
+    a_rows: _Matrix = []
+    b_vec: _Vector = []
+    for i in range(1, n):
+        xi, yi = world_pts[i]
+        di_sq = distances[i] ** 2
+        a_rows.append([2.0 * (x1 - xi), 2.0 * (y1 - yi)])
+        b_vec.append(di_sq - d1_sq - xi * xi + x1 * x1 - yi * yi + y1 * y1)
+
+    if n - 1 == 2:
+        # Exact 2×2 system
+        return tuple(_solve_linear(a_rows, b_vec))  # type: ignore[return-value]
+
+    # Overdetermined → least squares
+    result = _solve_least_squares(a_rows, b_vec)
+    return (result[0], result[1])
+
+
+def _estimate_trilateration(
+    px: float,
+    py: float,
+    image_pts: list[tuple[float, float]],
+    world_pts: list[tuple[float, float]],
+    scale: float,
+) -> tuple[float, float]:
+    """Estimate local-XY position using trilateration.
+
+    Converts pixel distances to estimated real distances using *scale*,
+    then solves the linearised trilateration system.
+
+    Args:
+        px: Query pixel x.
+        py: Query pixel y.
+        image_pts: Reference image positions.
+        world_pts: Reference world positions (local metres).
+        scale: Metres-per-pixel scale factor.
+
+    Returns:
+        (x, y) estimated world position in metres.
+    """
+    # Estimate real-world distance from query to each reference
+    distances: list[float] = []
+    for ix, iy in image_pts:
+        pixel_dist = math.hypot(px - ix, py - iy)
+        distances.append(pixel_dist * scale)
+
+    return _trilaterate(world_pts, distances)
+
+
+# ---------------------------------------------------------------------------
+# Fallback: inverse-distance-weighted interpolation
+# ---------------------------------------------------------------------------
+
+
+def _idw_interpolate(
+    image_point: tuple[float, float],
+    refs: Sequence[ReferencePoint],
+    power: float = 2.0,
+) -> GeoPoint:
+    """Inverse-distance-weighted interpolation as a fallback.
+
+    Used when neither homography nor trilateration can be computed
+    (e.g. collinear points or degenerate geometry).
+
+    Args:
+        image_point: (x, y) pixel coordinate to geolocate.
+        refs: Reference points.
+        power: Distance weighting exponent (default 2).
+
+    Returns:
+        Interpolated GeoPoint.
+    """
+    px, py = image_point
+    weights: list[float] = []
+    for ref in refs:
+        dist = math.hypot(ref.image_x - px, ref.image_y - py)
+        if dist < 1e-9:
+            # Exact match with a reference point
+            return GeoPoint(lat=ref.lat, lng=ref.lng)
+        weights.append(1.0 / (dist ** power))
+
+    total = sum(weights)
+    lat = sum(w * r.lat for w, r in zip(weights, refs)) / total
+    lng = sum(w * r.lng for w, r in zip(weights, refs)) / total
+    return GeoPoint(lat=lat, lng=lng)
+
+
+# ---------------------------------------------------------------------------
+# Public API
+# ---------------------------------------------------------------------------
+
+
+class CameraGeolocalizer:
+    """Reusable geolocalizer for a fixed camera with known reference points.
+
+    Precomputes the mapping data once so that repeated calls to
+    :meth:`estimate` are fast.
+
+    Args:
+        reference_points: At least 4 calibration points (3 for
+            trilateration) linking pixel positions to geographic
+            coordinates.
+        method: ``GeoMethod.HOMOGRAPHY`` (default) or
+            ``GeoMethod.TRILATERATION``.
+
+    Raises:
+        ValueError: If too few reference points or invalid coordinates.
+
+    Examples::
+
+        # Homography (default)
+        geo = CameraGeolocalizer(refs)
+
+        # Trilateration + linear interpolation
+        geo = CameraGeolocalizer(refs, method=GeoMethod.TRILATERATION)
+
+        location = geo.estimate(BoundingBox(left=100, top=50, width=60, height=180))
+    """
+
+    def __init__(
+        self,
+        reference_points: Sequence[ReferencePoint],
+        method: GeoMethod = GeoMethod.HOMOGRAPHY,
+    ) -> None:
+        self._method = method
+
+        min_points = 3 if method == GeoMethod.TRILATERATION else 4
+        if len(reference_points) < min_points:
+            raise ValueError(
+                f"At least {min_points} reference points are required for "
+                f"{method.value}, got {len(reference_points)}."
+            )
+
+        self._refs = list(reference_points)
+
+        # Validate geographic coordinates
+        for i, ref in enumerate(self._refs):
+            if not (-90.0 <= ref.lat <= 90.0):
+                raise ValueError(
+                    f"Reference point {i}: lat must be in [-90, 90], got {ref.lat}."
+                )
+            if not (-180.0 <= ref.lng <= 180.0):
+                raise ValueError(
+                    f"Reference point {i}: lng must be in [-180, 180], got {ref.lng}."
+                )
+
+        # Compute the geographic origin (centroid of reference geo-coords)
+        self._origin_lat = sum(r.lat for r in self._refs) / len(self._refs)
+        self._origin_lng = sum(r.lng for r in self._refs) / len(self._refs)
+        self._cos_origin = math.cos(math.radians(self._origin_lat))
+
+        # Guard against near-pole origins where cos → 0 (longitude becomes
+        # undefined and the equirectangular projection breaks down).
+        if self._cos_origin < 1e-6:
+            raise ValueError(
+                f"Reference points are too close to a pole "
+                f"(origin latitude {self._origin_lat:.4f}°). "
+                f"Equirectangular projection is not valid at extreme latitudes."
+            )
+
+        # Convert reference geo-coords to local XY (metres)
+        image_pts: list[tuple[float, float]] = []
+        world_pts: list[tuple[float, float]] = []
+        for ref in self._refs:
+            image_pts.append((ref.image_x, ref.image_y))
+            world_pts.append(
+                _geo_to_local(
+                    ref.lat, ref.lng,
+                    self._origin_lat, self._origin_lng,
+                    self._cos_origin,
+                )
+            )
+
+        self._image_pts = image_pts
+        self._world_pts = world_pts
+
+        # Method-specific initialisation
+        self._homography: _Matrix | None = None
+        self._pixel_to_metre_scale: float = 0.0
+        self._fallback = False
+
+        if method == GeoMethod.HOMOGRAPHY:
+            self._init_homography()
+        else:
+            self._init_trilateration()
+
+    def _init_homography(self) -> None:
+        """Compute the homography matrix from reference correspondences."""
+        try:
+            self._homography = _compute_homography(
+                self._image_pts, self._world_pts
+            )
+        except ValueError:
+            self._fallback = True
+
+    def _init_trilateration(self) -> None:
+        """Precompute the pixel-to-metre scale for trilateration."""
+        try:
+            self._pixel_to_metre_scale = _compute_pixel_to_metre_scale(
+                self._image_pts, self._world_pts
+            )
+        except ValueError:
+            self._fallback = True
+
+    @property
+    def method(self) -> GeoMethod:
+        """The geolocation method used by this instance."""
+        return self._method
+
+    @property
+    def is_fallback(self) -> bool:
+        """True if the chosen method failed and IDW interpolation is used."""
+        return self._fallback
+
+    def reprojection_errors(self) -> list[float]:
+        """Compute reprojection error (metres) for each reference point.
+
+        Projects each reference point's image coordinates through the
+        chosen method and measures the Euclidean distance to its known
+        world position.  Useful for assessing calibration quality.
+
+        Returns:
+            List of errors in metres, one per reference point.
+            Empty list if using fallback (IDW) mode.
+        """
+        if self._fallback:
+            return []
+
+        errors: list[float] = []
+        for idx, ((ix, iy), (wx, wy)) in enumerate(
+            zip(self._image_pts, self._world_pts)
+        ):
+            if self._method == GeoMethod.HOMOGRAPHY:
+                if self._homography is None:
+                    return []
+                px, py = _apply_homography(self._homography, ix, iy)
+            else:
+                # For trilateration, try linear interpolation first
+                enc = _find_enclosing_triangle(ix, iy, self._image_pts)
+                if enc is not None:
+                    i, j, k, u, v, w = enc
+                    # Skip if this point IS one of the triangle vertices
+                    # (would trivially give 0 error from itself)
+                    if idx in (i, j, k):
+                        # Use leave-one-out: trilaterate without this point
+                        other_img = [p for n, p in enumerate(self._image_pts) if n != idx]
+                        other_world = [p for n, p in enumerate(self._world_pts) if n != idx]
+                        try:
+                            px, py = _estimate_trilateration(
+                                ix, iy, other_img, other_world,
+                                self._pixel_to_metre_scale,
+                            )
+                        except ValueError:
+                            px, py = wx, wy  # can't compute, assume 0 error
+                    else:
+                        px = u * self._world_pts[i][0] + v * self._world_pts[j][0] + w * self._world_pts[k][0]
+                        py = u * self._world_pts[i][1] + v * self._world_pts[j][1] + w * self._world_pts[k][1]
+                else:
+                    try:
+                        px, py = _estimate_trilateration(
+                            ix, iy, self._image_pts, self._world_pts,
+                            self._pixel_to_metre_scale,
+                        )
+                    except ValueError:
+                        px, py = wx, wy
+
+            errors.append(math.hypot(px - wx, py - wy))
+        return errors
+
+    def mean_reprojection_error(self) -> float:
+        """Mean reprojection error across all reference points (metres).
+
+        Returns:
+            Mean error in metres, or 0.0 if using fallback mode.
+        """
+        errs = self.reprojection_errors()
+        if not errs:
+            return 0.0
+        return sum(errs) / len(errs)
+
+    def estimate(self, box: BoundingBox) -> GeoPoint:
+        """Estimate the geographic location of a detected object.
+
+        Derives the ground-contact point from the bounding box
+        (bottom-centre) and projects it using the chosen method.
+
+        Args:
+            box: Detection bounding box.
+
+        Returns:
+            Estimated geographic coordinate.
+        """
+        px, py = box.ground_point
+
+        if self._fallback:
+            return _idw_interpolate((px, py), self._refs)
+
+        if self._method == GeoMethod.HOMOGRAPHY:
+            return self._estimate_homography(px, py)
+        return self._estimate_trilateration_method(px, py)
+
+    def _estimate_homography(self, px: float, py: float) -> GeoPoint:
+        """Project pixel through the homography."""
+        if self._homography is None:
+            return _idw_interpolate((px, py), self._refs)
+
+        local_x, local_y = _apply_homography(self._homography, px, py)
+        return _local_to_geo(
+            local_x, local_y,
+            self._origin_lat, self._origin_lng,
+            self._cos_origin,
+        )
+
+    def _estimate_trilateration_method(self, px: float, py: float) -> GeoPoint:
+        """Estimate location via trilateration + linear interpolation.
+
+        Strategy:
+        1. If the pixel falls inside a triangle of reference points,
+           use barycentric (linear) interpolation of their world coords.
+           This is exact at reference points and smooth between them.
+        2. If outside (extrapolation), use trilateration with estimated
+           pixel-to-metre distances.
+        """
+        # Step 1: Try linear interpolation inside a reference triangle
+        enc = _find_enclosing_triangle(px, py, self._image_pts)
+        if enc is not None:
+            i, j, k, u, v, w = enc
+            # Interpolate in local-XY space (metres), then convert to geo
+            local_x = (
+                u * self._world_pts[i][0]
+                + v * self._world_pts[j][0]
+                + w * self._world_pts[k][0]
+            )
+            local_y = (
+                u * self._world_pts[i][1]
+                + v * self._world_pts[j][1]
+                + w * self._world_pts[k][1]
+            )
+            return _local_to_geo(
+                local_x, local_y,
+                self._origin_lat, self._origin_lng,
+                self._cos_origin,
+            )
+
+        # Step 2: Outside the reference mesh — use trilateration
+        try:
+            local_x, local_y = _estimate_trilateration(
+                px, py,
+                self._image_pts, self._world_pts,
+                self._pixel_to_metre_scale,
+            )
+        except ValueError:
+            # Degenerate case — fall back to IDW
+            return _idw_interpolate((px, py), self._refs)
+
+        return _local_to_geo(
+            local_x, local_y,
+            self._origin_lat, self._origin_lng,
+            self._cos_origin,
+        )
+
+    def estimate_batch(self, boxes: Sequence[BoundingBox]) -> list[GeoPoint]:
+        """Estimate geographic locations for multiple detections.
+
+        Args:
+            boxes: Sequence of bounding boxes.
+
+        Returns:
+            List of GeoPoints in the same order as *boxes*.
+        """
+        return [self.estimate(box) for box in boxes]
+
+
+def estimate_location(
+    box: BoundingBox,
+    reference_points: Sequence[ReferencePoint],
+    method: GeoMethod = GeoMethod.HOMOGRAPHY,
+) -> GeoPoint:
+    """One-shot geolocation estimate for a single detection.
+
+    Convenience function that builds the geolocalizer and projects in one
+    call.  For repeated estimates from the same camera, prefer
+    :class:`CameraGeolocalizer` to avoid recomputing.
+
+    Args:
+        box: Detection bounding box.
+        reference_points: Calibration points (≥ 4 for homography, ≥ 3 for
+            trilateration).
+        method: ``GeoMethod.HOMOGRAPHY`` (default) or
+            ``GeoMethod.TRILATERATION``.
+
+    Returns:
+        Estimated geographic coordinate.
+
+    Raises:
+        ValueError: If too few reference points are provided.
+    """
+    geo = CameraGeolocalizer(reference_points, method=method)
+    return geo.estimate(box)
+
+
+def estimate_locations_batch(
+    boxes: Sequence[BoundingBox],
+    reference_points: Sequence[ReferencePoint],
+    method: GeoMethod = GeoMethod.HOMOGRAPHY,
+) -> list[GeoPoint]:
+    """One-shot batch geolocation for multiple detections.
+
+    Args:
+        boxes: Sequence of bounding boxes.
+        reference_points: Calibration points (≥ 4 for homography, ≥ 3 for
+            trilateration).
+        method: ``GeoMethod.HOMOGRAPHY`` (default) or
+            ``GeoMethod.TRILATERATION``.
+
+    Returns:
+        List of GeoPoints in the same order as *boxes*.
+
+    Raises:
+        ValueError: If too few reference points are provided.
+    """
+    geo = CameraGeolocalizer(reference_points, method=method)
+    return geo.estimate_batch(boxes)