# 相机标定与激光点解算

## 文件说明

| 文件 | 用途 |
|------|------|
| `calibrate.py` | 相机内参标定，交互式拍照，输出 `calibration_result.json` |
| `laser_calibrate.py` | 激光器-相机联合标定，拟合相机坐标系下的激光直线，输出 `laser_result.json` |
| `pnp_solve.py` | 矩形检测 + PnP 位姿解算，实时显示相机位姿 |
| `laser_pnp.py` | 矩形检测 + PnP 位姿解算 + 激光点世界坐标计算 |

## 棋盘格规格

- 方格边长：**15 mm**
- 内角点数：**11 列 x 8 行**（12x9 个方格）

---

## 工作流程

### 第一步：相机内参标定

```bash
python calibrate.py
```

1. 输入是否为无畸变镜头（y/n），然后输入拍摄数量（建议 15~20 张）
2. 将棋盘格以不同角度/位置放在相机视野中
3. 画面显示绿色 **CHESSBOARD DETECTED** 后，输入 `y` 并回车拍照
4. 图像自动保存到 `calibration_images/` 目录
5. 拍完后自动标定，输出内参矩阵 K 和畸变系数
6. 结果保存为 `calibration_result.json`，同时终端打印完整 JSON

### 第二步（可选）：激光器-相机联合标定

> 需要开启激光，激光点投射在标定板上

```bash
python laser_calibrate.py
```

1. 自动加载 `calibration_result.json`
2. 输入拍摄数量（建议 >= 10 张），标定板以不同姿态摆放
3. 画面显示绿色棋盘格角点 + 红色激光点圆圈后，输入 `y` 拍照
4. 每张图像：PnP 解算标定板位姿 -> 提取激光点像素 -> 反投影求交 -> 得到相机坐标系 3D 点
5. 图像自动保存到 `laser_calib_images/` 目录
6. 采集完成后 PCA 拟合激光直线，输出：
   - `d_cam`：激光方向向量（相机坐标系）
   - `O_cam`：基准起点（直线与相机 Z=0 平面的交点）
7. 结果保存为 `laser_result.json`，同时终端打印完整 JSON

### 第三步：在线运行

**仅 PnP 位姿解算：**

```bash
python pnp_solve.py
```

将 `calibrate.py` 输出的 JSON 粘贴到文件顶部的 `CALIB_JSON` 变量中，修改 `RECT_L`/`RECT_W` 为实际矩形尺寸。

**PnP 位姿 + 激光点世界坐标：**

```bash
python laser_pnp.py
```

将 `calibrate.py` 输出的 JSON 粘贴到 `CALIB_JSON`，将 `laser_calibrate.py` 输出的 JSON 粘贴到 `LASER_JSON`，修改 `RECT_L`/`RECT_W`。

画面实时显示：
- 矩形四个角点标注（TL/BL/BR/TR）
- 右手坐标系坐标轴（X 红 / Y 绿 / Z 蓝）
- 相机平移向量 T 和旋转欧拉角 R
- 激光点世界坐标 (x, y)（仅在 `laser_pnp.py` 中）

---

## 内参矩阵缩放

内参矩阵 K 的单位是像素，与图像分辨率绑定。畸变系数（`dist`）是无量纲的，不随分辨率变化。

等比例缩放因子 $s = W_{\text{new}} / W_{\text{orig}}$：

```python
K_scaled = K.copy()
K_scaled[0, 0] *= s    # fx
K_scaled[1, 1] *= s    # fy
K_scaled[0, 2] *= s    # cx
K_scaled[1, 2] *= s    # cy
```

对应的 `pnp_solve.py` 和 `laser_pnp.py` 通过 `IMAGE_SCALE` 常量自动处理缩放。

---

## 坐标系约定

**世界坐标系**（矩形平面，Z=0）：
```
TL=(0,0) ──── TR=(l,0)
  │              │
BL=(0,w) ──── BR=(l,w)
```

- X 轴沿矩形长边，向右
- Y 轴沿矩形宽边，向下
- Z 轴由右手定则确定：X x Y = Z，指向观察者

**相机坐标系**：OpenCV 标准右手系，X 右、Y 下、Z 前（指向场景）。