内参、外参与畸变

一台手眼系统里的深度相机看见一只杯子，杯子在像素 \((u,v)\)，深度是 \(z\)。要抓它，机器人得把这个像素变成基座坐标系里的一个三维点，路上要走三步：像素 \(\to\) 相机坐标系里的射线（靠内参 K），修正镜头弯曲了的光线（靠畸变系数），相机坐标系 \(\to\) 基座坐标系（靠外参，也就是手眼标定的结果，见 7.3 的工具系与基座系）。每一步都有几个数，每个数都有单位和方向的约定。

这些数错一点，后果比想象中大。取 \(f=600\) 像素、深度 \(z=2\) m：横向坐标是 \(X=(u-c_x)\,z/f\)，如果主点 \(c_x\) 偏了 10 像素，杯子的横向位置就偏了 \(10\cdot2/600\approx3.3\) cm，已经超出大多数夹爪的容差；焦距偏 5%，离光轴 0.5 m 的点偏 2.5 cm。更糟的是误差和位置有关：一面平整的墙在点云里会弯成"香蕉"，两面垂直的墙夹角变成 85°。

10.1 节给出针孔模型，10.2 节把投影写成了 \(\lambda\tilde{\mathbf x}=K[R|t]\tilde{\mathbf X}\)。这一课打开这个式子：K 里每个元素是什么，\([R|t]\) 为什么是相机位姿的逆，镜头畸变怎么建模、为什么不能直接求逆，以及 OpenCV、OpenGL、ROS 三套坐标约定怎么互相换算。下一课 10.4 会反过来从图像里估计这些参数。

先把投影拆成两半。第一半与相机的"硬件"无关：相机坐标系（OpenCV 约定：x 向右，y 向下，z 向前）里的点 \((X,Y,Z)\) 除以深度，得到归一化坐标 \((x,y)=(X/Z,\,Y/Z)\)。它没有单位，就是光线方向的斜率，等价于一张放在 \(z=1\) 处的"理想底片"。第二半才是传感器：K 把这张理想底片上的坐标（以"焦距"为单位）换成像素。它只做三件事：

• 缩放：\(f_x,f_y\)，单位是像素，等于"一个单位斜率对应多少像素"。镜头变长（变焦），\(f\) 变大，视场变窄。
• 平移：主点 \((c_x,c_y)\)，光轴与传感器的交点。它不是平移相机，也不是转动相机：所有深度上的点都平移同样的像素数，没有视差。理想情况在图像中心，实际偏几个到几十个像素。
• 变形：\(f_x\neq f_y\)（像素不是正方形）和斜切 \(s\)（传感器行列不垂直）。现代相机几乎总是 \(f_x\approx f_y\)，\(s\approx0\)，但模型里留着它们。

下面的实验把这几件事摆在一起。左边从外面看相机和它的视锥，右边是相机拍到的图像（像素坐标 \(v\) 向下；图中坐标轴已按这个方向标出）。注意：改 K 时相机的位置和朝向一点没变，只有"底片上的刻度"变了。

\(f\) 以像素计，和镜头标称的毫米焦距 \(f_{\text{mm}}\) 差一个像素间距：\(f_x=f_{\text{mm}}/p_x\)（\(p_x\) 是像素在传感器上的宽度）。4 mm 的镜头配 3 µm 的像素，\(f_x\approx1333\) px。同一个镜头换成更高分辨率的传感器（像素更小，或者同样的传感器读出更多像素），\(f_x\) 的数字就变了，镜头和视场角却没变，这就是为什么 K 必须绑定到具体的分辨率上。视场角只由 \(f\) 和图像宽度的比值决定：

这就是视场角公式（见"数学"一节的推导）。例：\(w=640\)，\(\theta_h=90^\circ\) 时 \(f_x=320\)；\(60^\circ\) 时 \(f_x\approx554\)；\(120^\circ\) 时 \(f_x\approx185\)。主点偏离中心时左右两侧的半角不再相等：左边是 \(\arctan(c_x/f_x)\)，右边是 \(\arctan((w-c_x)/f_x)\)，FIG 1 的读数里能看到。

