# Task 1 — Tracked-camera AR overlay projection

## Goal
For every frame in `scene.json`, compute where a planar imaging probe's plane and a
tracked needle's trajectory land **in the camera's distorted pixel image**, plus
several 3D quantities. Write results to `result.json`.

## Conventions (fixed)
- A pose `T_A_to_B` is a 4×4 homogeneous matrix mapping a point in frame `A` to
  frame `B`: `p_B = T_A_to_B · p_A`. Composition: `T_A_to_C = T_B_to_C · T_A_to_B`.
- Matrices are **row-major** arrays of 4 rows of 4 doubles. Translation is in **mm**.
- The `image` frame is in **mm**; its `z = 0` plane is the imaging plane.

## Required transform chain (derive it — it is NOT pre-composed in the input)
```
T_tracker_to_cam   = T_camref_to_cam · inv(T_camref_to_tracker)
T_image_to_tracker = T_proberef_to_tracker · T_transducer_to_proberef · T_image_to_transducer
```

## Pinhole + distortion (Brown–Conrady)
Given a point in camera frame `(X, Y, Z)`:
- If `Z <= 0` the point does **not** project → output `null`.
- `x = X/Z`, `y = Y/Z`, `r² = x² + y²`
- `radial = 1 + k1·r² + k2·r⁴ + k3·r⁶`
- `x_d = x·radial + 2·p1·x·y + p2·(r² + 2x²)`
- `y_d = y·radial + p1·(r² + 2y²) + 2·p2·x·y`
- `u = fx·x_d + cx`, `v = fy·y_d + cy`

## Per-frame computations (input order)
1. **Imaging-plane equation** `(A,B,C,D)` in tracker frame. Map image-frame points
   `p0=(0,0,0)`, `p1=(1,0,0)`, `p2=(0,1,0)` through `T_image_to_tracker`; normal
   `n = (p1−p0) × (p2−p0)` (points along image +z); `D = −n·p0`. Report `n`
   **normalized** as `(A,B,C)`, and `D` computed with the normalized normal.
2. **Needle in tracker frame**: `tip = T_needleref_to_tracker · tip_needleref`;
   `dir = rotation(T_needleref_to_tracker) · axis_needleref`, renormalized to unit.
3. **signed_distance_mm** `= n̂·tip + D̂` (positive on the normal side).
4. **Ray–plane intersection**: `s = −(n̂·tip + D̂)/(n̂·dir)`, point `= tip + s·dir`.
   If `|n̂·dir| < 1e-9`, intersection is `null`.
5. **Projections** to distorted camera pixels: the needle tip, and the intersection
   point (`null` if behind the camera per the `Z<=0` rule).

## Input: `scene.json`
```
{
  "camera": { "fx","fy","cx","cy", "dist": {"k1","k2","p1","p2","k3"} },
  "static": {
    "T_camref_to_cam":        [[4x4]],
    "T_transducer_to_proberef":[[4x4]],
    "T_image_to_transducer":  [[4x4]],
    "needle": { "tip_needleref":[x,y,z], "axis_needleref":[x,y,z] }
  },
  "frames": [
    { "id":0, "T_camref_to_tracker":[[4x4]],
              "T_proberef_to_tracker":[[4x4]],
              "T_needleref_to_tracker":[[4x4]] }, ...
  ]
}
```

## Output: `result.json`
A JSON array, one object per input frame **in input order**, exactly these keys:
```
{
  "id": 0,
  "plane_tracker": {"A":..,"B":..,"C":..,"D":..},
  "tip_tracker": [x,y,z],
  "dir_tracker": [x,y,z],
  "signed_distance_mm": ..,
  "intersection_tracker": [x,y,z] | null,
  "tip_px": [u,v] | null,
  "intersection_px": [u,v] | null
}
```
Any undefined quantity must be JSON `null`. `(A,B,C)` and `dir_tracker` must be unit
length. Implement the geometry yourself with Python 3.11 + NumPy only — no OpenCV or
other vision library projection/pose calls. No network access.