系列导航 ：上一篇：01 针孔模型与内参矩阵 K ｜ 目录 ｜ 下一篇：03 畸变模型：径向、切向与鱼眼
本篇目标：读完本篇，你将能够------
- 说出旋转矩阵的性质，读懂它的列和行各代表什么，知道旋转的顺序为什么不能交换；
- 推导 Rodrigues 公式，在旋转矩阵和 3 个参数的旋转向量之间来回换算，并处理 0° 和 180° 两种特殊情况；
- 解释外参 t 的含义，推出相机光心在世界坐标系里的位置 C=−R⊤t，会对位姿求逆和复合；
- 从一个投影矩阵 P 反过来分解出 K、 R、 t，说明 P 为什么有 11 个自由度；
- 从像素出发写出世界坐标系里的视线，求它与棋盘平面的交点，分清"深度"和"距离"；
- 在 OpenCV 和 OpenGL 两种相机坐标系之间换算。
第 01 篇把投影链路的后半段讲透了：一个相机坐标系里的点 Xc，乘以 K、做透视除法，就得到像素。本篇讲前半段，也就是从世界坐标到相机坐标的那一步：
Xc=RXw+t.
R 和 t 合称外参 ，它们描述相机在世界里的位姿。这个式子看起来只是一次旋转加一次平移，但实际使用中出错最多的恰恰是它： t 是相机的位置吗（不是）？ R 是把世界转到相机，还是把相机转到世界？两个旋转先做哪个？OpenCV 的结果拿到 OpenGL 里为什么是倒着的？本篇逐一回答。
数值例子仍是标准数据集。第 0 幅图的外参（附录代码第 6、8 部分）是
R= 0.922977−0.1895420.3349450.3344620.825598−0.454448−0.1903930.5314710.825402 ,t= −54.603521−19.210297370.079863 mm.
一、旋转矩阵
1. 定义与性质
一个 3×3 矩阵 R 是旋转矩阵，当且仅当
R⊤R=I,detR=+1.
第一个条件说 R 是正交矩阵：它的三列是两两垂直的单位向量。由此 R 保持长度和夹角不变： ∥Rv∥2=v⊤R⊤Rv=∥v∥2。正交矩阵的行列式只能是 ±1（因为 det(R⊤R)=(detR)2=1）。第二个条件排除了 −1 的情况，那是镜面反射，会把右手坐标系变成左手坐标系，真实的刚体运动做不到。所有旋转矩阵构成的集合记作 SO(3)。
正交矩阵的逆就是转置， R−1=R⊤，这一点后面会反复用到。
第 0 幅图的 R： R⊤R−I 的最大元素是 1.2×10−17， detR=1.000000，确实是旋转矩阵。
2. 列和行各代表什么
在 Xc=RXw+t 里只看旋转部分。把世界坐标系的 X 轴方向 e1=(1,0,0)⊤ 代进去， Re1 就是 R 的第一列 r1。所以
R 的三列 r1,r2,r3 是世界坐标系的三个轴，在相机坐标系里的方向。
反过来， Xw=R⊤(Xc−t)，把相机的 zc 轴 (0,0,1)⊤ 代进 R⊤，得到 R⊤ 的第三列，也就是 R 的第三行。所以
R 的三行是相机坐标系的三个轴，在世界坐标系里的方向。
第 0 幅图里， r1=(0.922977, −0.189542, 0.334945)⊤：棋盘的行方向在相机里看是"偏右、略向上、往远处"。这正是第 01 篇求消失点时用的 Kr1。 R 的第三行 (0.334945, −0.454448, 0.825402) 是相机光轴在棋盘坐标系里的方向。它与棋盘法向量 (0,0,1) 的夹角是
arccosR33=arccos0.825402=34.37°,
与第 00 篇数据集表格里第 0 幅图的倾斜角 34.4° 一致。
图 1：第 0 幅图的棋盘坐标系（世界）和相机坐标系。相机的三个轴（左下角的彩色箭头）在世界里的方向就是 R 的三行。紫色箭头是 t，绿色虚线箭头是 C，第三节解释它们。
3. 基本旋转与组合顺序
绕三个坐标轴各转一个角度的旋转矩阵是（附录代码 rot_x、rot_y、rot_z）
Rx(α)= 1000cosαsinα0−sinαcosα ,Ry(β)= cosβ0−sinβ010sinβ0cosβ ,Rz(γ)= cosγsinγ0−sinγcosγ0001 .
任意旋转都可以拆成三个基本旋转的乘积，这三个角就是欧拉角 。第 00 篇的 random_pose 正是这样生成姿态的： R=Rz(c)Ry(b)Rx(a)。从 R 也能反解出三个角：
b=−arcsinR31,a=atan2(R32, R33),c=atan2(R21, R11).
（展开 RzRyRx 的第三行和第一列就能看出来，要求 ∣b∣<90°。）第 0 幅图的三个角是 a=−28.8362°， b=−19.5692°， c=−11.6049°，用它们重建 R，误差为 0。
矩阵乘法不可交换，所以旋转的顺序很重要。 对向量 v 做 RyRxv，是先做 Rx、再做 Ry（离 v 近的先做）。附录代码算了绕 x、 y 轴各转 90° 的两种顺序：
Rx(90°)Ry(90°)= 010001100 ,Ry(90°)Rx(90°)= 00−11000−10 ,
完全不同。图 2 把它们作用在一本"书"上。
图 2：同样是绕 x 轴和 y 轴各转 90°，顺序不同，书最后的朝向完全不同。彩色箭头是书自己的三个轴，也就是 R 的三列。
R=RzRyRx 有两种等价的读法：一是绕固定 的坐标轴，依次绕 x、 y、 z 转；二是绕物体自己 的坐标轴，依次绕 z、新的 y、新的 x 转。不同的软件、不同的领域（航空的"偏航--俯仰--横滚"、机器人、游戏引擎）对欧拉角的顺序和读法各有约定，拿到一组欧拉角时一定要先问清楚约定。欧拉角还有一个毛病：当中间那个角 b=±90° 时， a 和 c 绕的是同一根轴，只能确定它们的和或差，这叫万向锁（gimbal lock）。
4. 为什么还需要别的参数化
旋转矩阵有 9 个元素，却只有 3 个自由度（ R⊤R=I 给出 6 个约束）。第 07 篇用 LM 算法优化外参时，如果直接把 9 个元素当未知数，每走一步， R 都会离开 SO(3)，不再是旋转矩阵。欧拉角只有 3 个数，但有万向锁和顺序约定的麻烦。标定里最常用的是下一节的旋转向量：3 个数、没有约束、没有万向锁（只在转角接近 180° 时需要特殊处理）。
二、Rodrigues 公式
1. 推导
任何旋转都是绕某根轴转了某个角度（欧拉旋转定理）。设转轴是单位向量 k，转角是 θ（右手定则：拇指沿 k，四指的方向为正）。求任意向量 v 转动后的 Rv。
如图 3，把 v 分解成平行于 k 和垂直于 k 的两部分：
v∥=(k⋅v)k,v⊥=v−v∥.
平行分量在旋转中不动。垂直分量在垂直于 k 的平面里转过 θ。这个平面里有两个互相垂直、长度相同的向量： v⊥ 和 k×v（ k×v=k×v⊥，长度 ∥v⊥∥，方向是 v⊥ 绕 k 转 90°）。所以转过 θ 后，垂直分量变成 cosθv⊥+sinθ(k×v)，于是
Rv=v∥+cosθv⊥+sinθ(k×v).
图 3：绕单位向量 k 转 θ。平行分量 v∥ 不动，垂直分量 \perp v⊥ 在灰色虚线圆上转过 θ。 v⊥ 和 k×v 是这个平面里的一对正交基。
为了写成矩阵，引入叉积矩阵：
k×= 0k3−k2−k30k1k2−k10 ,k×v=k×v.
它是反对称矩阵， k×⊤=−k×。用二重叉积公式 k×(k×v)=k(k⋅v)−v(k⋅k)，因为 ∥k∥=1：
k×2v=v∥−v=−v⊥.
代入上式， v⊥=−k×2v， v∥=v+k×2v：
Rv=v+k×2v−cosθk×2v+sinθk×v.
对所有 v 成立，所以
R=I+sinθk×+(1−cosθ)k×2
这就是 Rodrigues 公式。
2. 旋转向量
把转轴和转角合成一个三维向量
r=θk,θ=∥r∥,k=r/∥r∥,
叫旋转向量 （rotation vector），OpenCV 里叫 rvec。cv2.calibrateCamera 和 cv2.solvePnP 返回的旋转都是它，cv2.Rodrigues 负责和旋转矩阵互相转换。附录代码里的 rodrigues 和 rodrigues_inv 是从零写的版本。
第 0 幅图的旋转向量（附录代码第 7 部分）是
r=(−0.531245, −0.283069, −0.282350)⊤,θ=38.0950°,k=(−0.799004, −0.425742, −0.424661)⊤.
即绕 k 转 38.095°。用 Rodrigues 公式从它重建 R，与原来的 R 最大差 1.1×10−16。
旋转向量还有一个更深的含义： R=exp(r×)，即矩阵指数 I+W+W2/2!+W3/3!+⋯（ W=r×）。因为 k×3=−k×（对 k×2v=−v⊥ 再乘一次 k× 即得），级数里的奇数次项合成 sinθk×，偶数次项合成 (1−cosθ)k×2，正好就是 Rodrigues 公式。附录代码把级数算到第 30 项，与 R 的最大差 2.2×10−16。
3. 反过来：从 R 求旋转向量
转角。 对 Rodrigues 公式取迹。 k× 的对角线全是 0，迹为 0； k×2=kk⊤−I，迹为 1−3=−2。所以
trR=3−2(1−cosθ)=1+2cosθ⟹cosθ=2trR−1.
第 0 幅图： trR=0.922977+0.825598+0.825402=2.573977， cosθ=0.786989， θ=38.0950°。
转轴。 I 和 k×2 都是对称矩阵， k× 是反对称矩阵，所以 R−R⊤=2sinθk×，从中读出
sinθk=21 R32−R23R13−R31R21−R12 .
除以 sinθ 就得到 k。
两种特殊情况。 θ=0 时 R=I，转轴任意，旋转向量就是 0。 θ=180° 时 sinθ=0， R−R⊤=0，上面的式子失效。这时 R=I+2k×2=2kk⊤−I，所以 (R+I)/2=kk⊤，它的任意非零列都与 k 成比例。rodrigues_inv 分别处理了这两种情况：对 Ry(180°) 得到 (0, 3.141593, 0)，对 I 得到 (0,0,0)，重建误差都是 0。
4. 小角度近似
θ 很小时 sinθ≈θ， 1−cosθ≈θ2/2，略去二阶项：
R≈I+r×.
这个近似在第 07 篇推导雅可比矩阵时会用到：外参的一个微小改变 δr，让点移动了 δr×X。它有多准？附录代码对转角 1°、5°、20° 分别计算了 R 与 I+r× 的最大元素差： 1.52×10−4、 3.81×10−3、 6.03×10−2，与 θ2/2 同阶。所以它只适合描述"微小的修正"，不能用来表示一般的旋转。
三、外参的两种读法
1. t 不是相机的位置
在 Xc=RXw+t 里令 Xw=0，得 Xc=t。所以
t 是世界原点在相机坐标系里的坐标。
第 0 幅图的 t=(−54.6, −19.2, 370.1) mm：棋盘第一个内角点在光心前方 370 mm、略偏左上。它是"从相机看出去"的量，单位虽是毫米，坐标轴却是相机的坐标轴（图 1 的紫色箭头）。
2. 相机中心 C=−R⊤t
相机光心在世界坐标系里的位置 C，是那个变到相机坐标系后成为原点的点：
RC+t=0⟹C=−R⊤t
第 0 幅图（附录代码第 8 部分）：
C=−R⊤t=(−77.199807, 202.30479, −305.651185) mm.
Cz<0，相机在棋盘的正面（ Zw 指向棋盘背面）； Cy>0，相机比棋盘的第一行低（ Yw 向下）。 t 和 C 是同一段线段在两个坐标系里的描述，方向相反，长度相同： ∣t∣=∣C∣=374.5793 mm（旋转不改变长度）。
有了 C，外参可以换一种写法：
Xc=RXw+t=RXw−RC=R(Xw−C).
读作：先把世界坐标的原点挪到相机中心，再旋转到相机的朝向。 (R,t) 描述的是"世界到相机"的变换；它的逆 (R⊤,C) 描述的是"相机到世界"的变换，也就是相机在世界里的位姿 ：位置 C，朝向的三个轴是 R⊤ 的三列（ R 的三行）。OpenCV 的 rvec、tvec 属于前者；很多图形学软件和机器人软件里，"相机位姿"指的是后者。两者混用，是实际项目里最常见的外参错误。
3. 4×4 齐次变换：求逆与复合
把旋转和平移写进一个 4×4 矩阵，
T=R0⊤t1,Xc1=TXw1,
刚体变换就成了矩阵乘法。逆变换是
T−1=R⊤0⊤−R⊤t1,
右上角正是 C。附录代码验证了 T−1T 与 I 的最大差是 2.8×10−14。
两个变换的复合也是矩阵乘法：先做 (R1,t1)、再做 (R2,t2)，得 (R2R1, R2t1+t2)。一个典型的应用是求两台相机之间的相对位姿 。第 0 幅图和第 1 幅图可以看作两台相机同时拍同一块棋盘：从相机 0 的坐标系出发，先回到世界（ T0−1），再进入相机 1（ T1），
T10=T1T0−1,R10=R1R0⊤,t10=t1−R10t0.
附录代码算出 R10 的转角是 28.855°， t10=(−205.857562, 24.177331, 175.362102) mm。 ∣t10∣=271.5028 mm，正好等于两个光心的距离 ∣C1−C0∣。把角点 #53 在相机 0 里的坐标用 (R10,t10) 变换，得到 (114.722017, 32.162989, 446.292671)，与直接用第 1 幅图的外参算的结果完全一致。第 10 篇的双目标定，求的就是这样一个相对位姿。
4. 15 个相机在哪里
标定时我们说"在 15 个不同的姿态下拍棋盘"。但对几何来说，棋盘动还是相机动没有区别。把每幅图的外参换成相机中心，就得到 15 个相机围着一块固定棋盘的样子（附录代码第 8 部分）：
| 图 | Cx | Cy | Cz | 到棋盘中心的距离 | 旋转角 |
|---|---|---|---|---|---|
| 0 | −77.2 | 202.3 | −305.7 | 380.0 | 38.10° |
| 1 | 152.3 | 256.3 | −440.3 | 483.9 | 24.01° |
| 2 | 365.8 | −72.4 | −385.7 | 487.5 | 38.06° |
| 3 | 41.7 | −107.2 | −396.2 | 434.9 | 27.16° |
| 4 | −104.2 | −135.9 | −291.4 | 407.5 | 44.86° |
| 5 | 109.9 | 386.5 | −423.5 | 533.4 | 36.93° |
| 6 | 19.9 | 119.9 | −310.9 | 326.1 | 5.83° |
| 7 | −268.8 | 141.2 | −355.8 | 518.5 | 29.60° |
| 8 | −144.1 | −0.2 | −429.2 | 497.7 | 19.34° |
| 9 | 245.3 | 199.7 | −362.8 | 414.2 | 22.93° |
| 10 | −135.0 | 351.9 | −324.8 | 494.4 | 36.61° |
| 11 | 177.7 | 218.7 | −466.4 | 498.0 | 9.18° |
| 12 | −230.5 | −180.3 | −275.7 | 494.2 | 41.20° |
| 13 | 19.3 | 41.0 | −513.6 | 520.4 | 20.83° |
| 14 | −71.6 | 27.8 | −302.7 | 349.7 | 26.42° |
（坐标与距离单位 mm，棋盘中心是 (100, 62.5, 0)；"旋转角"是旋转向量的长度 θ。）
图 4：标准数据集的 15 个相机中心 C=−R⊤t。左：从棋盘正前方看，箭头是光轴方向在棋盘平面上的分量；右：从上往下看，箭头是光轴在 Xw-- Zw 平面上的分量。相机散布在棋盘前方 0.3～0.5 m 处的各个方向，光轴都大致指向棋盘。
所有相机的 Cz 都是负的，都在棋盘正面；它们从上下左右不同的方向看棋盘。第 00 篇说过，标定需要姿态足够"多样"，这张图就是多样性的直观样子。第 6 幅图的旋转角只有 5.83°，几乎正对棋盘；第 4 幅图最斜，44.86°。
四、投影矩阵 P
1. 三种写法
把外参和内参合起来，就是第 01 篇的投影矩阵：
P=KR∣t=KR∣Kt=KRI∣−C.
最后一种写法把 t=−RC 代了进去，它把投影拆成三步：平移到相机中心、旋转、乘内参。
2. 11 个自由度
P 有 12 个元素，但只确定到一个比例因子（第 01 篇），所以有 11 个自由度： K 的 5 个、 R 的 3 个、 t 的 3 个，一个不多，一个不少。这意味着反过来也成立：任何一个左边 3×3 块可逆的 3×4 矩阵，都能唯一地分解成 KR∣t 的形式（差一个比例因子）。下面就来做这件事。
3. 从 P 分解出 K、R、t
设 P=M∣p4， M=KR。 K 是上三角矩阵， R 是旋转矩阵，"上三角 × 正交"的分解叫 RQ 分解 。它可以从最后一行往上，用 Gram--Schmidt 正交化逐行求出。记 M 的三行为 m1⊤,m2⊤,m3⊤， R 的三行为 ρ1⊤,ρ2⊤,ρ3⊤。展开 M=KR：
m3=k33ρ3,m2=k22ρ2+k23ρ3,m1=k11ρ1+k12ρ2+k13ρ3.
因为 ρi 是两两正交的单位向量：
- k33=∥m3∥， ρ3=m3/k33；
- k23=m2⋅ρ3， k22=∥m2−k23ρ3∥， ρ2=(m2−k23ρ3)/k22；
- k13=m1⋅ρ3， k12=m1⋅ρ2， k11=∥m1−k12ρ2−k13ρ3∥， ρ1 同理。
这样得到的 K 对角元都是正的。最后 t=K−1p4，再把 K 除以 k33，使右下角为 1。
还剩一个符号问题： P 和 −P 是同一个投影，但 −M=K(−R)，而 det(−R)=−1， −R 不是旋转矩阵。因为 detM=detK⋅detR， detK>0， detR=1，所以只要先检查 detM，若为负就把 P 整个取反（附录代码 decompose_P）。练习 5 会看到不做这一步的后果。
附录代码第 9 部分故意把第 0 幅图的 P 乘以 −3.7 再分解，得到的 K 与真值的最大差 1.5×10−14， R 的最大差 1.1×10−16， t 的最大差 2.8×10−14 mm。
4. 相机中心是 P 的零空间
把 C 的齐次坐标代入 P：
PC1=K(RC+t)=K0=0.
所以相机中心是 P 的零空间 ，这是唯一一个"投影不到任何像素"的点（所有视线都从它出发）。用 SVD 求零空间（最小奇异值对应的右奇异向量），附录代码得到 (−77.199807, 202.30479, −305.651185)，与 −R⊤t 完全相同。这个结论不需要先分解出 K 和 R。
5. 为什么张正友标定不直接求 P
既然 P 能分解出 K，那么从图像里估计出 P，不就完成标定了吗？ P 可以用 DLT 从 6 对以上的"三维点--像素"对应估计出来，但前提是三维点不能共面 。棋盘格的角点都在 Zw=0 平面上， P 的第三列 p3（乘的是 Zw）在投影里根本不出现，没法估计。这时能估计的只是 P 去掉第三列后的 3×3 矩阵，也就是下一节的单应矩阵 H。张正友方法的巧妙之处，就是用多幅图的 H 把 K 解出来（第 05、06 篇）。
五、反投影：从像素回到三维
1. 世界坐标系里的视线
第 01 篇说过，像素 u~ 在相机坐标系里的视线是 Xc=λ(x,y,1)⊤，其中 (x,y,1)⊤=K−1u~（有畸变时先去畸变）， λ 就是深度 Zc。用 Xw=R⊤(Xc−t) 换到世界坐标系：
Xw(λ)=C+λdw,dw=R⊤ xy1 .
视线从相机中心 C 出发，方向是 dw。 λ>0 的部分才在相机前方；求出的三维点如果 λ<0，说明它在相机背后，不可能被拍到。
2. 与平面求交
如果知道这个点在某个平面上，就能从一个像素确定它的三维位置。对棋盘平面 Zw=0，令视线的第三个分量为 0：
Cz+λdw,z=0⟹λ=−dw,zCz.
这正是第 00 篇渲染器的原理（反向追光），也是第 09 篇"地面约束测距"的原理：把棋盘平面换成地面，就能从行人脚底的像素算出他站在哪里。
附录代码第 10 部分把第 0 幅图里（带畸变）的 54 个角点反投影回棋盘平面。角点 #53 的像素是 (556.601514, 298.356522)：
- 先去畸变 ，再求交，得到 (200, 125, 0)，54 个角点的最大误差 8.5×10−14 mm；
- 忽略畸变 ，直接用 K−1，得到 (190.847729, 119.691664, 0)，误差 10.58 mm；54 个角点平均误差 2.10 mm，最大 10.58 mm。
图 5：第 0 幅图的角点反投影回棋盘平面。蓝色圆圈（先去畸变）与真值重合；橙点（忽略畸变）向画面中心方向收缩，离画面中心越远的角点误差越大。
10.58 mm 是 25 mm 方格的四成。换算到测距场景，误差还会随距离增大（第 09 篇）。
3. 单应矩阵：平面到图像的映射
对棋盘平面上的点， Zw=0，投影方程里 R 的第三列不起作用：
λu~=Kr1 r2 r3 t XwYw01 =H Kr1 r2 t XwYw1 .
3×3 矩阵 H 叫单应矩阵 （homography），它把平面上的点一一映射到（理想针孔的）像素。第 0 幅图的 H（除以 H33）是
H= 1.663578−0.0655350.0009050.1010450.92602−0.001228241.35012207.7098571 .
它的第三列（除以 H33 前是 Kt）就是世界原点的像 (241.35, 207.71)，与第 00 篇练习 4 的理想针孔位置一致。 H(200,125,1)⊤ 除以第三个分量得 (570.99, 302.04)，正是角点 #53 的理想像素；反过来， H−1 把这个像素变回 (200.0000, 125.0000)。所以对平面来说，"与平面求交"和"乘 H−1"是同一件事。
H 有 8 个自由度，可以从 4 对以上的对应点估计出来（第 05 篇的 DLT）。张正友标定的核心观察是： H 的前两列 Kr1、 Kr2 里， r1 和 r2 是正交的单位向量，这就给了 K 两个约束（第 06 篇）。
4. 深度与距离
"这个点离相机多远"有两种说法，常被混淆：
- 深度 （depth） Zc：点在光轴方向上的坐标，即到"过光心、平行于成像平面的平面"的距离；
- 距离 （range） ∥Xc∥：点到光心的直线距离。
由 Xc=Zc(x,y,1)⊤，
∥Xc∥=Zc1+x2+y2 .
角点 #53 的深度 380.2629 mm，距离 419.8076 mm，比值 1.10399。在画面左上角，这个比值（理想针孔）达到 1.23822，两者差了 24%。
图 6：左：深度相同的点在一个平面上，距离相同的点在一个球面上。中：第 0 幅图里每个像素的深度（先去畸变再与棋盘平面求交），倾斜的棋盘上深度连续变化。右：距离与深度之比，主点处为 1，向四角增大。
不同的传感器给出的量不同：立体相机和大多数 RGB-D 相机的深度图存的是深度 Zc；激光雷达和一些飞行时间（ToF）传感器测的是距离。把一张深度图反投影成点云时（第 09 篇），必须先确认存的是哪一种：把距离当深度用，画面边缘的点会被推远 20% 以上。
六、OpenCV 与 OpenGL 的相机坐标系
1. 只差一个绕 x 轴 180° 的旋转
本系列用 OpenCV 的约定：相机坐标系 x 向右、 y 向下、 z 朝前。OpenGL（以及 Blender 等很多图形软件）的相机坐标系是 x 向右、 y 向上、相机看向 −z。两者都是右手系，差别只是 y 和 z 都反了：
XGL= 1000−1000−1 XCV.
这个矩阵的行列式是 +1，它是一个旋转：用 Rodrigues 公式验证，旋转向量 (π,0,0) 给出的正是 diag(1,−1,−1)，即绕 x 轴转 180°。所以从 OpenCV 换到 OpenGL，不需要镜像，只需要把相机绕自己的 x 轴翻个跟头。
角点 #53 在 OpenCV 相机系里是 (171.799524, 46.081105, 380.262891)，在 OpenGL 相机系里是 (171.799524, −46.081105, −380.262891)：在下方、在前方，在 OpenGL 里就是 y 和 z 都为负。
图 7：同一台相机（第 0 幅图）、同一个点（角点 #53），在两种约定下的坐标轴和坐标。
2. OpenGL 的视图矩阵
OpenGL 的视图矩阵（view matrix）把世界坐标变到 OpenGL 的相机坐标。由 OpenCV 的外参得到它，只要在左边乘上这个翻转：
V=diag(1,−1,−1)0⊤01R0⊤t1=R′0⊤t′1,
即 R 和 t 的第二、三行变号。附录输出里有第 0 幅图的 V。投影矩阵那一半（从 K 构造 OpenGL 的透视投影矩阵、处理像素原点和像素中心）留到第 08 篇。
3. 常见约定一览
| 坐标系 | x | y | z | 手性 |
|---|---|---|---|---|
| OpenCV 相机系；ROS 的 optical frame | 右 | 下 | 前（视线方向） | 右手 |
| OpenGL 相机系；Blender 相机 | 右 | 上 | 后（看向 −z） | 右手 |
| ROS 的机体坐标系（REP 103） | 前 | 左 | 上 | 右手 |
| Unity | 右 | 上 | 前 | 左手 |
前三种之间都只差一个旋转。Unity 是左手系，与它们之间差一次镜像，换算时除了旋转还要把某一根轴取反，行列式为 −1 的变换不能用旋转向量表示。拿到任何一份外参或位姿数据，第一件事都是确认：它是哪个坐标系的约定？是"世界到相机"还是"相机到世界"？
七、参考资料
- R. Hartley, A. Zisserman. Multiple View Geometry in Computer Vision , 2nd ed. Cambridge University Press, 2004. ------第 6 章讲有限射影相机： P=KRI∣−C、相机中心是 P 的零空间、用 RQ 分解从 P 求 K 和 R；附录 A4 讲旋转与 RQ 分解。
- R. Szeliski. Computer Vision: Algorithms and Applications, 2nd ed. Springer, 2022. ------第 2.1.3 节讲三维旋转的各种表示：欧拉角、轴角与 Rodrigues 公式、四元数。
- Y. Ma, S. Soatto, J. Košecká, S. S. Sastry. An Invitation to 3-D Vision . Springer, 2004. ------第 2 章用矩阵指数和李群 SO(3) 系统地讲刚体运动，包括 exp(r×) 与 Rodrigues 公式的关系。
- P. Corke. Robotics, Vision and Control: Fundamental Algorithms in Python. Springer, 2023. ------第 2 章讲位姿的表示、复合与求逆，有大量机器人坐标系的例子。
- T. Foote, M. Purvis. REP 103: Standard Units of Measure and Coordinate Conventions. ROS Enhancement Proposals. ------ROS 的机体坐标系和相机 optical frame 约定。
- OpenCV 官方文档：
cv::Rodrigues、cv::decomposeProjectionMatrix、cv::RQDecomp3x3（calib3d 模块）：docs.opencv.org
小结
- 旋转矩阵 满足 R⊤R=I、 detR=1，逆就是转置。 R 的列是世界坐标轴在相机系里的方向，行是相机坐标轴在世界系里的方向。 旋转的顺序不可交换，欧拉角必须说明约定，而且有万向锁。
- Rodrigues 公式 R=I+sinθk×+(1−cosθ)k×2。旋转向量 r=θk 用 3 个无约束的数表示旋转， R=exp(r×)。反过来， cosθ=(trR−1)/2，转轴来自 R−R⊤； θ=0 和 180° 要单独处理。第 0 幅图：绕 (−0.799, −0.426, −0.425) 转 38.095°。
- t 是世界原点在相机系里的坐标，不是相机的位置 ；相机中心 C=−R⊤t，外参也可写成 Xc=R(Xw−C)。"世界到相机"和"相机到世界"是互逆的两个变换，混用是最常见的错误。
- P=KRI∣−C 有 11 个自由度 。用 RQ 分解（从最后一行往上 Gram--Schmidt）可以从 P 分解出 K、 R、 t，注意先处理 P 的符号；相机中心是 P 的零空间。共面的棋盘只能确定单应矩阵 H=Kr1 r2 t，不能确定 P。
- 反投影 ：视线 Xw=C+λR⊤K−1u~，与平面求交就能从像素恢复三维点。忽略畸变时，第 0 幅图的角点平均偏 2.10 mm、最多偏 10.58 mm。深度 Zc 与距离 ∥Xc∥ 相差 1+x2+y2 倍，画面角上达 24%。
- OpenCV 与 OpenGL 的相机坐标系只差 diag(1,−1,−1) ，即绕 x 轴转 180°。
练习题
- 判断旋转矩阵 ：下面四个矩阵里哪些是旋转矩阵？是旋转矩阵的，求出转角和转轴。 A= 0.80.60−0.60.80001 ， B=diag(1,1,−1)， C= 010001100 ， D= 1000.110001 。
- Rodrigues 公式手算 ：(a) 用 Rodrigues 公式求旋转向量 (0, 0, π/2) 对应的旋转矩阵，它把 (1,0,0) 转到哪里？(b) 绕 (1,1,1)/3 转 120° 的旋转矩阵是什么？用几何直觉解释结果。
- 相机中心 ：第 1 幅图的外参是 R1= 0.984146−0.111318−0.1380740.0521950.925806−0.3743780.1695050.3612360.916939 ， t1=(−88.631577, −61.29909, 520.704736) mm。求相机中心 C1、它到棋盘中心 (100, 62.5, 0) 的距离，以及光轴与棋盘法向量的夹角。
- 相对位姿 ：求从第 0 幅图的相机到第 6 幅图的相机的相对位姿 (R60,t60)，两台相机的光轴之间转了多少度？光心相距多远？
- 分解投影矩阵 ：把第 5 幅图的投影矩阵乘以 −0.01，得到 P= −6.095618−0.704591−0.0023191.192984−3.1973980.005531−1.439049−4.955028−0.008002−400.808204−785.325615−5.272095 。 用附录的
decompose_P分解出 K、 R、 t 和相机中心。如果跳过" detM<0 时取反"这一步，直接对 M 做 RQ 分解，得到的 R 的行列式是多少？为什么？ - 编程题：构造一个看向棋盘中心的相机 。相机放在 C=(−150, −50, −350) mm，看向棋盘中心 T=(100, 62.5, 0)，相机的"下方"尽量对齐世界的 +Yw 方向（棋盘的"下"）。 (a) 写出构造 R 的步骤： zc 沿视线方向， xc=normalize(d×zc)（ d=(0,1,0)）， yc=zc×xc， R 的三行依次是 xc,yc,zc。为什么是"行"？ (b) 求 R、 t 和旋转向量。棋盘中心投影到哪个像素？为什么？ (c) 54 个角点（理想针孔）落在画面的什么范围？棋盘有没有出画？
参考答案
- A： A⊤A=I， detA=1，是旋转矩阵。 trA=2.6， cosθ=0.8， θ=36.87°； A−A⊤ 只有 (1,2)、 (2,1) 元素非零，转轴是 z 轴，旋转向量 (0, 0, 0.643501)（0.643501 弧度 = 36.87°）。 B：正交，但 detB=−1，是关于 xy 平面的镜面反射，不是旋转。 C：正交， detC=1，是旋转矩阵， trC=0， cosθ=−1/2， θ=120°；旋转向量 (1.2092, 1.2092, 1.2092)，即绕 (1,1,1)/3 转 120°（ 1.2092=(2π/3)/3 ）。 D： detD=1，但 D⊤D−I 的最大元素是 0.1，不正交，是一个剪切，不是旋转。可见只看行列式不够，两个条件缺一不可。
- (a) θ=π/2， k=(0,0,1)， sinθ=1， 1−cosθ=1， k×2=diag(−1,−1,0)， R=I+k×+k×2= 010−100001 =Rz(90°)，把 (1,0,0) 转到 (0,1,0)。(b) cos120°=−1/2， sin120°=3 /2， k=(1,1,1)/3 。 k×2=kk⊤−I，对角元 1/3−1=−2/3，非对角元 1/3。代入得对角元 1+1.5×(−2/3)=0； (2,1) 元素 23 ⋅3 1+1.5×31=1， (1,2) 元素 −21+21=0，依此类推， R= 010001100 ，正是练习 1 的 C（附录
rodrigues验证一致）。它把 x 轴转到 y 轴、 y 轴转到 z 轴、 z 轴转到 x 轴：三根坐标轴关于 (1,1,1) 对称，转三分之一圈正好轮换一次。 - C1=−R1⊤t1=(152.298445, 256.317729, −440.287364) mm，到棋盘中心的距离 483.89 mm（与第三节表格的 483.9 一致）。光轴在世界系里是 R1 的第三行 (−0.138074, −0.374378, 0.916939)，与法向量的夹角 arccos0.916939=23.52°，与第 00 篇表格里第 1 幅图的倾斜角 23.5° 一致。
- R60=R6R0⊤， t60=t6−R60t0=(−96.765878, 82.701307, −7.528983) mm。 R60 的旋转向量长度对应 33.736°，所以两台相机的朝向差 33.736°。光心距离 ∣t60∣=∣C6−C0∣=127.514 mm。
- detM<0，先取反再分解，得 K= 5500005450322.52361 （与真值最大差 1.1×10−13）， R= 0.9723080.0288580.2319140.1073990.826177−0.553079−0.2075630.5626710.800199 ， t=(−236.262242, −84.199767, 527.209468) mm，相机中心 (109.88224, 386.527033, −423.535143) mm，与
null_vector(P)和第三节表格一致。如果不取反，RQ 分解仍然会给出对角元为正的 K（它在构造上就是正的），于是负号只能留在 R 里，得到的 R 行列式是 −1：它是"旋转 × 镜像"，不是合法的旋转矩阵，相应的 t 也会变号，算出的相机跑到了棋盘背面。 - (a) zc=(T−C)/∥T−C∥。 xc=d×zc：当相机正对 +Zw 时， (0,1,0)×(0,0,1)=(1,0,0)，正是"右"。 yc=zc×xc 保证右手系，它与 d 不一定平行，只是尽量接近。因为第一节第 2 小节说过， R 的行是相机坐标轴在世界系里的方向。(b) R= 0.813733−0.1470790.56232200.9674550.253045−0.581238−0.2059110.78725 （行列式 1）， t=−RC=(−81.373347, −45.757987, 372.538032) mm，旋转向量 (0.247554, −0.616819, −0.079332)，转角 38.35°。棋盘中心投影到 (322.5, 236.0)，正好是主点：它在光轴上，相机坐标是 (0,0,444.585)， x=y=0。深度 444.585 mm 就是 ∥T−C∥=2502+112.52+3502 。(c) 角点的 u 在 202.4～414.8、 v 在 151.5～337.4 之间；连黑白方格的外边界都在 163.6, 436.0×122.0, 377.7 之内，没有出画。光轴与棋盘法向量的夹角是 38.07°。这种"look-at"构造在第 08 篇放置虚拟相机、在 OpenGL 系列里构造
gluLookAt时都会用到（OpenGL 版本的 y 和 z 方向相反）。
附录：完整代码
下面的代码验证了本篇的全部结论：旋转矩阵的性质和欧拉角，从零实现的 Rodrigues 公式及其逆，相机中心与位姿的求逆、复合，投影矩阵的 RQ 分解和零空间，反投影到棋盘平面，单应矩阵，以及 OpenCV 到 OpenGL 的换算。只依赖 numpy 和 matplotlib，保存为 example.py 后运行 python example.py 即可（约 3 秒）。第 1 部分与第 00 篇相同；rodrigues、rodrigues_inv、camera_center、decompose_P、backproject_to_board、homography 后面各篇会继续用到。
python
"""相机模型与标定自学教程 02：外参 [R|t] 与完整投影链路 ------ 附录代码
本篇讨论外参，用 numpy 验证文中的每个结论：
1. 旋转矩阵的性质，欧拉角的组合顺序，Rodrigues 公式及其逆（旋转向量 <-> 旋转矩阵）；
2. 外参的两种读法：t 是世界原点在相机系里的坐标，相机中心 C = -R^T t；
位姿求逆与复合，15 个相机在棋盘坐标系里的位置；
3. 投影矩阵 P = K[R|t] 的 11 个自由度：从 P 反过来分解出 K、R、t 和相机中心；
4. 反投影：从像素出发的视线与棋盘平面求交，深度与距离，单应矩阵 H = K[r1 r2 t]；
5. OpenCV 与 OpenGL 相机坐标系的换算。
运行：python example.py
"""
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams["font.sans-serif"] = ["Microsoft YaHei", "SimHei", "DejaVu Sans"]
plt.rcParams["axes.unicode_minus"] = False
# ---------------- 1. 虚拟相机与棋盘格（与第 00 篇相同） ----------------
IMG_W, IMG_H = 640, 480 # 图像宽、高（像素）
FX, FY = 550.0, 545.0 # 以像素为单位的焦距
CX, CY = 322.5, 236.0 # 主点（像素）
SKEW = 0.0 # 斜切参数
DIST = np.array([-0.28, 0.08, 0.001, -0.0005, 0.0]) # k1, k2, p1, p2, k3
BOARD_COLS, BOARD_ROWS = 9, 6 # 内角点：每行 9 个，共 6 行
SQUARE = 25.0 # 方格边长（mm）
MARGIN = 0.6 # 棋盘外的白边宽度（以方格边长为单位）
N_VIEWS, SEED = 15, 2023 # 标准数据集的图像数与随机种子
IDX = BOARD_COLS * BOARD_ROWS - 1 # 第 00 篇手算过的角点 #53
def camera_matrix(fx=FX, fy=FY, cx=CX, cy=CY, skew=SKEW):
"""内参矩阵 K。"""
return np.array([[fx, skew, cx],
[0.0, fy, cy],
[0.0, 0.0, 1.0]])
K_TRUE = camera_matrix()
def board_points(cols=BOARD_COLS, rows=BOARD_ROWS, square=SQUARE):
"""内角点的世界坐标 (rows*cols, 3)，原点在第一个内角点，按行排列。"""
j, i = np.mgrid[0:rows, 0:cols]
return np.stack([i.ravel() * square, j.ravel() * square, np.zeros(rows * cols)], axis=1)
def board_outline(margin=0.0, n=40):
"""棋盘格的外边界，每条边取 n 个点，世界坐标 (4n, 3)。"""
x0, x1 = -(1 + margin) * SQUARE, (BOARD_COLS + margin) * SQUARE
y0, y1 = -(1 + margin) * SQUARE, (BOARD_ROWS + margin) * SQUARE
s = np.linspace(0, 1, n, endpoint=False)
edges = [(x0 + (x1 - x0) * s, np.full(n, y0)), (np.full(n, x1), y0 + (y1 - y0) * s),
(x1 - (x1 - x0) * s, np.full(n, y1)), (np.full(n, x0), y1 - (y1 - y0) * s)]
xy = np.concatenate([np.stack(e, axis=1) for e in edges])
return np.hstack([xy, np.zeros((len(xy), 1))])
def distort(x, y, dist=DIST):
"""Brown--Conrady 畸变模型（第 03 篇详细讲）。"""
k1, k2, p1, p2, k3 = dist
r2 = x * x + y * y
radial = 1 + k1 * r2 + k2 * r2 ** 2 + k3 * r2 ** 3
return (x * radial + 2 * p1 * x * y + p2 * (r2 + 2 * x * x),
y * radial + p1 * (r2 + 2 * y * y) + 2 * p2 * x * y)
def undistort_normalized(xd, yd, dist=DIST, n_iter=20):
"""distort 的逆，不动点迭代（第 03 篇详细讲）。"""
k1, k2, p1, p2, k3 = dist
xd, yd = np.asarray(xd, dtype=float), np.asarray(yd, dtype=float)
x, y = xd.copy(), yd.copy()
for _ in range(n_iter):
r2 = x * x + y * y
radial = 1 + k1 * r2 + k2 * r2 ** 2 + k3 * r2 ** 3
x = (xd - 2 * p1 * x * y - p2 * (r2 + 2 * x * x)) / radial
y = (yd - p1 * (r2 + 2 * y * y) - 2 * p2 * x * y) / radial
return x, y
def project(Xw, R, t, K=K_TRUE, dist=DIST):
"""第 00 篇的逐步投影：世界 -> 相机 -> 归一化 -> 畸变 -> 像素。dist=None 为理想针孔。"""
Xc = Xw @ R.T + t
x, y = Xc[:, 0] / Xc[:, 2], Xc[:, 1] / Xc[:, 2]
if dist is not None:
x, y = distort(x, y, dist)
return np.stack([K[0, 0] * x + K[0, 1] * y + K[0, 2], K[1, 1] * y + K[1, 2]], axis=1)
def pixel_to_normalized(u, v, K=K_TRUE, dist=DIST):
"""像素坐标 -> 去畸变后的归一化坐标 (x, y)。dist=None 表示不考虑畸变。"""
yd = (np.asarray(v, dtype=float) - K[1, 2]) / K[1, 1]
xd = (np.asarray(u, dtype=float) - K[0, 2] - K[0, 1] * yd) / K[0, 0]
if dist is None:
return xd, yd
return undistort_normalized(xd, yd, dist)
def rot_x(a):
c, s = np.cos(a), np.sin(a)
return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])
def rot_y(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
def rot_z(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
def random_pose(rng, max_tilt=35.0, max_roll=15.0, depth_range=(260.0, 520.0), border=8):
"""与第 00 篇相同的随机姿态（X_c = R X_w + t），R = rot_z(c) rot_y(b) rot_x(a)。"""
center = np.array([(BOARD_COLS - 1) / 2 * SQUARE, (BOARD_ROWS - 1) / 2 * SQUARE, 0.0])
outline = board_outline()
while True:
a, b = np.radians(rng.uniform(-max_tilt, max_tilt, 2))
c = np.radians(rng.uniform(-max_roll, max_roll))
R = rot_z(c) @ rot_y(b) @ rot_x(a)
depth = rng.uniform(*depth_range)
u0, v0 = rng.uniform(0.15, 0.85) * IMG_W, rng.uniform(0.15, 0.85) * IMG_H
t = depth * np.array([(u0 - CX) / FX, (v0 - CY) / FY, 1.0]) - R @ center
uv = project(outline, R, t, dist=None)
if (uv.min(axis=0) >= border).all() and (uv[:, 0].max() <= IMG_W - 1 - border) \
and (uv[:, 1].max() <= IMG_H - 1 - border):
return R, t
def make_views(n_views=N_VIEWS, seed=SEED):
"""全系列的标准数据集：n_views 个棋盘格姿态 [(R, t), ...]。"""
rng = np.random.default_rng(seed)
return [random_pose(rng) for _ in range(n_views)]
# ---------------- 2. 旋转：欧拉角、Rodrigues 公式 ----------------
def euler_zyx(R):
"""把 R = rot_z(c) rot_y(b) rot_x(a) 分解回 (a, b, c)（弧度），要求 |b| < 90°。"""
b = -np.arcsin(R[2, 0])
a = np.arctan2(R[2, 1], R[2, 2])
c = np.arctan2(R[1, 0], R[0, 0])
return a, b, c
def skew(k):
"""叉积矩阵 [k]_x：[k]_x v = k × v。"""
return np.array([[0.0, -k[2], k[1]],
[k[2], 0.0, -k[0]],
[-k[1], k[0], 0.0]])
def rodrigues(rvec):
"""旋转向量 rvec = θ k（|k| = 1）-> 旋转矩阵：R = I + sinθ [k]_x + (1 - cosθ) [k]_x^2。"""
rvec = np.asarray(rvec, dtype=float)
theta = np.linalg.norm(rvec)
if theta < 1e-12:
return np.eye(3) + skew(rvec) # 一阶近似，θ -> 0 时精确
Kx = skew(rvec / theta)
return np.eye(3) + np.sin(theta) * Kx + (1 - np.cos(theta)) * Kx @ Kx
def rodrigues_inv(R):
"""旋转矩阵 -> 旋转向量：θ = arccos((tr R - 1)/2)，k 由 R - R^T 的反对称部分给出。"""
cos_t = np.clip((np.trace(R) - 1) / 2, -1.0, 1.0)
theta = np.arccos(cos_t)
w = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) / 2 # = sinθ k
if theta < 1e-12:
return w
if np.pi - theta < 1e-6: # θ ≈ 180°：sinθ ≈ 0，改用 R + I = 2 k k^T
B = (R + np.eye(3)) / 2
k = B[:, np.argmax(np.diag(B))]
return theta * k / np.linalg.norm(k)
return theta * w / np.sin(theta)
def exp_series(Wx, n_terms=30):
"""矩阵指数 exp(W) 的泰勒级数 I + W + W^2/2! + ...，用来核对 Rodrigues 公式。"""
out, term = np.eye(3), np.eye(3)
for n in range(1, n_terms):
term = term @ Wx / n
out = out + term
return out
# ---------------- 3. 位姿：相机中心、求逆、复合 ----------------
def camera_center(R, t):
"""相机光心在世界坐标系里的位置 C = -R^T t。"""
return -R.T @ t
def invert_pose(R, t):
"""X_c = R X_w + t 的逆变换：X_w = R^T X_c - R^T t。"""
return R.T, -R.T @ t
def compose(R2, t2, R1, t1):
"""先做 (R1, t1)，再做 (R2, t2)：X'' = R2 (R1 X + t1) + t2。"""
return R2 @ R1, R2 @ t1 + t2
def to_4x4(R, t):
"""把 (R, t) 写成 4×4 齐次变换矩阵。"""
T = np.eye(4)
T[:3, :3], T[:3, 3] = R, t
return T
# ---------------- 4. 投影矩阵的分解 ----------------
def projection_matrix(K, R, t):
"""3×4 投影矩阵 P = K [R | t]。"""
return K @ np.hstack([R, t.reshape(3, 1)])
def rq3(M):
"""把 3×3 矩阵分解成 M = K R：K 上三角、对角元为正，R 是正交矩阵。
从最后一行往上做 Gram--Schmidt：M 的第 3 行 = K33 r3，第 2 行 = K22 r2 + K23 r3，
第 1 行 = K11 r1 + K12 r2 + K13 r3（r_i 是 R 的行）。
"""
m1, m2, m3 = M
k33 = np.linalg.norm(m3)
r3 = m3 / k33
k23 = m2 @ r3
k22 = np.linalg.norm(m2 - k23 * r3)
r2 = (m2 - k23 * r3) / k22
k13, k12 = m1 @ r3, m1 @ r2
k11 = np.linalg.norm(m1 - k12 * r2 - k13 * r3)
r1 = (m1 - k12 * r2 - k13 * r3) / k11
K = np.array([[k11, k12, k13], [0.0, k22, k23], [0.0, 0.0, k33]])
return K, np.stack([r1, r2, r3])
def decompose_P(P):
"""P（只确定到一个比例因子）-> K（K33 = 1）、R（行列式为 +1）、t。"""
if np.linalg.det(P[:, :3]) < 0: # P 和 -P 是同一个投影，选 det(KR) > 0 的那个
P = -P
K, R = rq3(P[:, :3])
t = np.linalg.solve(K, P[:, 3])
return K / K[2, 2], R, t
def null_vector(P):
"""P 的零空间（SVD 最小奇异值对应的右奇异向量），归一化成齐次坐标 (X, Y, Z, 1)。"""
_, _, Vt = np.linalg.svd(P)
c = Vt[-1]
return c[:3] / c[3]
# ---------------- 5. 反投影与单应矩阵 ----------------
def backproject_to_board(uv, R, t, K=K_TRUE, dist=DIST):
"""像素 (N, 2) -> 视线与棋盘平面 Z_w = 0 的交点（世界坐标 (N, 3)）。
视线在世界系里是 X_w = C + λ d_w，C = -R^T t，d_w = R^T (x, y, 1)；
令 Z_w = 0 解出 λ = -C_z / d_wz。
"""
x, y = pixel_to_normalized(uv[:, 0], uv[:, 1], K, dist)
d_w = np.stack([x, y, np.ones_like(x)], axis=1) @ R # 每行是 R^T (x, y, 1)
C = camera_center(R, t)
lam = -C[2] / d_w[:, 2]
return C + lam[:, None] * d_w
def homography(K, R, t):
"""棋盘平面 (X_w, Y_w, 1) -> 像素的单应矩阵 H = K [r1 r2 t]（理想针孔）。"""
return K @ np.column_stack([R[:, 0], R[:, 1], t])
# OpenCV 相机系（x 右、y 下、z 朝前）-> OpenGL 相机系（x 右、y 上、看向 -z）
CV_TO_GL = np.diag([1.0, -1.0, -1.0])
if __name__ == "__main__":
np.set_printoptions(precision=6, suppress=True)
poses = make_views()
R, t = poses[0]
Xw = board_points()
# ---------------- 6. 旋转矩阵的性质 ----------------
print("第 0 幅图的 R：")
for row in R:
print(" ", " ".join(f"{e:9.6f}" for e in row))
print(f"|R^T R - I| 的最大元素 {np.abs(R.T @ R - np.eye(3)).max():.1e}，det R = {np.linalg.det(R):.6f}")
print(f"第 1 列 r1 = {R[:, 0]}：世界 X 轴在相机系里的方向")
print(f"第 3 行 = {R[2]}：相机光轴 z_c 在世界系里的方向")
a, b, c = euler_zyx(R)
print(f"欧拉角 R = rot_z(c) rot_y(b) rot_x(a)：a = {np.degrees(a):.4f}°，"
f"b = {np.degrees(b):.4f}°，c = {np.degrees(c):.4f}°")
print(f" 用这三个角重建 R 的最大误差 {np.abs(rot_z(c) @ rot_y(b) @ rot_x(a) - R).max():.1e}")
q = np.radians(90)
print("rot_x(90°) rot_y(90°) =\n", (rot_x(q) @ rot_y(q)).round(12) + 0.0) # + 0.0 去掉 -0
print("rot_y(90°) rot_x(90°) =\n", (rot_y(q) @ rot_x(q)).round(12) + 0.0)
# ---------------- 7. Rodrigues 公式 ----------------
rvec = rodrigues_inv(R)
theta = np.linalg.norm(rvec)
print(f"\n旋转向量 rvec = {rvec}，转角 θ = {np.degrees(theta):.4f}°，转轴 k = {rvec / theta}")
print(f" tr R = {np.trace(R):.6f}，cos θ = (tr R - 1)/2 = {(np.trace(R) - 1) / 2:.6f}")
print(f" rodrigues(rvec) 与 R 的最大差 {np.abs(rodrigues(rvec) - R).max():.1e}")
print(f" 与矩阵指数级数 exp([rvec]_x) 的最大差 {np.abs(exp_series(skew(rvec)) - R).max():.1e}")
for deg in (1.0, 5.0, 20.0):
w = np.radians(deg) * np.array([0.6, 0.0, 0.8])
err = np.abs(rodrigues(w) - (np.eye(3) + skew(w))).max()
print(f" 转 {deg:4.1f}° 时一阶近似 I + [rvec]_x 的最大误差 {err:.2e}")
for name, Rt in (("θ = 180°", rot_y(np.pi)), ("θ = 0", np.eye(3))):
rv = rodrigues_inv(Rt)
print(f" {name}：rvec = {rv + 0.0}，重建误差 {np.abs(rodrigues(rv) - Rt).max():.1e}")
# ---------------- 8. 外参的两种读法 ----------------
C = camera_center(R, t)
print(f"\nt = {t} mm：世界原点（棋盘第一个内角点）在相机系里的坐标")
print(f"C = -R^T t = {C} mm：相机光心在棋盘坐标系里的位置")
print(f"|t| = {np.linalg.norm(t):.4f}，|C| = {np.linalg.norm(C):.4f}（同一段距离）")
print(f"把 C 变到相机系：R C + t = {(R @ C + t).round(12) + 0.0}")
Ri, ti = invert_pose(R, t)
print(f"逆变换 (R^T, -R^T t) 与原变换复合：4×4 矩阵乘积与 I 的最大差 "
f"{np.abs(to_4x4(Ri, ti) @ to_4x4(R, t) - np.eye(4)).max():.1e}")
tilt = np.degrees(np.arccos(R[2, 2]))
print(f"光轴与棋盘法向量的夹角 arccos(R33) = {tilt:.2f}°")
R1, t1 = poses[1]
R10, t10 = compose(R1, t1, *invert_pose(R, t)) # 相机 0 系 -> 世界 -> 相机 1 系
C1 = camera_center(R1, t1)
print(f"\n相机 0 -> 相机 1 的相对位姿：转角 {np.degrees(np.linalg.norm(rodrigues_inv(R10))):.3f}°，"
f"t_10 = {t10} mm")
print(f" 两个光心的距离 |C1 - C0| = {np.linalg.norm(C1 - C):.4f} mm = |t_10| = {np.linalg.norm(t10):.4f} mm")
Xc0 = R @ Xw[IDX] + t
print(f" 角点 #{IDX}：R_10 X_c0 + t_10 = {R10 @ Xc0 + t10}，R1 X_w + t1 = {R1 @ Xw[IDX] + t1}")
centre = np.array([(BOARD_COLS - 1) / 2 * SQUARE, (BOARD_ROWS - 1) / 2 * SQUARE, 0.0])
print("\n15 个相机在棋盘坐标系里的位置（mm），以及到棋盘中心的距离：")
print(" 图 C_x C_y C_z 距离 旋转角/°")
for k, (R_, t_) in enumerate(poses):
Ck = camera_center(R_, t_)
print(f" {k:2d} {Ck[0]:9.1f} {Ck[1]:9.1f} {Ck[2]:9.1f} {np.linalg.norm(Ck - centre):7.1f}"
f" {np.degrees(np.linalg.norm(rodrigues_inv(R_))):8.2f}")
# ---------------- 9. 分解投影矩阵 ----------------
P = -3.7 * projection_matrix(K_TRUE, R, t) # 故意乘一个负的比例因子
K_hat, R_hat, t_hat = decompose_P(P)
print("\n从 -3.7 K[R|t] 分解出的 K =\n", K_hat)
print(f" K、R、t 的最大误差：{np.abs(K_hat - K_TRUE).max():.1e}，{np.abs(R_hat - R).max():.1e}，"
f"{np.abs(t_hat - t).max():.1e}")
print(f" P 的零空间 = {null_vector(P)}，-R^T t = {C}")
print(f" 自由度：12 个元素 - 1 个比例 = 11 = K 的 5 个 + R 的 3 个 + t 的 3 个")
# ---------------- 10. 反投影、深度与距离 ----------------
uv = project(Xw, R, t) # 图像里（带畸变）的角点
print(f"\n角点 #{IDX} 的像素 {uv[IDX]} 反投影到棋盘平面：")
back = backproject_to_board(uv, R, t)
print(f" 先去畸变：{back[IDX]}，54 个角点的最大误差 {np.abs(back - Xw).max():.1e} mm")
naive = backproject_to_board(uv, R, t, dist=None)
e = np.linalg.norm(naive - Xw, axis=1)
print(f" 忽略畸变：{naive[IDX]}，误差 {e[IDX]:.2f} mm；54 个角点平均 {e.mean():.2f} mm、最大 {e.max():.2f} mm")
Xc = R @ Xw[IDX] + t
rng_ = np.linalg.norm(Xc)
print(f" 深度 Z_c = {Xc[2]:.4f} mm，距离 |X_c| = {rng_:.4f} mm，"
f"比值 sqrt(1 + x^2 + y^2) = {rng_ / Xc[2]:.5f}")
xc, yc = pixel_to_normalized(np.array([-0.5]), np.array([-0.5]), dist=None)
print(f" 画面左上角（理想针孔）的比值 {np.sqrt(1 + xc[0] ** 2 + yc[0] ** 2):.5f}")
H = homography(K_TRUE, R, t)
print("\n单应矩阵 H = K [r1 r2 t]（除以 H33）=\n", H / H[2, 2])
h = H @ np.array([Xw[IDX, 0], Xw[IDX, 1], 1.0])
print(f" H (200, 125, 1) -> ({h[0] / h[2]:.2f}, {h[1] / h[2]:.2f})")
g = np.linalg.solve(H, np.array([*project(Xw[IDX:IDX + 1], R, t, dist=None)[0], 1.0]))
print(f" H^-1 把理想像素变回棋盘坐标 ({g[0] / g[2]:.4f}, {g[1] / g[2]:.4f})")
# ---------------- 11. OpenCV 与 OpenGL 相机坐标系 ----------------
print(f"\n角点 #{IDX}：OpenCV 相机系 {Xc}，OpenGL 相机系 {CV_TO_GL @ Xc + 0.0}")
print(f"diag(1, -1, -1) 的行列式 {np.linalg.det(CV_TO_GL):.0f}，"
f"它是绕 x 轴转 180°：rodrigues((π, 0, 0)) = diag{np.diag(rodrigues([np.pi, 0, 0])).round(12) + 0.0}")
V = to_4x4(CV_TO_GL @ R, CV_TO_GL @ t)
print("OpenGL 的视图矩阵 V = diag(1, -1, -1, 1) [R t; 0 1] =\n", V)
fig = plt.figure(figsize=(7, 6))
ax = fig.add_subplot(projection="3d")
ax.plot(*board_outline().T, color="k")
for R_, t_ in poses:
Ck = camera_center(R_, t_)
ax.scatter(*Ck, color="tab:blue")
ax.plot(*np.stack([Ck, Ck + 60 * R_[2]]).T, color="tab:orange") # 光轴方向
ax.set_xlabel("X_w")
ax.set_ylabel("Y_w")
ax.set_zlabel("Z_w")
ax.set_title("15 个相机在棋盘坐标系里的位置（橙线是光轴）")
plt.tight_layout()
plt.show()运行输出：
text
第 0 幅图的 R：
0.922977 0.334462 -0.190393
-0.189542 0.825598 0.531471
0.334945 -0.454448 0.825402
|R^T R - I| 的最大元素 1.2e-17，det R = 1.000000
第 1 列 r1 = [ 0.922977 -0.189542 0.334945]：世界 X 轴在相机系里的方向
第 3 行 = [ 0.334945 -0.454448 0.825402]：相机光轴 z_c 在世界系里的方向
欧拉角 R = rot_z(c) rot_y(b) rot_x(a)：a = -28.8362°，b = -19.5692°，c = -11.6049°
用这三个角重建 R 的最大误差 0.0e+00
rot_x(90°) rot_y(90°) =
[[0. 0. 1.]
[1. 0. 0.]
[0. 1. 0.]]
rot_y(90°) rot_x(90°) =
[[ 0. 1. 0.]
[ 0. 0. -1.]
[-1. 0. 0.]]
旋转向量 rvec = [-0.531245 -0.283069 -0.28235 ]，转角 θ = 38.0950°，转轴 k = [-0.799004 -0.425742 -0.424661]
tr R = 2.573977，cos θ = (tr R - 1)/2 = 0.786989
rodrigues(rvec) 与 R 的最大差 1.1e-16
与矩阵指数级数 exp([rvec]_x) 的最大差 2.2e-16
转 1.0° 时一阶近似 I + [rvec]_x 的最大误差 1.52e-04
转 5.0° 时一阶近似 I + [rvec]_x 的最大误差 3.81e-03
转 20.0° 时一阶近似 I + [rvec]_x 的最大误差 6.03e-02
θ = 180°：rvec = [0. 3.141593 0. ]，重建误差 0.0e+00
θ = 0：rvec = [0. 0. 0.]，重建误差 0.0e+00
t = [-54.603521 -19.210297 370.079863] mm：世界原点（棋盘第一个内角点）在相机系里的坐标
C = -R^T t = [ -77.199807 202.30479 -305.651185] mm：相机光心在棋盘坐标系里的位置
|t| = 374.5793，|C| = 374.5793（同一段距离）
把 C 变到相机系：R C + t = [0. 0. 0.]
逆变换 (R^T, -R^T t) 与原变换复合：4×4 矩阵乘积与 I 的最大差 2.8e-14
光轴与棋盘法向量的夹角 arccos(R33) = 34.37°
相机 0 -> 相机 1 的相对位姿：转角 28.855°，t_10 = [-205.857562 24.177331 175.362102] mm
两个光心的距离 |C1 - C0| = 271.5028 mm = |t_10| = 271.5028 mm
角点 #53：R_10 X_c0 + t_10 = [114.722017 32.162989 446.292671]，R1 X_w + t1 = [114.722017 32.162989 446.292671]
15 个相机在棋盘坐标系里的位置（mm），以及到棋盘中心的距离：
图 C_x C_y C_z 距离 旋转角/°
0 -77.2 202.3 -305.7 380.0 38.10
1 152.3 256.3 -440.3 483.9 24.01
2 365.8 -72.4 -385.7 487.5 38.06
3 41.7 -107.2 -396.2 434.9 27.16
4 -104.2 -135.9 -291.4 407.5 44.86
5 109.9 386.5 -423.5 533.4 36.93
6 19.9 119.9 -310.9 326.1 5.83
7 -268.8 141.2 -355.8 518.5 29.60
8 -144.1 -0.2 -429.2 497.7 19.34
9 245.3 199.7 -362.8 414.2 22.93
10 -135.0 351.9 -324.8 494.4 36.61
11 177.7 218.7 -466.4 498.0 9.18
12 -230.5 -180.3 -275.7 494.2 41.20
13 19.3 41.0 -513.6 520.4 20.83
14 -71.6 27.8 -302.7 349.7 26.42
从 -3.7 K[R|t] 分解出的 K =
[[550. 0. 322.5]
[ 0. 545. 236. ]
[ 0. 0. 1. ]]
K、R、t 的最大误差：1.5e-14，1.1e-16，2.8e-14
P 的零空间 = [ -77.199807 202.30479 -305.651185]，-R^T t = [ -77.199807 202.30479 -305.651185]
自由度：12 个元素 - 1 个比例 = 11 = K 的 5 个 + R 的 3 个 + t 的 3 个
角点 #53 的像素 [556.601514 298.356522] 反投影到棋盘平面：
先去畸变：[200. 125. 0.]，54 个角点的最大误差 8.5e-14 mm
忽略畸变：[190.847729 119.691664 0. ]，误差 10.58 mm；54 个角点平均 2.10 mm、最大 10.58 mm
深度 Z_c = 380.2629 mm，距离 |X_c| = 419.8076 mm，比值 sqrt(1 + x^2 + y^2) = 1.10399
画面左上角（理想针孔）的比值 1.23822
单应矩阵 H = K [r1 r2 t]（除以 H33）=
[[ 1.663578 0.101045 241.35012 ]
[ -0.065535 0.92602 207.709857]
[ 0.000905 -0.001228 1. ]]
H (200, 125, 1) -> (570.99, 302.04)
H^-1 把理想像素变回棋盘坐标 (200.0000, 125.0000)
角点 #53：OpenCV 相机系 [171.799524 46.081105 380.262891]，OpenGL 相机系 [ 171.799524 -46.081105 -380.262891]
diag(1, -1, -1) 的行列式 1，它是绕 x 轴转 180°：rodrigues((π, 0, 0)) = diag[ 1. -1. -1.]
OpenGL 的视图矩阵 V = diag(1, -1, -1, 1) [R t; 0 1] =
[[ 0.922977 0.334462 -0.190393 -54.603521]
[ 0.189542 -0.825598 -0.531471 19.210297]
[ -0.334945 0.454448 -0.825402 -370.079863]
[ 0. 0. 0. 1. ]]