第 1 章相机模型与标定（算法侧）¶

标定是把"像素"翻译成"米"的第一块基石：它产出一组内参数与畸变系数，让后续的对极几何、三角化、深度反投影都建立在正确的投影关系上。本章从算法侧讲清针孔与畸变模型为什么这样写、标定求解在什么条件下会退化、结果质量怎么判读；操作侧（ROS 2 camera_calibration 包、标定板摆放）承接 ROS2-note 第 8 章，此处不重复。代码段以 OpenCV 为主线（opencv-python-headless 5.0.0）。

从投影模型到可用内参的求解链：

flowchart LR
    pin["针孔模型<br/>K 内参"] --> dist["畸变模型<br/>径向 + 切向"]
    dist --> solve["标定求解<br/>角点 + 优化"]
    solve --> val["质量判读：重投影误差 / 退化条件"]
    val --> extr["外参与手眼标定"]

1. 针孔投影与齐次坐标¶

小孔成像的数学抽象是针孔模型（pinhole model）：物点先由外参（旋转 \(R\)、平移 \(t\)）从世界坐标变到相机坐标，再由内参矩阵 \(K\) 投影到像素平面。

\[ s\begin{bmatrix}u\\ v\\ 1\end{bmatrix} = K\,[R\,|\,t]\begin{bmatrix}X\\ Y\\ Z\\ 1\end{bmatrix},\quad K=\begin{bmatrix}f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1\end{bmatrix} \]

第三行除下来就是深度 \(Z\)，\(f_x=f/\,ds_x\) 是焦距以像素为单位（\(ds_x\) 像元尺寸）。齐次坐标把"除以 \(Z\)"的非线性藏在尺度因子 \(s\) 里，投影因此成为线性映射——这是第 2 章一切射影关系能写成矩阵乘积的前提。

cv2.projectPoints 就是这条链的实现。投影矩阵是 3×4 矩阵，共 12 个元素，去掉整体尺度后有 11 个自由度；理解"哪些参数耦合"直接决定第 3 节的退化判读。

2. 畸变模型¶

真实镜头不是小孔：光线在透镜边缘弯折更厉害，成像几何偏离针孔。OpenCV 的 Brown-Conrady 模型用径向 + 切向多项式描述这种偏移，作用在归一化平面坐标 \((x,y)=(X/Z,Y/Z)\) 上：

\[ x_{\text{dist}}=x(1+k_1r^2+k_2r^4+k_3r^6)+\underbrace{2p_1xy+p_2(r^2+2x^2)}_{\text{切向}} \]

\(r^2=x^2+y^2\)，径向畸变随离主点距离的平方增长——这解释了为什么画面边缘的直线弯得最狠。畸变位移是径向的，只与到光轴的距离有关：

import numpy as np, cv2
K = np.array([[600.,0,320.],[0,600.,240.],[0,0,1]])
D = np.array([0.10,-0.05,0.001,0.001,0.0])          # k1 k2 p1 p2 k3
P = np.random.default_rng(7).uniform([-0.5,-0.5,0.8],[0.5,0.5,1.5],(500,1,3))
u,_  = cv2.projectPoints(P, np.zeros(3), np.zeros(3), K, D)     # 加畸变
u0,_ = cv2.projectPoints(P, np.zeros(3), np.zeros(3), K, np.zeros(5))  # 不加
# 按离主点的环带统计畸变位移
edge  = np.hypot(*(u.reshape(-1,2)-[320,240]).T)
shift = np.hypot(*(u.reshape(-1,2)-u0.reshape(-1,2)).T)

同一组点在环带上的统计显示：距主点越远，畸变位移越大，边缘环带比中心高出约两个数量级。去畸变的反解 undistortPoints(u, K, D, R=K) 把像素映回无畸变像素系，与正向投影逐点对照的偏差在浮点精度内（远小于 0.01 px），即反解与正向投影自洽。

