# 手眼标定（Eye-in-Hand / Eye-to-Hand）操作指南

本指南配合脚本 `src/scripts/hand_eye_cali.py` 使用，完成相机与机器人之间的外参（手眼）标定。脚本基于 OpenCV 的 `calibrateHandEye` 实现，支持“眼在手上 (eye-in-hand)”与“眼在手外 (eye-to-hand)”两种模式。

---

## 一、原理与坐标定义

- 机器人末端在基座坐标系下的位姿：`T_b^g`（4x4 齐次矩阵）
- 标定板在相机坐标系下的位姿：`T_c^t`（4x4 齐次矩阵），可由 `solvePnP` 得到
- 需求目标：
  - 眼在手上 (eye-in-hand)：求 `T_c^g`（相机在末端坐标系下的位姿）
  - 眼在手外 (eye-to-hand)：求 `T_b^c`（相机在基座坐标系下的位姿）

OpenCV `calibrateHandEye` 的输入为每一帧的“绝对位姿”：
- `R_gripper2base, t_gripper2base` 对应 `T_g^b`（由 `T_b^g` 求逆获得）
- `R_target2cam,  t_target2cam` 对应 `T_t^c`（`T_c^t` 的旋转和平移部分）

---

## 二、前置条件

- 已有相机的内参（fx, fy, cx, cy, 畸变等），并能在图像中稳定检测到标定板角点或 AprilTag/ArUco 标记
- 能够从机器人系统读取末端在基座下的位姿 `T_b^g`
- 至少采集 8–12 组数据，包含多轴、较大幅度旋转和平移（信息量不足会导致求解失败或误差大）

---

## 三、数据采集

1. 固定标定板（推荐刚性固定在稳定位置），或在“眼在手上”场景中保持相机跟随末端运动
2. 对每一帧：
   - 记录机器人末端位姿 `T_b^g`（4x4）
   - 在对应图像中用 `solvePnP` 算法求解 `T_c^t`（4x4）
3. 采集方式一：直接保存数据集 `.npz`
   - 数组名必须是 `robot_poses` 和 `camera_poses`
   - 形状为 `(N, 4, 4)`

数据格式示例（Python 生成）：
```python
np.savez('dataset.npz',
         robot_poses=np.stack(robot_pose_list, axis=0),
         camera_poses=np.stack(camera_pose_list, axis=0))
```

4. 采集方式二：仅保存图像与机器人位姿
   - 保存 N 张图像（彩色或红外），用于检测棋盘/Charuco 角点
   - 同时保存 N 条机器人末端位姿 `T_b^g`（或 `T_ab^g`）到 `.csv/.json/.npz`
   - 运行时脚本会：
     1) 从相机内参文件（YAML/JSON）读入 `K,D`
     2) 对每张图像用 `solvePnP` 求解 `T_c^t`
     3) 组合成数据集并求解手眼外参

> 仅有机械臂基座（arm_base）的位姿可以吗？可以。
> - 如果记录的是 `T_ab^g`（arm_base->末端），也能完成手眼标定。
> - `eye_to_hand` 模式下，脚本将输出 `arm_base^T_camera`。若 `T_rb^ab`（robot_base->arm_base），可传给脚本自动合成 `robot_base^T_camera`。


## 四、脚本使用方法

脚本路径：`src/scripts/hand_eye_cali.py`

- 计算两种模式并导出 JSON：
```bash
python src/scripts/hand_eye_cali.py \
  --data dataset.npz \
  --out handeye_result.json \
  --eval
```

- 仅计算眼在手外 (eye-to-hand)：
```bash
python src/scripts/hand_eye_cali.py \
  --data dataset.npz \
  --mode eye_to_hand \
  --out handeye_result.json
```

- 没有数据时，可生成模拟数据用于测试：
```bash
python src/scripts/hand_eye_cali.py \
  --num-poses 12 --eval \
  --out /tmp/handeye_result.json \
  --export-dataset /tmp/handeye_dataset.npz
```

### 仅用图像 + 末端位姿进行标定

前提：准备好相机标定文件（YAML/JSON，包含 `cameraMatrix`/`camera_matrix`/`K` 和 `distCoeffs`/`dist_coeffs`/`D`）。

- 棋盘格（默认 6x9 内角点，格长 0.024 m）：
```bash
python src/scripts/hand_eye_cali.py \
  --images data/images/*.png \
  --camera-calib data/camera.yaml \
  --robot-poses-file data/robot_poses.csv \
  --pattern chessboard --board-rows 6 --board-cols 9 --square-size 0.024 \
  --robot-poses-frame arm_base \
  --mode eye_to_hand \
  --out handeye_result.json --eval
```

- Charuco（需要 opencv-contrib-python）：
```bash
python src/scripts/hand_eye_cali.py \
  --images data/images/*.png \
  --camera-calib data/camera.yaml \
  --robot-poses-file data/robot_poses.csv \
  --pattern charuco --board-rows 6 --board-cols 9 --square-size 0.024 --charuco-marker 0.016 \
  --robot-poses-frame robot_base \
  --mode eye_to_hand \
  --out handeye_result.json --eval
```

