相机内参初始值估计的解析解推导【简洁明了】（cvInitIntrinsicParams2D）
相机内参初始值估计的解析解推导【简洁明了】（cvInitIntrinsicParams2D）
这是一个基于**张正友标定法（Zhang’s Method）**的解析解推导过程。其目标是在已知主点 (cx,cy)(c_x, c_y)(cx,cy) 且假设相机偏斜（skew）为 0 的条件下，求解相机的焦距 fxf_xfx 和 fyf_yfy。
1. 基本模型与单应性
相机针孔模型将一个世界坐标点 Mw=[X,Y,Z,1]TM_w = [X, Y, Z, 1]^TMw=[X,Y,Z,1]T 投影到图像点 m=[u,v,1]Tm = [u, v, 1]^Tm=[u,v,1]T：
s⋅m=K[R∣t]Mws \cdot m = K [R | t] M_ws⋅m=K[R∣t]Mw
其中 KKK 是内参矩阵，[R∣t][R | t][R∣t] 是外参矩阵。
假设标定板在世界坐标系的 Z=0Z=0Z=0 平面上，则 MwM_wMw 可简化为 M=[X,Y,1]TM = [X, Y, 1]^TM=[X,Y,1]T。投影关系简化为：
s[uv1]=K[r1 r2 t][XY1] s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K [r_1 \ r_2 \ t] \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix}suv1=K[r1 r2 t]XY1
其中 r1,r2r_1, r_2r1,r2 是旋转矩阵 RRR 的前两列。
这个 3×33 \times 33×3 的变换矩阵 H=K[r1 r2 t]H = K [r_1 \ r_2 \ t]H=K[r1 r2 t] 被称为单应性矩阵（Homography）。它可以从 MMM 和 mmm 的对应点对中计算出来（通常会有一个尺度因子 λ\lambdaλ）：
H=λK[r1 r2 t](1)H = \lambda K [r_1 \ r_2 \ t] \quad (1)H=λK[r1 r2 t](1)
2. 简化内参矩阵
根据我们的假设：
- 主点 (cx,cy)(c_x, c_y)(cx,cy) 已知。
- 相机无偏斜（s=0s=0s=0）。
KKK 矩阵可以写作：
K=[fx0cx0fycy001]K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}K=fx000fy0cxcy1
我们可以将其分解为 K=KcKfK = K_c K_fK=KcKf：
K=[10cx01cy001]⏟Kc[fx000fy0001]⏟KfK = \underbrace{\begin{bmatrix} 1 & 0 & c_x \\ 0 & 1 & c_y \\ 0 & 0 & 1 \end{bmatrix}}_{K_c} \underbrace{\begin{bmatrix} f_x & 0 & 0 \\ 0 & f_y & 0 \\ 0 & 0 & 1 \end{bmatrix}}_{K_f}K=Kc100010cxcy1Kffx000fy0001
我们定义一个“归一化”的单应性矩阵 H′H'H′，它将世界坐标 MMM 映射到以主点为原点的图像坐标 [u−cx,v−cy,1]T[u-c_x, v-c_y, 1]^T[u−cx,v−cy,1]T。
这个 H′H'H′ 与我们从像素坐标计算出的 HHH 之间的关系是：
H=K[r1 r2 t]=(KcKf)[r1 r2 t]=Kc(Kf[r1 r2 t])H = K [r_1 \ r_2 \ t] = (K_c K_f) [r_1 \ r_2 \ t] = K_c (K_f [r_1 \ r_2 \ t])H=K[r1 r2 t]=(KcKf)[r1 r2 t]=Kc(Kf[r1 r2 t])
令 H′=Kf[r1 r2 t]H' = K_f [r_1 \ r_2 \ t]H′=Kf[r1 r2 t]，则 H=KcH′H = K_c H'H=KcH′。
因此，我们可以通过 HHH 计算 H′H'H′：
H′=Kc−1H=[10−cx01−cy001]HH' = K_c^{-1} H = \begin{bmatrix} 1 & 0 & -c_x \\ 0 & 1 & -c_y \\ 0 & 0 & 1 \end{bmatrix} HH′=Kc−1H=100010−cx−cy1H
这个操作等价于将 HHH 的前两行分别减去 cxc_xcx 和 cyc_ycy 乘以第三行。
令 H′H'H′ 的列向量为 h1′,h2′,h3′h'_1, h'_2, h'_3h1′,h2′,h3′。根据 H′=λKf[r1 r2 t]H' = \lambda K_f [r_1 \ r_2 \ t]H′=λKf[r1 r2 t]（我们把 λ\lambdaλ 吸收进 H′H'H′）：
[h1′ h2′ h3′]=[fx000fy0001][r1 r2 t][h'_1 \ h'_2 \ h'_3] = \begin{bmatrix} f_x & 0 & 0 \\ 0 & f_y & 0 \\ 0 & 0 & 1 \end{bmatrix} [r_1 \ r_2 \ t][h1′ h2′ h3′]=fx000fy0001[r1 r2 t]
我们得到 h1′h'_1h1′ 和 h2′h'_2h2′ 与 r1,r2r_1, r_2r1,r2 的关系（其中 rijr_{ij}rij 是 RRR 的元素）：
- h1′=[fxr11,fyr21,r31]Th'_1 = [f_x r_{11}, f_y r_{21}, r_{31}]^Th1′=[fxr11,fyr21,r31]T
- h2′=[fxr12,fyr22,r32]Th'_2 = [f_x r_{12}, f_y r_{22}, r_{32}]^Th2′=[fxr12,fyr22,r32]T
我们可以反解 r1r_1r1 和 r2r_2r2（其中 hij′h'_{ij}hij′ 是 H′H'H′ 矩阵第 iii 行第 jjj 列的元素）：
r1=[h11′/fxh21′/fyh31′](2)r_1 = \begin{bmatrix} h'_{11}/f_x \\ h'_{21}/f_y \\ h'_{31} \end{bmatrix} \quad (2)r1=h11′/fxh21′/fyh31′(2)
r2=[h12′/fxh22′/fyh32′](3)r_2 = \begin{bmatrix} h'_{12}/f_x \\ h'_{22}/f_y \\ h'_{32} \end{bmatrix} \quad (3)r2=h12′/fxh22′/fyh32′(3)
3. 利用旋转矩阵的正交约束
旋转矩阵 RRR 的列向量 r1r_1r1 和 r2r_2r2 是正交的单位向量，因此它们满足：
- 正交性：r1Tr2=0r_1^T r_2 = 0r1Tr2=0
- 单位长度：r1Tr1=r2Tr2=1r_1^T r_1 = r_2^T r_2 = 1r1Tr1=r2Tr2=1 （我们这里使用 r1Tr1=r2Tr2r_1^T r_1 = r_2^T r_2r1Tr1=r2Tr2）
4. 建立线性方程组
我们定义两个未知数，令 F0=1/fx2F_0 = 1/f_x^2F0=1/fx2 和 F1=1/fy2F_1 = 1/f_y^2F1=1/fy2。
约束 1: r1Tr2=0r_1^T r_2 = 0r1Tr2=0
将 (2) 和 (3) 代入：
h11′h12′fx2+h21′h22′fy2+h31′h32′=0\frac{h'_{11} h'_{12}}{f_x^2} + \frac{h'_{21} h'_{22}}{f_y^2} + h'_{31} h'_{32} = 0fx2h11′h12′+fy2h21′h22′+h31′h32′=0
整理为 F0,F1F_0, F_1F0,F1 的线性方程：
(h11′h12′)F0+(h21′h22′)F1=−h31′h32′(A)(h'_{11} h'_{12}) F_0 + (h'_{21} h'_{22}) F_1 = -h'_{31} h'_{32} \quad \text{(A)}(h11′h12′)F0+(h21′h22′)F1=−h31′h32′(A)
约束 2: r1Tr1=r2Tr2r_1^T r_1 = r_2^T r_2r1Tr1=r2Tr2
将 (2) 和 (3) 代入：
h11′2fx2+h21′2fy2+h31′2=h12′2fx2+h22′2fy2+h32′2\frac{h'^{2}_{11}}{f_x^2} + \frac{h'^{2}_{21}}{f_y^2} + h'^{2}_{31} = \frac{h'^{2}_{12}}{f_x^2} + \frac{h'^{2}_{22}}{f_y^2} + h'^{2}_{32}fx2h11′2+fy2h21′2+h31′2=fx2h12′2+fy2h22′2+h32′2
将 F0,F1F_0, F_1F0,F1 的项移到左边，常数项移到右边：
(h11′2−h12′2)1fx2+(h21′2−h22′2)1fy2=h32′2−h31′2(h'^{2}_{11} - h'^{2}_{12}) \frac{1}{f_x^2} + (h'^{2}_{21} - h'^{2}_{22}) \frac{1}{f_y^2} = h'^{2}_{32} - h'^{2}_{31}(h11′2−h12′2)fx21+(h21′2−h22′2)fy21=h32′2−h31′2
整理为 F0,F1F_0, F_1F0,F1 的线性方程：
(h11′2−h12′2)F0+(h21′2−h22′2)F1=−(h31′2−h32′2)(B)(h'^{2}_{11} - h'^{2}_{12}) F_0 + (h'^{2}_{21} - h'^{2}_{22}) F_1 = -(h'^{2}_{31} - h'^{2}_{32}) \quad \text{(B)}(h11′2−h12′2)F0+(h21′2−h22′2)F1=−(h31′2−h32′2)(B)
（注： hij′h'_{ij}hij′ 是矩阵 H′=Kc−1HH' = K_c^{-1} HH′=Kc−1H 的元素）
5. 求解
对于每一个（第 iii 个）标定板姿态，我们都可以计算一个 Hi′H'_iHi′，并得到两个线性方程 (A) 和 (B)。
如果我们有 NNN 个姿态，我们就得到 2N2N2N 个关于 F0F_0F0 和 F1F_1F1 的方程。我们将它们组合成一个超定线性方程组 A⋅x=bA \cdot x = bA⋅x=b：
[(h11′h12′)1(h21′h22′)1(h11′2−h12′2)1(h21′2−h22′2)1⋮⋮(h11′h12′)N(h21′h22′)N(h11′2−h12′2)N(h21′2−h22′2)N][F0F1]=[(−h31′h32′)1−(h31′2−h32′2)1⋮(−h31′h32′)N−(h31′2−h32′2)N]\begin{bmatrix} (h'_{11} h'_{12})_1 & (h'_{21} h'_{22})_1 \\ (h'^{2}_{11} - h'^{2}_{12})_1 & (h'^{2}_{21} - h'^{2}_{22})_1 \\ \vdots & \vdots \\ (h'_{11} h'_{12})_N & (h'_{21} h'_{22})_N \\ (h'^{2}_{11} - h'^{2}_{12})_N & (h'^{2}_{21} - h'^{2}_{22})_N \end{bmatrix} \begin{bmatrix} F_0 \\ F_1 \end{bmatrix} = \begin{bmatrix} (-h'_{31} h'_{32})_1 \\ -(h'^{2}_{31} - h'^{2}_{32})_1 \\ \vdots \\ (-h'_{31} h'_{32})_N \\ -(h'^{2}_{31} - h'^{2}_{32})_N \end{bmatrix}(h11′h12′)1(h11′2−h12′2)1⋮(h11′h12′)N(h11′2−h12′2)N(h21′h22′)1(h21′2−h22′2)1⋮(h21′h22′)N(h21′2−h22′2)N[F0F1]=(−h31′h32′)1−(h31′2−h32′2)1⋮(−h31′h32′)N−(h31′2−h32′2)N
通过最小二乘法（例如 SVD）求解这个 2N×22N \times 22N×2 的方程组，得到 F0F_0F0 和 F1F_1F1 的最优解。
6. 恢复焦距
最后，从解 F0F_0F0 和 F1F_1F1 中恢复焦距：
fx=1/F0f_x = \sqrt{1 / F_0}fx=1/F0
fy=1/F1f_y = \sqrt{1 / F_1}fy=1/F1