相机内参估计：从针孔模型到 Zhang 标定法

1. 标定目标

相机内参矩阵为

\[ K=\begin{bmatrix}f_x&s&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}, \]

其中 \(f_x,f_y\) 是像素焦距，\((c_x,c_y)\) 是主点，\(s\) 是坐标轴倾斜量（通常取 0）。真实镜头还需要畸变参数

\[ D=(k_1,k_2,p_1,p_2,k_3,\ldots). \]

\(K\) 不包含畸变：它描述理想针孔投影；\(D\) 描述镜头对理想模型的偏离。

2. 针孔投影与齐次尺度

世界点变换到相机坐标系：

\[ P_c=RP_w+t=(X_c,Y_c,Z_c)^T. \]

忽略畸变时，

\[ \lambda\begin{bmatrix}u\\v\\1\end{bmatrix} =K\begin{bmatrix}X_c\\Y_c\\Z_c\end{bmatrix} =K[R\mid t]\begin{bmatrix}X\\Y\\Z\\1\end{bmatrix}. \]

展开可得

\[ u=f_x\frac{X_c}{Z_c}+s\frac{Y_c}{Z_c}+c_x, \qquad v=f_y\frac{Y_c}{Z_c}+c_y. \]

在标准针孔模型中 \(lambda=Z_c\)。写成 \(lambda\) 是为了强调齐次坐标只确定到非零比例。投影会丢失深度：同一条经过光心的射线上的三维点投影到同一像素；像素加深度才能唯一恢复空间点。

当 \(s=0\) 且像素已去畸变时，反投影为

\[ X_c=\frac{u-c_x}{f_x}Z_c, \qquad Y_c=\frac{v-c_y}{f_y}Z_c. \]

3. 平面标定板与单应矩阵

把棋盘格坐标系定义在标定板平面上，则所有角点满足 \(Z=0\)。代入投影模型：

\[ \lambda\begin{bmatrix}u\\v\\1\end{bmatrix} =K\begin{bmatrix}r_1&r_2&r_3&t\end{bmatrix} \begin{bmatrix}X\\Y\\0\\1\end{bmatrix} =K\begin{bmatrix}r_1&r_2&t\end{bmatrix} \begin{bmatrix}X\\Y\\1\end{bmatrix}. \]

定义

\[ \boxed{H=K[r_1\ r_2\ t]}, \]

便得到平面单应关系

\[ \boxed{\lambda\begin{bmatrix}u\\v\\1\end{bmatrix} =H\begin{bmatrix}X\\Y\\1\end{bmatrix}}. \]

标定板角点 \((X_i,Y_i)\) 已知，图像角点 \((u_i,v_i)\) 可检测，因此每张图都能估计一个 \(H\)。理论上至少需要四个不共线点，实际使用大量角点做最小二乘估计。

4. 从 \(H\) 约束内参

将 \(H\) 按列写成 \(H=[h_1\ h_2\ h_3]\)。由 \(H=K[r_1\ r_2\ t]\) 可得：

\[ h_1=Kr_1,\qquad h_2=Kr_2,\qquad h_3=Kt. \]

所以

\[ r_1=K^{-1}h_1, \qquad r_2=K^{-1}h_2. \]

严格来说，单应矩阵只确定到一个整体比例；但这个比例会同时出现在 \(h_1,h_2,h_3\) 中，并在下面的正交、等长约束里消去，因此推导时可以省略，不必再引入新的符号。

旋转矩阵的列正交且等长：

\[ r_1^Tr_2=0, \qquad r_1^Tr_1=r_2^Tr_2=1. \]

代入上式并消去尺度：

\[ \boxed{h_1^TK^{-T}K^{-1}h_2=0}, \]

\[ \boxed{h_1^TK^{-T}K^{-1}h_1 =h_2^TK^{-T}K^{-1}h_2}. \]

定义对称矩阵

\[ B=K^{-T}K^{-1} =\begin{bmatrix} B_{11}&B_{12}&B_{13}\\ B_{12}&B_{22}&B_{23}\\ B_{13}&B_{23}&B_{33} \end{bmatrix}. \]

于是每张图提供两条约束：

\[ h_1^TBh_2=0, \qquad h_1^TBh_1-h_2^TBh_2=0. \]

5. 展开为 \(Vb=0\)

令 \(h_i=(h_{1i},h_{2i},h_{3i})^T\)，定义

\[ v_{ij}=\begin{bmatrix} h_{1i}h_{1j}\\ h_{1i}h_{2j}+h_{2i}h_{1j}\\ h_{2i}h_{2j}\\ h_{3i}h_{1j}+h_{1i}h_{3j}\\ h_{3i}h_{2j}+h_{2i}h_{3j}\\ h_{3i}h_{3j} \end{bmatrix}, \quad b=\begin{bmatrix}B_{11}&B_{12}&B_{22}&B_{13}&B_{23}&B_{33}\end{bmatrix}^T. \]