改变图像大小时 K 怎么跟着变？如果把图像缩放 \(a\) 倍，每个像素坐标都乘 \(a\)，所以 \(f_x,f_y,c_x,c_y\) 都乘 \(a\)（\(s\) 也是）；如果裁出一个左上角在 \((x_0,y_0)\) 的窗口，像素坐标整体平移，只有 \(c_x,c_y\) 减去偏移，\(f\) 不变。归一化坐标和畸变系数与像素无关，所以畸变系数 D 在缩放和裁剪下保持不变。写成矩阵是 \(K'=S\,T\,K\)，在"数学"一节推导。

内参管"相机内部"，外参管"相机在世界里的哪里"。世界系里一个点 \(\mathbf X_w\) 要经过三次变换才变成像素：

1. 世界 \(\to\) 相机：\(\mathbf X_c={}^{c}T_{w}\,\mathbf X_w=R\mathbf X_w+\mathbf t\)。这里的 \([R|t]\) 就是外参，它把世界坐标变成相机坐标。
2. 相机 \(\to\) 归一化平面：\((x,y)=(X_c/Z_c,\;Y_c/Z_c)\)，除以深度（这一步丢掉了深度信息）。
3. 归一化 \(\to\) 像素：\((u,v)\) 由 K 给出（再加上上一节说的畸变，我们放在后面）。

最容易混淆的是方向：外参是相机位姿 \({}^{w}T_{c}\) 的逆。相机位姿说"相机坐标系的原点和三根轴在世界里的样子"，它的平移部分就是相机中心 \(C\)；外参是把世界点搬到相机里去，它的平移 \(\mathbf t=-R\,C\) 是"世界原点在相机坐标系里的位置"，不是相机的位置。FIG 2 让你自己摆相机，看读数里的 \([R|t]\) 和 \(C=-R^{\top}\mathbf t\)。

真实镜头会把光线"弯"一点，所以直线在图像里不再是直线。标准做法是在归一化坐标里先弯一下，再交给 K 换成像素：\((x,y)\to(x_d,y_d)\to(u,v)\)。弯的方式用两组项描述：径向项 \(k_1,k_2,k_3\)，让点沿半径方向向内或向外移动，移动量随半径 \(r=\sqrt{x^2+y^2}\) 迅速增大；切向项 \(p_1,p_2\)，来自镜片与传感器没有完全对中（"偏心畸变"）。

\(k_1<0\) 时放大率随半径减小，图像边缘被"收"回来，直线向外鼓，叫桶形畸变，广角镜头常见；\(k_1>0\) 反过来，叫枕形，长焦常见；\(k_1\) 与 \(k_2\) 符号相反时可以得到先桶后枕的"胡须形"。FIG 3 里的棋盘格是真实算出来的：对输出图像的每个像素，用迭代去畸变求出它对应的归一化射线，再查表格上的颜色。

前向的 \(D\) 是一个多项式，随便算。可是图像里测到的是畸变后的点 \((x_d,y_d)\)，要做三角化、PnP、极线几何都需要无畸变的 \((x,y)\)，也就是解 \(D(x,y)=(x_d,y_d)\)。这是两个变量的高次方程组（\(k_3\ne0\) 时 \(r\) 的七次），没有闭式解，只能迭代。常见的三种：

• A. 残差修正：\(\mathbf x\leftarrow\mathbf x-\bigl(D(\mathbf x)-\mathbf x_d\bigr)\)，即把 \(D\) 近似成恒等映射。本课的 Vision.undistort 就是它。
• B. 不动点（OpenCV 的做法）：\(\mathbf x\leftarrow(\mathbf x_d-\Delta_{\text{tan}}(\mathbf x))/\rho(r^2)\)，其中 \(\rho=1+k_1r^2+k_2r^4+k_3r^6\)；OpenCV 的 undistortPoints 就是这类迭代，经典接口默认只迭代几次（5 次）。
• N. Newton：\(\mathbf x\leftarrow\mathbf x-J_D(\mathbf x)^{-1}(D(\mathbf x)-\mathbf x_d)\)，二次收敛，每步多算一个 \(2\times2\) 雅可比。

A 和 B 是线性收敛，收敛因子等于迭代映射的雅可比范数；N 则几步就到机器精度。FIG 4 把它们同时跑出来。这里还藏着一个重要的事实：当 \(k_1\) 很大时，\(r_d(r)\) 在 \(r^\ast\) 处到达最大值后往回走（FIG 3 右图的红点），超过它的 \(r_d\) 根本没有原像，迭代无论怎么做都收敛不到零残差。畸变系数是在标定图像覆盖的范围内拟合的，范围之外的"逆"没有意义。

针孔的像距满足 \(r=f\tan\theta\)（\(\theta\) 是光线与光轴的夹角）。\(\theta\to90^\circ\) 时 \(r\to\infty\)：针孔相机的视场角永远到不了 180°，而且越靠近极限边缘被拉伸得越厉害。鱼眼镜头换一种映射，让大角度的光线也能落进有限的传感器：最常用的等距模型 \(r=f\theta\)，还有立体投影 \(r=2f\tan(\theta/2)\)、等立体角 \(r=2f\sin(\theta/2)\)、正交 \(r=f\sin\theta\)。Kannala–Brandt 通用模型在等距模型上再加一个角度的多项式：\(r=f\,\theta_d\)，\(\theta_d=\theta(1+k_1\theta^2+k_2\theta^4+k_3\theta^6+k_4\theta^8)\)，OpenCV 的 fisheye 模块用的就是这个。

鱼眼图像不能先"去畸变"成针孔图再用：180° 的视场投到一个有限的平面上是不可能的，只能裁掉边缘，或者投到球面/立方体贴图上。实用的做法是直接在单位球上的射线里工作（11.1 节的对极几何可以写成射线的形式），或把鱼眼切成几块虚拟针孔相机。

\(\tilde{\mathbf u}=K\tilde{\mathbf x}_n\)，其中 \(\tilde{\mathbf x}_n=(x,y,1)^\top\) 是归一化坐标，\(\tilde{\mathbf u}=(u,v,1)^\top\)。K 是上三角矩阵，逆矩阵也是：\(K^{-1}=\begin{bmatrix}1/f_x & -s/(f_xf_y) & (sc_y-c_xf_y)/(f_xf_y)\\ 0&1/f_y&-c_y/f_y\\0&0&1\end{bmatrix}\)。所以像素 \(\to\) 射线也是线性的：\(\mathbf d\propto K^{-1}\tilde{\mathbf u}\)，射线上深度为 \(Z\) 的点是 \(\mathbf X_c=Z\,K^{-1}\tilde{\mathbf u}\)（\(Z\) 是沿光轴的深度，不是到相机中心的距离，见 FIG 6）。

5 个自由度。K 只有 5 个自由度：\(f_x,f_y,s,c_x,c_y\)，右下角固定为 1，因为 \(\tilde{\mathbf u}\) 是齐次坐标，整个矩阵乘一个常数不改变像素。为什么最后一行是 \((0,0,1)\)？因为相机坐标系的 z 轴就是光轴，齐次像素的第三个分量只是深度，不能混入 \(x,y\)。为什么左下的 \(K_{21}\) 也是 0？因为相机的 x 轴取得与传感器的像素行平行；剩下的任何转动都算进外参 \(R\)（这也是 RQ 分解唯一的原因）。

用 5.1、5.3 节的记号，\({}^{a}T_{b}\) 把 \(b\) 系里的坐标变到 \(a\) 系（\(\mathbf X_a={}^{a}T_{b}\mathbf X_b\)），相继变换按 1.4 节"从右往左"复合。相机位姿 \({}^{w}T_{c}\) 和外参 \({}^{c}T_{w}\) 互为逆：

相机中心为什么是 \(-R^\top\mathbf t\)。相机中心是投影不到任何地方的那个点：\(P\tilde C=\mathbf 0\)，即 \([R|\mathbf t]\tilde C=R\,C+\mathbf t=\mathbf 0\)，所以 \(C=-R^{-1}\mathbf t=-R^\top\mathbf t\)（\(R\) 正交）。\(\mathbf t\) 本身是世界原点在相机系里的坐标：\(R\cdot\mathbf 0+\mathbf t\)。只有 \(R=I\) 时 \(\mathbf t=-C\)。另外，\(R\) 的行是相机三根轴在世界系里的方向（\({}^{w}R_c=R^\top\) 的列）；第三行就是光轴方向。

装在机器人上的相机。外参往往是一串变换的复合：\({}^{c}T_{w}={}^{c}T_{b}\,{}^{b}T_{w}\)，其中 \({}^{b}T_{w}\) 是机器人的定位结果（或者臂的正运动学，7.3 节），\({}^{c}T_{b}\) 是相机相对机器人本体的安装位姿的逆，由手眼标定得到（10.4 节）。

\(P\) 是 \(3\times4\)，12 个数，整体乘常数不改变投影，所以最多 11 个自由度。数一下参数：K 有 5 个，\(R\) 有 3 个（SO(3) 的维数），\(\mathbf t\) 有 3 个，恰好 \(5+3+3=11\)：P 里没有多余的东西，也没有缺少的东西。这也是一个测得的 P 能用"RQ 分解"唯一地拆回 K、R、t 的原因（Hartley–Zisserman 第 6 章）。带畸变的完整模型是 \(\tilde{\mathbf u}=K\,D\bigl(\pi(\,{}^{c}T_{w}\tilde{\mathbf X}_w)\bigr)\)，\(\pi\) 是除以深度，D 在 \(\pi\) 之后、K 之前。

设新旧像素坐标之间是仿射关系 \(u'=a_xu-x_0\)，\(v'=a_yv-y_0\)（先缩放 \(a\) 倍，再把窗口左上角平移到原点）。写成齐次形式 \(\tilde{\mathbf u}'=A\tilde{\mathbf u}\)，那么 \(\tilde{\mathbf u}'=A\,K\,\tilde{\mathbf x}_n\)，也就是说新的内参是

归一化坐标和 D 都在 K 之前，不受影响。一个细节：本课（和 FIG 1）把像素当作面积，坐标原点在左上角像素的角上，理想相机的主点是 \((w/2,h/2)\)，缩放时 \(c'=a\,c\)。OpenCV 把整数坐标放在像素中心，理想主点是 \(((w-1)/2,(h-1)/2)\)，缩放变成 \(c'=a(c+\tfrac12)-\tfrac12\)；差半个像素，对精细的三维重建不是小事。

这是 Brown–Conrady 模型（OpenCV 的 plumb_bob；Conrady 1919 提出偏心畸变，Brown 1966 给出摄影测量中的公式）。OpenCV 把系数存成 \((k_1,k_2,p_1,p_2,k_3)\)，本课代码里的数组是 \([k_1,k_2,k_3,p_1,p_2]\)，换库时注意顺序。更复杂的变体包括有理模型（\(\rho\) 的分母再加 \(k_4r^2+k_5r^4+k_6r^6\)）、薄棱镜和倾斜传感器项。

径向部分对 \(r\) 求导：\(\dfrac{\mathrm d r_d}{\mathrm d r}=1+3k_1r^2+5k_2r^4+7k_3r^6\)。例如只有 \(k_1<0\) 时，导数在 \(r^\ast=1/\sqrt{3|k_1|}\) 处为零，\(r_d\) 的最大值是 \(r_{d,\max}=\tfrac23r^\ast\)；FIG 3 的红点和 FIG 4 的虚线圆就是它（\(k_1=-0.3\) 时 \(r^\ast\approx1.05\)，已经超出常见相机的图像范围，\(k_1=-0.9\) 时 \(r^\ast\approx0.61\)，图像里就能碰到）。折返点之外 D 不是单射，任何"逆"都不是唯一的，常见的办法是把去畸变限制在标定覆盖的半径之内。

Newton 迭代需要 \(J_D\)，它是 \(2\times2\) 矩阵，对 \(x_d\) 的两个分量求导即可得到。若需要对整个图像去畸变，则像上面的"视角"提示那样，预先算好查表。

这就是 FIG 6 的全部算法：像素中心 \((i+\tfrac12,j+\tfrac12)\) 先去畸变，再乘 \(Z\,K^{-1}\)，最后用相机位姿搬到世界系。大多数深度相机的深度图存的是 \(Z\)（沿光轴），有些传感器（激光雷达、部分 ToF）给的是距离 \(d\)，两者差一个因子 \(\lVert K^{-1}\tilde{\mathbf u}\rVert\)，把它们混用会让视野边缘的墙往外鼓。

与光轴夹角 \(\alpha\)（在 \(xz\) 平面内）的光线有 \(x=\tan\alpha\)，落在 \(u=f_x\tan\alpha+c_x\)。图像右边缘 \(u=w\) 对应 \(\tan\alpha_R=(w-c_x)/f_x\)，左边缘 \(u=0\) 对应 \(\tan\alpha_L=c_x/f_x\)。总视场角 \(\theta_h=\alpha_L+\alpha_R=\arctan\frac{c_x}{f_x}+\arctan\frac{w-c_x}{f_x}\)，主点居中时 \(c_x=w/2\)，化为 \(2\arctan\frac{w}{2f_x}\)。垂直方向同理；对角线 \(2\arctan\frac{\sqrt{w^2+h^2}}{2f}\)（\(f_x=f_y=f\)，主点居中）。鱼眼的等距模型 \(r=f\theta\) 给出 \(\theta_h=w/f\)（弧度），所以 \(f\le w/\pi\) 时视场角 \(\ge180^\circ\)。

一台机器人上同时有好几套相机坐标系的说法。OpenCV 与 ROS 的光学系（optical frame）：x 向右，y 向下，z 向前，本课一直用它。OpenGL（以及 Blender、glTF 里的相机）：x 向右，y 向上，相机看向 \(-z\)，所以和 OpenCV 差一个绕 x 轴的 180° 旋转 \(\mathrm{diag}(1,-1,-1)\)（Unity 还是左手系）。ROS 本体系（REP-103）：x 向前，y 向左，z 向上，相机的本体系 camera_link 沿用它，而 camera_..._optical_frame 才是光学系。从本体系到光学系是一个固定的旋转，URDF 里常写成 rpy="-π/2 0 -π/2"。它们三个都是同一个相机，只是轴的取法不同，换算与相机在哪里无关。

FIG 6 有两种模式。"坐标约定"把三套坐标系画在同一台相机上，并列出它们之间的固定旋转。"深度图 → 点云"是整课的综合检验：一个房间角（两面垂直的墙和地面）和一个球，从一台镜头带桶形畸变的深度相机看过去，得到 \(64\times48\) 的深度图。你来选择重建时用的 K，看点云是不是还贴着真墙（虚线是墙上的真实直线）。