注意形参：undistortPoints 不传 R 时输出的是归一化坐标（\((x,y)\)），要回像素系须传 R=K。

鱼眼（wide field-of-view）镜头视场超过 100°，针孔的 \(r=f\tan\theta\) 在 \(\theta\to 90°\) 发散，改用等距投影（equidistant projection） \(r=f\theta\)。同一入射角下两种模型差多少：

Kf = np.array([[300,0,320],[0,300,240],[0,0,1]], np.float64)
for th_deg in (10, 30, 50):
    th = np.radians(th_deg)
    p = np.array([[[np.sin(th), 0, np.cos(th)]]], np.float64)  # (N,1,3)
    r_fish = ...  # cv2.fisheye.projectPoints，D=zeros(4,1)
    r_pin  = ...  # cv2.projectPoints

同一入射角下两模型的偏差：入射角 10° 时等距 \(r=f\theta=52.4\)px、针孔 \(r=f\tan\theta=52.9\)px，比值 1.010（几乎重合）；30° 时比值 1.103；50° 时 1.366——角越大针孔高估得越多。所以鱼眼必须用 cv2.fisheye 一套独立接口标定（输入形状 \((N,1,3)\)、\(D\) 为 \((4,1)\)），不能套用 Brown-Conrady。

3. 标定求解与退化判读¶

标定即在多组"已知 3D ↔ 观测 2D"对应下反解 \(K,D\) 与各帧 \((R,t)\)。OpenCV 的 calibrateCamera 用 Zhang 方法：先由每帧的平面单应估内参闭式解，再非线性最小二乘（LM）精修重投影误差。标定板的几何设计是为求解条件服务的：

为什么用棋盘格：角点是亚像素可精确定位的强特征，且格距已知给出 3D 标尺。
为什么要非共线、多姿态：所有标定板点共面（平面目标），单帧只能约束部分参数；焦距与到板距离之间存在耦合，只有让板从多个倾斜角度观测，才能把这条耦合脊压掉。

最小样（合成 12 姿态、真值 \(f_x=f_y=600, c_x=320, c_y=240, k_1=0.10\)，角点加 0.03px 噪声）：

rms, K2, D2, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, (640,480), None, None)

在这个最小样上，重投影 rms 落在角点噪声量级，回归出的内参与真值吻合——这是姿态充分多样时应达到的质量。

退化对照（同样噪声，只让标定板在自身平面内平移、姿态恒定）中，同一份数据多次取样得到的焦距会在数百像素的范围内乱跳，而重投影 rms 仍然很低——残差小不等于标定好：退化的解照样能把观测角点重投影得很准，但内参本身是错的。焦距不可辨的根源是：共面单姿态下，整体缩放（焦距与距离同乘一个因子）产生完全相同的像点。这也解释了「标定一次终身有效」为何不成立——除下节的物理因素外，几何采样不足本身就是系统性误差源。

物理侧的时变因素（温度导致镜筒热胀、机械形变使 \(c_x,c_y\) 漂移）是另一类"标定会过期"的缘由，属操作侧，回引 ROS2-note 第 8 章。

3.1 质量判读¶

重投影 rms：单位是像素，应接近角点定位噪声水平；过低（如 <0.05）要警惕是过拟合或退化，过高说明模型没吃满畸变或板面污染。逐帧 rms 分布比单一均值更能定位坏帧。
内参置信度：calibrateCamera 内部给出协方差，实践中更直接的检验是"换一批图像重标，内参是否稳定"——本节的退化对照正是这一思想的极端版。
\(k_1\) 符号：负值为桶形（枕形相反），量级异常大（\(|k_1|>1\)）往往提示标定板姿态不够或鱼眼误用了针孔模型。

3.2 角点检测的两条实现¶

