# hand_eye_calib/ — Camera-to-robot calibration

This is the linchpin folder for the whole project (see
[`../../docs/system_overview.md`](../../docs/system_overview.md)). The
goal: recover `T_cam_world` (camera pose in the robot's world frame) and
`CAM_K` (intrinsics) accurately enough that 3D unprojection from PARA's
predicted `(u, v, h)` lands within a few mm of the right world point.

## Files

| File | What it does |
|---|---|
| `calibrate.py` | The reference joint hand-eye solver. Pipeline: render the Panda from a known camera in MuJoCo (with the ArUco board mounted on the hand) → detect ArUco at each pose → `cv2.calibrateCamera` for intrinsics → PnP per pose for `T_cam_board` → joint nonlinear solve for `T_cam_world` *and* `T_hand_board` simultaneously. Saves a 4-panel comparison image at the end. |
| `command_calib_poses.py` | Drives the real Panda through a list of calibration poses by publishing to `/gello/joint_states` over rosbridge. Handles initial-state sync, settling, and velocity-limited interpolation between poses. Press Enter to advance, `b` to go back, `q` to quit. |
| `panda_exo_handeye.py` | The MuJoCo + ArUco config used by `calibrate.py` and `render_test.py`. Reuses the 4×4 even-larger board definition from `panda_exo.py`. **Note:** there is also `ExoConfigs/panda_exo_handeye_4x2.py` which is a 4×2 rectangular variant used by `test_ik_recovery.py` and `data_panda_para.py`. Pick one and stay consistent. |
| `render_test.py` | Quick MuJoCo render from a fixed camera viewpoint with the Panda + board, plus an ArUco detection overlay. Useful for confirming the board size / placement is right *before* running the full calibration. |
| `viewer_test.py` | MuJoCo `viewer.launch` to interactively pose the Panda + board and see what each calibration pose actually looks like. |
| `aruco_reproject_check.png` | Reference output: ArUco corners reprojected on the calibration image. Should sit on the printed markers. |
| `mujoco_overlay_check.png` | Reference output: MuJoCo render of the calibrated camera vs the real frame. The arm silhouettes should align closely. |
| `calibrate_output.png` | Reference output: the 4-panel summary `calibrate.py` saves at the end. |

## How to actually run a calibration

End-to-end:

1. **Print** the 4×2 (or 4×4) ArUco board from
   `robot_models/board_imgs/`. Mount it on the gripper. Measure it. The
   physical edge length must match `BOARD_LENGTH_4X2` (or
   `BOARD_LENGTH_EVEN_LARGER`) in the corresponding config file. *If it
   doesn't, your `T_hand_board` solve is meaningless even when the
   numbers look small.*

2. **Pose list.** Use the `CALIB_POSES` list in `calibrate.py` as a
   starting point — these are 17 joint configurations that move the
   board across the camera FoV. Add or modify so the board is detected
   in at least 10–12 poses; sparse coverage hurts the joint solve.

3. **Drive the robot** to each pose:
   ```bash
   python command_calib_poses.py --duration 6
   ```
   Capture a RealSense frame at each one (the recording script can do
   this in one pass — wire the rosbridge sub into `simple_dataset_record_panda.py`
   if you want it automated; otherwise capture manually).

4. **Run the solver** on the captured frames:
   - For now `calibrate.py` operates on MuJoCo-rendered frames as a
     correctness check (validates the solver against GT).
   - To run on real frames: replace the renderer block with code that
     loads the captured RGB + GT joint state per pose, detects ArUco,
     stuffs into `all_obj_pts`/`all_img_pts`/`all_T_world_hand`, and runs
     the same `calibrateCamera` + `solve_hand_eye` chain.

5. **Always visualize the result.** Save:
   - `aruco_reproject_check.png` — markers reprojected on the captured
     image.
   - `mujoco_overlay_check.png` — MuJoCo render at the solved camera
     vs the captured RGB.
   - `calibrate_output.png` — 4-panel solver summary.

## Sim-only sanity check first

`calibrate.py` as it stands runs entirely in MuJoCo: it renders the
Panda + board from a known camera, runs the solver, and compares to GT.
**Run this first** on any new branch before touching real data — it
catches solver regressions immediately. Acceptable error in sim is ~mm
position and ~tenths of a degree rotation.

## Solver details

`solve_hand_eye(T_wh_list, T_cb_list)`:

- Initialize `T_cw0` via `cv2.calibrateHandEye(..., CALIB_HAND_EYE_TSAI)`,
  then `T_hb0 = inv(T_wh_0) @ inv(T_cw0) @ T_cb_0`.
- Optimize 12 params (6 for `T_cam_world`, 6 for `T_hand_board`) with
  L-BFGS-B over a position + soft rotation cost.
- 15 random restarts, take the best.

This is intentionally simple. If you find it gets stuck, the right move
is usually:

1. **Add more pose diversity** (the cost surface is non-convex; bad
   coverage = bad minima).
2. **Check your `T_world_hand`** — make sure FK matches the *actual*
   robot, not a slightly off URDF (e.g. flange offset can be ~10 mm).
3. **Verify the board edge length** physically.

## Output format

When you eventually do a real calibration, dump the result as JSON next
to `panda_streaming/data_panda_para.py` and have the dataset class load
it instead of the current hardcoded constants:

```json
{
  "T_cam_world": [[...], [...], [...], [...]],
  "K": [[fx, 0, cx], [0, fy, cy], [0, 0, 1]],
  "image_size": [1920, 1080],
  "captured_at": "2026-04-28T...",
  "n_poses_used": 14,
  "rms_reprojection_px": 0.42,
  "T_cam_world_pos_err_mm": null,
  "T_hand_board": [[...], [...], [...], [...]]
}
```

## Don'ts

- Don't pick poses that all look similar from the camera — the solver
  becomes ill-conditioned.
- Don't move the camera mid-capture. If you bump it, restart.
- Don't trust low reprojection error alone — render the MuJoCo overlay
  on a *new* held-out frame and look at it.