则

\[ h_i^TBh_j=v_{ij}^Tb. \]

一张图的两行线性约束为

\[ \begin{bmatrix}v_{12}^T\\(v_{11}-v_{22})^T\end{bmatrix}b=0. \]

把所有图像的约束堆叠起来：

\[ \boxed{Vb=0}. \]

用 SVD 取 \(V\) 最小奇异值对应的右奇异向量，即得到只差整体尺度的 \(b\)。

\(B\) 有 6 个独立元素，但整体尺度不确定，本质上只有 5 个自由度。每张图给两条约束，因此理论上至少需要 3 张姿态不同且不退化的标定图；实践中需要更多图片抵抗检测误差与噪声。

6. 从 \(B\) 恢复 \(K\) 和外参

由

\[ B=K^{-T}K^{-1} \]

可得

\[ B^{-1}=KK^T. \]

对 \(B^{-1}\) 做 Cholesky 分解并按 \(K_{33}=1\) 归一化，即可恢复上三角矩阵 \(K\)；也可使用 Zhang 论文中的显式公式。

得到 \(K\) 后，每张图的外参初值为

\[ \text{scale}=\frac1{\lVert K^{-1}h_1\rVert}, \]

\[ r_1=\text{scale}\,K^{-1}h_1, \quad r_2=\text{scale}\,K^{-1}h_2, \quad r_3=r_1\times r_2, \quad t=\text{scale}\,K^{-1}h_3. \]

噪声会使 \(R=[r_1\ r_2\ r_3]\) 不严格正交，通常再通过 SVD 将它投影到最近的旋转矩阵。

7. 加入镜头畸变

Zhang 法的线性初始化近似使用无畸变针孔模型，完整标定随后加入畸变。先计算归一化坐标：

\[ x=\frac{X_c}{Z_c},\qquad y=\frac{Y_c}{Z_c},\qquad r^2=x^2+y^2. \]

常见径向加切向畸变模型为

\[ x_d=x(1+k_1r^2+k_2r^4+k_3r^6) +2p_1xy+p_2(r^2+2x^2), \]

\[ y_d=y(1+k_1r^2+k_2r^4+k_3r^6) +p_1(r^2+2y^2)+2p_2xy. \]

最后映射到像素：

\[ u=f_xx_d+sy_d+c_x, \qquad v=f_yy_d+c_y. \]

完整成像链路是

\[ \boxed{P_w\rightarrow(R_i,t_i)\rightarrow(x,y) \rightarrow D\rightarrow K\rightarrow(u,v)}. \]

8. 联合非线性优化

线性推导给出 \(K\) 和各图外参的初值。随后把内参、畸变和每张图的外参一起优化，最小化所有角点的重投影误差：

\[ \boxed{ \min_{K,D,R_i,t_i}\sum_{i,j} \left\lVert p_{ij}^{\mathrm{obs}} -p_{ij}^{\mathrm{pred}}(K,D,R_i,t_i)\right\rVert^2}. \]

因此，\(K\) 本身不包含畸变，但最终的 \(K\) 是在考虑畸变后联合优化得到的。完整 Zhang 标定并非假设真实相机完全无畸变，而是“用理想模型求初值，再加入畸变并精化”。

9. 为什么需要多姿态图像

不同倾角产生不同的 \(H_i\)，为 \(K\) 提供相互独立的约束。若所有图片都近似正视标定板，只做前后平移，约束容易退化，焦距、主点和外参难以稳定区分。

畸变在图像边缘通常更明显，因此标定板还应覆盖中心、四角和边缘。可靠的数据集应做到：

• 左右、上下倾斜，角度丰富；
• 覆盖中心、四角与边缘；
• 距离有变化，同时保证角点清晰；
• 避免大量几乎重复的姿态。

重点不是图片越多越好，而是约束是否丰富。

10. OpenCV 中的实现

ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
    object_points,
    image_points,
    image_size,
    None,
    None,
)

• K：最终内参矩阵；
• dist：畸变参数；
• rvecs, tvecs：每张图的外参；
• ret：总体重投影误差指标。

整个流程可概括为

\[ \boxed{ \text{角点对应} \rightarrow H_i \rightarrow Vb=0 \rightarrow K_{\mathrm{init}} \rightarrow(R_i,t_i)_{\mathrm{init}} \rightarrow\text{加入畸变} \rightarrow\text{联合非线性优化}} \]

对于已完成出厂标定的 RGB-D 相机，通常优先通过厂商 SDK 读取 RGB、Depth 的 \(K,D\) 以及二者间外参；只有在精度验证、镜头改装或标定数据失效时才需要重新标定。