4. 外参矩阵：旋转矩阵与平移向量，从世界坐标到相机坐标的刚体变换
好，我们继续往下走。前面几章我们把内参、畸变聊透了，说白了就是搞清楚了光线怎么穿过镜头、怎么落在传感器上。但有个关键问题一直悬着：相机到底放在世界坐标系里的哪个位置？
你想想看，一个机械臂抓取工件，视觉系统告诉它“目标在像素坐标 (500, 300)”，这完全没用。机械臂需要的是目标在世界坐标系下的 X、Y、Z 值。从像素坐标到世界坐标，中间必须经过一道桥梁——外参矩阵。
我个人习惯把外参理解为“相机的身份证”。它记录了相机在世界坐标系里的位置和朝向。有了它，我们才能把世界坐标系里的一个点，映射到相机坐标系里，然后再用内参去投影到像素平面。
核心公式：
Pcamera = R · Pworld + t
其中 R 是旋转矩阵，t 是平移向量。这个变换就是刚体变换——物体不会变形，只是被挪了个位置、转了个方向。
4.1 刚体变换的本质：不拉伸、不扭曲、只旋转和平移
刚体变换，英文叫 Rigid Body Transformation。名字听着唬人，其实意思很简单：你把一个杯子从桌上拿到柜子里，杯子本身没变形，只是位置和朝向变了。
在三维空间里，任何刚体运动都可以分解为两部分：
- 旋转：绕 X、Y、Z 三个轴转角度
- 平移：沿 X、Y、Z 三个方向移动
为什么不能先平移再旋转？嗯，这里要注意：矩阵乘法不满足交换律。先转再挪和先挪再转，结果完全不同。我早期做手眼标定时就踩过这个坑，顺序搞反了，标定出来的结果偏差了十几毫米。
标准做法是：先旋转，后平移。也就是公式里 R 乘在 Pworld 前面，t 加在后面。
4.2 旋转矩阵 R：3×3 的正交矩阵
旋转矩阵 R 是一个 3×3 的矩阵，它有三个重要性质：
- 正交性：RT · R = I，即矩阵的逆等于它的转置
- 行列式为 +1：保证是纯旋转，不包含镜像
- 自由度为 3：虽然矩阵有 9 个元素，但受正交约束限制，实际只有 3 个独立参数
绕 X、Y、Z 轴旋转的基矩阵如下：
| 旋转轴 | 旋转矩阵（绕 θ 角） | 说明 |
|---|---|---|
| 绕 X 轴 | [[1, 0, 0], [0, cosθ, -sinθ], [0, sinθ, cosθ]] | X 坐标不变，Y、Z 旋转 |
| 绕 Y 轴 | [[cosθ, 0, sinθ], [0, 1, 0], [-sinθ, 0, cosθ]] | Y 坐标不变，X、Z 旋转 |
| 绕 Z 轴 | [[cosθ, -sinθ, 0], [sinθ, cosθ, 0], [0, 0, 1]] | Z 坐标不变，X、Y 旋转 |
实际应用中，我们很少直接写这些矩阵。OpenCV 提供了 cv2.Rodrigues() 函数，可以把旋转向量转成旋转矩阵，反过来也行。
import cv2
import numpy as np
# 假设我们有一个旋转向量（3x1）
rvec = np.array([[0.1], [0.2], [0.3]], dtype=np.float64)
# 转成旋转矩阵
R, _ = cv2.Rodrigues(rvec)
print("旋转矩阵 R:")
print(R)
我的经验：标定板上的角点坐标，我习惯用 棋盘格左上角第一个内角点 作为世界坐标系原点。这样平移向量 t 的物理意义最直观——就是相机光心相对于棋盘格原点的偏移量。
4.3 平移向量 t：3×1 的偏移量
平移向量 t 就是相机坐标系原点在世界坐标系下的坐标。说白了，就是相机光心在世界坐标系里的位置。
举个例子：如果 t = [100, 50, 200]T（单位毫米），那就意味着相机光心在世界坐标系的 X 方向偏了 100mm，Y 方向偏了 50mm，Z 方向偏了 200mm。
注意：t 的单位和世界坐标系一致。如果你标定板上的格子是 30mm，那 t 的单位就是毫米。如果你用的是 1m 的标定板，那 t 的单位就是米。千万别混用，我见过有人把毫米和米混在一起，结果算出来的距离差了 1000 倍。
4.4 外参矩阵的齐次形式
为了计算方便，我们通常把旋转和平移合并成一个 4×4 的齐次矩阵：
| R t |
| 0 1 |
这样，一个世界坐标点 [X, Y, Z, 1]T 乘以这个 4×4 矩阵，就直接得到相机坐标系下的坐标了。一步到位，不用先乘 R 再加 t。
齐次坐标的好处是：可以把多个变换串联起来。比如手眼标定中，机械臂基座到相机、相机到标定板、标定板到机械臂末端，这些变换矩阵可以连乘，最终得到机械臂基座到末端的变换关系。
4.5 外参矩阵的物理意义：从世界到相机
我们来看一个完整的变换流程：
- 世界坐标系：标定板上的一个角点，坐标 Pw = [Xw, Yw, Zw]T
- 乘以旋转矩阵 R：把世界坐标旋转到与相机坐标系同向
- 加上平移向量 t：把原点从世界坐标系原点移到相机光心
- 得到相机坐标：Pc = [Xc, Yc, Zc]T
说白了，外参矩阵就是把世界坐标系“搬”到相机坐标系。搬的过程中，物体不能变形，只能转和挪。
我曾经踩过的坑：在做双目立体视觉标定时，左右相机的 R 和 t 是相对于同一个世界坐标系的。但如果你分别标定左右相机，得到的外参是各自独立的，不能直接拿来用。必须做立体标定，计算出左右相机之间的相对旋转和平移。否则，重建出来的三维点会严重扭曲。
4.6 知识结构图：外参矩阵的核心逻辑
下面这张图帮你理清外参矩阵在整个相机标定中的位置和作用：
4.7 外参矩阵的获取方式
实际工程中，外参矩阵通常通过以下方式获得：
| 方法 | 适用场景 | 精度 | 操作复杂度 |
|---|---|---|---|
| 张正友标定法 | 单目相机标定 | 高 | 低（拍棋盘格即可） |
| 手眼标定 | 机械臂+相机系统 | 中高 | 高（需要机械臂运动） |
| PnP 求解 | 已知 3D-2D 对应点 | 中 | 低（OpenCV 直接调用） |
| 直接测量 | 固定安装、精度要求低 | 低 | 低（用尺子量） |
我个人最常用的是张正友标定法。你只需要打印一张棋盘格，拍十几张不同角度的照片，OpenCV 的 cv2.calibrateCamera() 就能同时算出内参和外参。每张照片对应一个外参矩阵，因为相机和标定板的相对位置在变。
import cv2
import numpy as np
# 假设你已经提取了所有角点
# objpoints: 世界坐标系下的角点坐标（3D）
# imgpoints: 图像坐标系下的角点坐标（2D）
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, gray.shape[::-1], None, None
)
# rvecs 和 tvecs 是列表，每张图片对应一个
# 第 i 张图片的外参
R, _ = cv2.Rodrigues(rvecs[i]) # 旋转向量 -> 旋转矩阵
t = tvecs[i] # 平移向量
print(f"第 {i} 张图片的旋转矩阵:\n{R}")
print(f"第 {i} 张图片的平移向量:\n{t}")
我的习惯：标定完成后，我会把外参矩阵保存下来。然后在实际部署时，用 PnP 求解 实时计算当前帧的外参。因为标定板一旦移动，外参就变了。PnP 只需要 4 个以上的 3D-2D 对应点，就能算出新的 R 和 t，非常灵活。
4.8 避坑指南：外参矩阵的常见问题
- 旋转矩阵不正交：数值计算误差可能导致 R 不满足正交性。可以用 SVD 分解强制正交化：
U, _, Vt = np.linalg.svd(R); R_orth = U @ Vt - 平移向量单位不一致：标定板格子尺寸用毫米，t 就是毫米；用米，t 就是米。混用会导致灾难性后果。
- 坐标系定义混乱：OpenCV 的相机坐标系是 X 向右、Y 向下、Z 向前（沿光轴方向）。如果你用其他库，一定要确认坐标系定义是否一致。
- 外参随标定板移动而改变：每次重新放置标定板，外参都会变。这是正常的，因为世界坐标系是固定在标定板上的。
我曾经踩过的坑：有一次做机器人抓取项目，标定结果总是偏 2-3mm。查了两天，最后发现是旋转矩阵的 欧拉角顺序 搞错了。OpenCV 默认的旋转顺序是 Z-Y-X，而我用的机器人库默认是 X-Y-Z。同样的角度，顺序不同，结果天差地别。从那以后，我每次做手眼标定都会先打印出旋转矩阵看一眼，确认数值合理再往下走。
4.9 小结
外参矩阵是连接世界坐标系和相机坐标系的桥梁。它由旋转矩阵 R 和平移向量 t 组成，本质是一个刚体变换。记住三点：
- 先旋转，后平移，顺序不能乱
- R 是正交矩阵，行列式为 +1
- t 的单位和世界坐标系一致
掌握了外参，你就能把任意一个世界坐标点映射到相机坐标系下。再结合内参，就能算出它在图像上的像素位置。反过来，有了像素坐标和深度信息，也能反推出世界坐标。这就是视觉定位和测量的基础。