dev01
fd3d6b7ab5
- Integrate PnP rectangle pose estimation + laser world coordinate computation from scripts/calibrate/laser_pnp.py - Publish normalized measurement (laser/RECT) and reference (rect center) via Float32MultiArray - Update gimbal 0xA5 frame to 18-byte format: measure_x, ref_x, measure_y, ref_y - Add camera calibration, laser line params, and RECT_L/W constants - Add corner ordering (TL/BL/BR/TR), SOLVEPNP_IPPE, and pose/laser visualization overlay - Remove standalone vision_example.py (superseded by ROS2 nodes) - Add scripts/calibrate/ with calibration and PnP tools
110 lines
3.3 KiB
Markdown
110 lines
3.3 KiB
Markdown
# 相机标定与激光点解算
|
||
|
||
## 文件说明
|
||
|
||
| 文件 | 用途 |
|
||
|------|------|
|
||
| `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 前（指向场景）。
|