cv2.findChessboardCorners（经典，基于鞍点）与 cv2.findChessboardCornersSB（基于 AprilTag 式标记，对透视和遮挡更稳）在同一张合成棋盘上的表现不同：强透视或局部遮挡时，经典接口可能漏检整板，SB 路径仍能给出角点。SB 的代价是对图案类型更挑剔，且返回 (retval, corners) 两元组。选标定板图像质量不足时，SB 路径更稳。

4. 外参与手眼标定¶

相机装到机械臂上，还要知道相机相对末端法兰的固定刚体变换 \(X\)——这就是手眼标定（hand-eye calibration），满足 \(A_i X = X B_i\)：\(A_i\) 是相邻两帧末端在基座系的相对运动，\(B_i\) 是相邻两帧标定板在相机系的相对运动（由每帧 solvePnP 得到）。

求解分两条路线：分离解（Park-Martin 1994，先用旋转部分 \([\text{skew}(\alpha_i+\beta_i)-\text{skew}(\alpha_i\times\beta_i)]\) 堆叠取零空间解转轴、再回代解平移）与 Tsai-Lenz 闭式解；也可在李代数 \(\mathfrak{se}(3)\) 上直接非线性最小二乘。三者的数学目标一致，差别在是否拆分旋转/平移、对噪声与激励的敏感度（Tsai-Lenz 与 Park-Martin 都要求激励中旋转轴不平行，见 control-note 第 7 章姿态表达）。

用真值 \(X\) 生成 12 组精确相对位姿、LM 求解：

def solve_axxb(A, B):
    # min Σ ‖log(A_i X B_i⁻¹ X⁻¹)‖²，初值单位阵
    ...
    sol = scipy.optimize.least_squares(cost, np.zeros(6), method="lm")

在理想数据上，旋转与平移残差、方程余数都落在机器精度量级——这只能说明手眼求解器实现无误；真实数据受每帧 PnP 噪声、时间同步、运动激励不足影响，残差会升到角分级/毫米级。AX=XB 的 Park-Martin 与 Tsai-Lenz 两种解法在噪声与激励敏感度上的差异标「待核实」。

5. 在线 / 自标定¶

固定安装、长期运行的相机系统会遇内参漂移（温漂、震动）。两条应对路线：

自标定（self-calibration）：不依赖标定板，用连续图像间的对极约束反推内参（承接第 2 章本质矩阵）。精度不如工厂标定，适合在线检测内参是否偏离、为 SLAM 提供初值。
在线校正：把标定结果当先验、在运行时以少量特征做温和微调，防止尺度/焦距长期漂移。

两者的适用面判据：任务对外参/内参绝对精度敏感（测量、抓取）时仍需周期性物理标定；仅要求相对几何一致（视觉里程计）时自标定更契合。完整推导超出本章。

6. 要点¶

针孔投影 = 外参 + 内参 \(K\)，齐次坐标把透视除法藏进尺度因子，投影因而线性。
畸变位移沿径向、随离光轴距离平方增长；鱼眼用等距模型 \(r=f\theta\)，大入射角下针孔系统性高估。
标定板的非共线多姿态是为压掉"焦距-距离"耦合脊；退化时残差照样小但内参发散，残差小不等于标定好。
手眼 \(AX=XB\) 有分离解（Park-Martin / Tsai-Lenz）与 \(\mathfrak{se}(3)\) 直接最小二乘两族，理想数据可解到机器精度。
findChessboardCornersSB 返回 (retval, corners)；undistortPoints 不传 R 时输出归一化坐标，回像素系须传 R=K。

参考文献与延伸阅读¶

Szeliski, R. Computer Vision: Algorithms and Applications (2nd ed.). Springer, 2022. 书籍主页
Hartley, R., & Zisserman, A. Multiple View Geometry in Computer Vision (2nd ed.). Cambridge University Press, 2004. DOI
Zhang, Z. A Flexible New Technique for Camera Calibration. IEEE TPAMI, 2000. DOI
OpenCV. Camera Calibration 教程. 文档

第 1 章 相机模型与标定（算法侧）¶