robot_poses.csv 示例（头部为列名，按行对齐图像顺序）：
```csv
image,tx,ty,tz,qx,qy,qz,qw
0001.png,0.10,0.00,0.45,0.00,0.00,0.00,1.00
0002.png,0.10,0.05,0.45,0.05,0.00,0.00,0.9987
...
```

相机 YAML 示例（可兼容 OpenCV 格式）：
```yaml
camera_matrix: {data: [fx, 0, cx, 0, fy, cy, 0, 0, 1]}
dist_coeffs: {data: [k1, k2, p1, p2, k3]}
```

### 参数说明
- `--data`: 读取 `.npz` 数据集（包含 `robot_poses`、`camera_poses`）
- `--mode`: `eye_in_hand` 或 `eye_to_hand`（缺省则两者都算）
- `--out`: 输出 JSON 路径（包含 4x4 矩阵、旋转矩阵、平移、四元数）
- `--eval`: 计算 AX≈XB 的简单残差（眼在手上），用于快速自检
- `--export-dataset`: 将当前使用的数据集导出（便于复现）
- `--num-poses`, `--seed`: 生成模拟数据的数量与种子（无 `--data` 时生效）
- `--robot-poses-frame`: `robot_base`（默认）或 `arm_base`，用于指明 `robot_poses` 的基座框架
- `--arm-to-robot-base`: `T_robot_base^arm_base` 的 4x4 矩阵文件路径（.npy/.npz/.json），若提供则在内部进行 `T_b^g = T_rb^ab @ T_ab^g` 组合
-. `--images`: 图像目录或通配（按文件名排序）
-. `--camera-calib`: 相机内参文件（YAML/JSON）
-. `--pattern`: `chessboard` 或 `charuco`
-. `--board-rows/--board-cols/--square-size`: 标定板参数（单位：米）
-. `--charuco-marker`: Charuco 的方块内 marker 尺寸（米）
-. `--robot-poses-file`: 末端位姿文件（.csv/.json/.npz），与图像一一对应

---

## 五、结果解释与落地

- 脚本输出 JSON 中字段：
  - `matrix`: 4x4 齐次矩阵（行主序）
  - `rotation_matrix`: 3x3 旋转矩阵
  - `translation`: 3 维平移 `[x, y, z]`（单位：米）
  - `quaternion_xyzw`: 四元数 `[x, y, z, w]`

- 眼在手外结果：
  - 若 `--robot-poses-frame robot_base` 或提供了 `--arm-to-robot-base`，则输出为 `robot_base^T_camera`
  - 若 `--robot-poses-frame arm_base` 且未提供 arm->robot 变换，则输出为 `arm_base^T_camera`
  - 发布 TF 示例：
```bash
ros2 run tf2_ros static_transform_publisher \
  x y z qx qy qz qw \
  base_link camera_link
```
将 `x y z qx qy qz qw` 替换为 JSON 中的 `translation` 与 `quaternion_xyzw`，帧名根据机器人而定（如 `base_link` 与 `camera_link`）。

- 眼在手上（`camera^T_end_effector`）则在末端坐标系下描述相机位姿，常用于抓取/视觉伺服求解。

---

## 六、常见问题与排查

- 报错“Not enough informative motions”或残差很大：
  - 增加数据数量（>=12）
  - 扩大旋转幅度，覆盖多个轴，避免纯平移或单轴小角度
  - 确认 `T_b^g` 与 `T_c^t` 的定义方向正确（基座->末端、标定板->相机）
- `solvePnP` 不稳定/跳变：
  - 使用更鲁棒的标定板（AprilTag/Charuco）
  - 固定/稳定曝光，提升角点检测质量
  - 确认相机内参/畸变准确
- 结果帧名/方向不符合期望：
  - 仔细对照：脚本的 `eye_to_hand` 输出的是 `base^T_camera`，`eye_in_hand` 输出的是 `camera^T_end_effector`
  - 若需要 `end_effector^T_camera`，取逆即可

---

## 七、建议采集策略

- 首先让末端在 3 个轴上分别做正/反方向旋转，各配合一定平移
- 保证每一帧采集时 `T_b^g` 与 `T_c^t` 时间匹配（尽量同步）
- 目标板尽可能占据成像区域较大比例，避免深远距离下的姿态估计不稳定

---

## 八、附录：数据制作参考

使用 `solvePnP` 生成 `T_c^t`：
```python
# objectPoints: (N,3) mm 或 m；imagePoints: (N,2) 像素
retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)
T_c_t = create_homogeneous_matrix(rvec, tvec)
```

保存数据集：
```python
np.savez('dataset.npz',
         robot_poses=np.stack(robot_pose_list, axis=0),
         camera_poses=np.stack(camera_pose_list, axis=0))
```

若 `robot_poses` 是以机械臂基座 `arm_base` 为基准，且已知 `T_robot_base^arm_base`，可以在运行时提供：
```bash
python src/scripts/hand_eye_cali.py \
  --data dataset.npz \
  --robot-poses-frame arm_base \
  --arm-to-robot-base T_robot_base_from_arm_base.npy \
  --mode eye_to_hand \
  --out handeye_result.json
```

---
## 九、数据处理脚本

提供一个模板脚本，读取机器人驱动与视觉检测的实时话题，自动同步采样并生成 dataset.npz
将手眼标定结果直接写为 ROS2 的 static_transform_publisher 配置或 URDF 片段，便于一键加载