opencv焦距估计函数cvInitIntrinsicParams2D原理解析

背景介绍

在进行相机标定时，通常会包括内参初始化估计、外参初始化估计，以及非线性优化参数。在opencv中内参数估计主要在cvInitIntrinsicParams2D函数中实现，外参数估计在cvFindExtrinsicCameraParams2函数中实现。本文对内参数估计原理进行解析，外参数估计在我之前的博客中已经做了较为全面的原理解析。

算法原理

opencv中对内参矩阵的定义为

式中的s表示CCD传感器横纵分布不垂直程度的系数，也就是传感器理想是矩形，但其实可能是具有剪切变形，是直角边不垂直的平行四边形。而Cx和Cy表示主点在图像中的位置，包含了主点偏差。在标定初始时，通常是考虑所有畸变量为零，也就是说此时的内参矩阵为

[ f x 0 u 0 0 f y v 0 0 0 1 ] (1) \begin{bmatrix} f_x & 0 & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1\end{bmatrix}\tag{1} ⎣ ⎡​fx​00​0fy​0​u0​v0​1​⎦ ⎤​(1)式中， u 0 , v 0 u_0, v_0 u0​,v0​为图像宽度和高度的一半，单位为像素，含义是从像素坐标系转换到像平面坐标的偏移量。cvInitIntrinsicParams2D就是用于估计式（1）中焦距的值，前提是用于平面标定板。

参考张正友的文献【1】 假设世界坐标系在平面标定板上，Z轴垂直于标定平面，那么标定板上点的Z=0。根据投影仪方程表达为 λ [ u v 1 ] = A [ r 1 r 2 r 3 t ] [ X Y 0 1 ] = A [ r 1 r 2 t ] [ X Y 1 ] (2) \lambda \begin{bmatrix} u \\ v\\ 1\end{bmatrix}=\bf{A} \begin{bmatrix} \bf{r_1} & \bf{r_2}&\bf{r_3} &\bf{t}\end{bmatrix} \begin{bmatrix} X \\ Y\\0\\ 1\end{bmatrix}=\bf{A} \begin{bmatrix} \bf{r_1} & \bf{r_2}&\bf{t}\end{bmatrix} \begin{bmatrix} X \\ Y\\1\end{bmatrix}\tag{2} λ⎣ ⎡​uv1​⎦ ⎤​=A[r1​​r2​​r3​​t​]⎣ ⎡​XY01​⎦ ⎤​=A[r1​​r2​​t​]⎣ ⎡​XY1​⎦ ⎤​(2)式中， A \bf{A} A表示式（1）的内参矩阵， r 1 , r 2 , r 3 , t \bf{r_1}, \bf{r_2},\bf{r_3} ,\bf{t} r1​,r2​,r3​,t是外参矩阵中的列向量。由于标定板上三维点和图像上二维点各自都近似在平面上，利用平面单应性关系有 λ [ u v 1 ] = H [ X Y 1 ] (3) \lambda \begin{bmatrix} u \\ v\\ 1\end{bmatrix} = \bf{H}\begin{bmatrix} X \\ Y\\ 1\end{bmatrix}\tag{3} λ⎣ ⎡​uv1​⎦ ⎤​=H⎣ ⎡​XY1​⎦ ⎤​(3)在每个拍摄角度下，都能解算出一个单应性矩阵H，根据式（2）和式（3）可知， H = [ H 1 , H 2 , H 3 ] = λ A [ r 1 r 2 t ] (4) \bf{H} = \begin{bmatrix} \bf{H_1},\bf{H_2},\bf{H_3}\end{bmatrix} = \lambda \bf{A} \begin{bmatrix} \bf{r_1} & \bf{r_2}&\bf{t}\end{bmatrix} \tag{4} H=[H1​,H2​,H3​​]=λA[r1​​r2​​t​](4)式中，H1，H2和H3表示单应性矩阵的三列，特别注意的是，后面的符号中， H 12 H_{12} H12​不是表示的H矩阵第1行2列的元素，而是列向量 H 1 \bf{H_1} H1​中的第二个元素，在矩阵中的位置是第1列第2行，这里和常规的表示不太一样。

根据式（4）可以得到外参矩阵中的列向量r1、r2，根据旋转矩阵单位正交的性质可知

r 1 T ⋅ r 2 = 0 (5) \bf{r_1}^T \cdot \bf{r_2}=0 \tag{5} r1​T⋅r2​=0(5) r 1 T ⋅ r 1 = r 2 T ⋅ r 2 = 1 (6) \bf{r_1}^T \cdot \bf{r_1}= \bf{r_2}^T \cdot \bf{r_2} = 1\tag{6} r1​T⋅r1​=r2​T⋅r2​=1(6)根据式（4）得到r1和r2，并带入上面两个式子得到

( A − 1 H 1 ) T ⋅ ( A − 1 H 2 ) = 0 (7) (\bf{A}^{-1}\bf{H_1})^T \cdot (\bf{A}^{-1}\bf{H_2})=0\tag{7} (A−1H1​)T⋅(A−1H2​)=0(7) ( A − 1 H 1 ) T ⋅ ( A − 1 H 1 ) = ( A − 1 H 2 ) T ⋅ ( A − 1 H 2 ) (8) (\bf{A}^{-1}\bf{H_1})^T \cdot (\bf{A}^{-1}\bf{H_1})= (\bf{A}^{-1}\bf{H_2})^T \cdot (\bf{A}^{-1}\bf{H_2})\tag{8} (A−1H1​)T⋅(A−1H1​)=(A−1H2​)T⋅(A−1H2​)(8)进一步简化得到 H 1 T A − T A − 1 H 2 = 0 (9) \bf{H_1^T } \bf{A}^{-T} \bf{A}^{-1}\bf{H_2}=0\tag{9} H1T​A−TA−1H2​=0(9) H 1 T A − T A − 1 H 1 = H 2 T A − T A − 1 H 2 (10) \bf{H_1^T } \bf{A}^{-T} \bf{A}^{-1}\bf{H_1} = \bf{H_2^T } \bf{A}^{-T} \bf{A}^{-1}\bf{H_2} \tag{10} H1T​A−TA−1H1​=H2T​A−TA−1H2​(10)式中， A − T \bf{A}^{-T} A−T表示 ( A − 1 ) T (\bf{A}^{-1})^T (A−1)T. 接下来就是利用式（9）和（10）来解算焦距 f x , f y f_x,f_y fx​,fy​。为了和代码对应，接下来的推导过程可能比较慢，公式比较长，这一段其实就是上述两个式子进一步带入具体的元素，来构建 A x = b Ax=b Ax=b的解算式。

文献【1】中令

在所有畸变量初始为零的情况下，上式简化为 B = [ 1 α 2 0 − u 0 α 2 0 1 β 2 − v 0 β 2 − u 0 α 2 − v 0 β 2 u 0 2 α 2 + v 0 2 β 2 + 1 ] (11) B=\begin{bmatrix} \frac{1}{\alpha^2} & 0 & -\frac{u_0}{\alpha^2} \\ 0 & \frac{1}{\beta^2} & -\frac{v_0}{\beta^2} \\ -\frac{u_0}{\alpha^2} & -\frac{v_0}{\beta^2} & \frac{u_0^2}{\alpha^2}+ \frac{v_0^2}{\beta^2}+1\end{bmatrix}\tag{11} B=⎣ ⎡​α21​0−α2u0​​​0β21​−β2v0​​​−α2u0​​−β2v0​​α2u02​​+β2v02​​+1​⎦ ⎤​(11)论文中 α , β \alpha,\beta α,β对应我这里的 f x , f y f_x,f_y fx​,fy​，为了后面推导表达方便，令 B 1 = 1 α 2 , B 2 = 1 β 2 B_1=\frac{1}{\alpha^2},B_2=\frac{1}{\beta^2} B1​=α21​,B2​=β21​, 下面分两部分分别对式（9）和式（10）详细展开

式（9）的展开

[ H 11 H 12 H 13 ] [ B 1 0 − u 0 B 1 0 B 2 − v 0 B 2 − u 0 B 1 − v 0 B 2 u 0 2 B 1 + v 0 2 B 2 + 1 ] [ H 21 H 22 H 23 ] = 0 (12) \begin{bmatrix} H_{11} & H_{12}& H_{13} \end{bmatrix} \begin{bmatrix} B_1 & 0 & -u_0B_1 \\ 0 &B_2 & -v_0B_2 \\ -u_0B_1 & -v_0B_2 & u_0^2B_1+v_0^2B_2+1\end{bmatrix} \begin{bmatrix} H_{21} \\ H_{22}\\H_{23} \end{bmatrix}=0\tag{12} [H11​​H12​​H13​​]⎣ ⎡​B1​0−u0​B1​​0B2​−v0​B2​​−u0​B1​−v0​B2​u02​B1​+v02​B2​+1​⎦ ⎤​⎣ ⎡​H21​H22​H23​​⎦ ⎤​=0(12) [ H 11 B 1 − u 0 H 13 B 1 H 12 B 2 − v 0 H 13 B 2 − u 0 H 11 B 1 − v 0 H 12 B 2 + u 0 2 H 13 B 1 + v 0 2 H 13 B 2 + H 13 ] [ H 21 H 22 H 23 ] = 0 (13) \begin{bmatrix} H_{11}B_1-u_0H_{13}B_1 & H_{12}B_2-v_0H_{13}B_2&-u_0H_{11}B_1-v_0H_{12}B_2+u_0^2H_{13}B_1+v_0^2H_{13}B_2+H_{13}\end{bmatrix} \begin{bmatrix} H_{21} \\ H_{22}\\H_{23} \end{bmatrix}=0\tag{13} [H11​B1​−u0​H13​B1​​H12​B2​−v0​H13​B2​​−u0​H11​B1​−v0​H12​B2​+u02​H13​B1​+v02​H13​B2​+H13​​]⎣ ⎡​H21​H22​H23​​⎦ ⎤​=0(13) ( H 21 H 11 B 1 − u 0 H 21 H 13 B 1 ) + ( H 22 H 12 B 2 − v 0 H 22 H 13 B 2 ) + ( − u 0 H 23 H 11 B 1 − v 0 H 23 H 12 B 2 + u 0 2 H 23 H 13 B 1 + v 0 2 H 23 H 13 B 2 + H 23 H 13 ) = 0 (14) (H_{21}H_{11}B_1-u_0H_{21}H_{13}B_1) \\ + (H_{22}H_{12}B_2-v_0H_{22}H_{13}B_2) \\ + (-u_0H_{23}H_{11}B_1-v_0H_{23}H_{12}B_2+u_0^2H_{23}H_{13}B_1+v_0^2H_{23}H_{13}B_2+H_{23}H_{13})=0\tag{14} (H21​H11​B1​−u0​H21​H13​B1​)+(H22​H12​B2​−v0​H22​H13​B2​)+(−u0​H23​H11​B1​−v0​H23​H12​B2​+u02​H23​H13​B1​+v02​H23​H13​B2​+H23​H13​)=0(14) B 1 ( H 21 H 11 − u 0 H 21 H 13 − u 0 H 23 H 11 + u 0 2 H 23 H 13 ) + B 2 ( H 22 H 12 − v 0 H 22 H 13 − v 0 H 23 H 12 + v 0 2 H 23 H 13 ) + H 23 H 13 = 0 (15) B_1(H_{21}H_{11}-u_0H_{21}H_{13}-u_0H_{23}H_{11}+u_0^2H_{23}H_{13}) \\ +B_2 (H_{22}H_{12}-v_0H_{22}H_{13}-v_0H_{23}H_{12}+v_0^2H_{23}H_{13}) \\ +H_{23}H_{13}=0\tag{15} B1​(H21​H11​−u0​H21​H13​−u0​H23​H11​+u02​H23​H13​)+B2​(H22​H12​−v0​H22​H13​−v0​H23​H12​+v02​H23​H13​)+H23​H13​=0(15) B 1 ( H 11 − u 0 H 13 ) ( H 21 − u 0 H 23 ) + B 2 ( H 12 − v 0 H 13 ) ( H 22 − v 0 H 23 ) + H 23 H 13 = 0 (16) B_1(H_{11}-u_0H_{13})(H_{21}-u_0H_{23}) \\ + B_2(H_{12}-v_0H_{13})(H_{22}-v_0H_{23}) \\ +H_{23}H_{13}=0\tag{16} B1​(H11​−u0​H13​)(H21​−u0​H23​)+B2​(H12​−v0​H13​)(H22​−v0​H23​)+H23​H13​=0(16)

式（10）的展开

和式（9）的推导相似，对式（16）可以直接替换下标，结果为

B 1 ( H 11 − u 0 H 13 ) ( H 11 − u 0 H 13 ) + B 2 ( H 12 − v 0 H 13 ) ( H 12 − v 0 H 13 ) + H 13 H 13 − B 1 ( H 21 − u 0 H 23 ) ( H 21 − u 0 H 23 ) − B 2 ( H 22 − v 0 H 23 ) ( H 22 − v 0 H 23 ) − H 23 H 23 = 0 (17) B_1(H_{11}-u_0H_{13})(H_{11}-u_0H_{13}) \\ + B_2(H_{12}-v_0H_{13})(H_{12}-v_0H_{13}) \\ +H_{13}H_{13} \\ -B_1(H_{21}-u_0H_{23})(H_{21}-u_0H_{23}) \\ - B_2(H_{22}-v_0H_{23})(H_{22}-v_0H_{23}) \\ -H_{23}H_{23}=0\tag{17} B1​(H11​−u0​H13​)(H11​−u0​H13​)+B2​(H12​−v0​H13​)(H12​−v0​H13​)+H13​H13​−B1​(H21​−u0​H23​)(H21​−u0​H23​)−B2​(H22​−v0​H23​)(H22​−v0​H23​)−H23​H23​=0(17) B 1 [ ( H 11 − u 0 H 13 ) 2 − ( H 21 − u 0 H 23 ) 2 ] + B 2 ( H 12 − v 0 H 13 ) 2 − ( H 22 − v 0 H 23 ) 2 + H 13 2 − H 23 2 = 0 (18) B_1[(H_{11}-u_0H_{13})^2-(H_{21}-u_0H_{23})^2] \\ + B_2(H_{12}-v_0H_{13})^2-(H_{22}-v_0H_{23})^2 \\ +H_{13}^2-H_{23}^2=0\tag{18} B1​[(H11​−u0​H13​)2−(H21​−u0​H23​)2]+B2​(H12​−v0​H13​)2−(H22​−v0​H23​)2+H132​−H232​=0(18)

式（9）和式（10）推导出的最终形式，联合式（16）和（18）并写出矩阵形式

[ ( H 11 − u 0 H 13 ) ( H 21 − u 0 H 23 ) ( H 12 − v 0 H 13 ) ( H 22 − v 0 H 23 ) ( H 11 − u 0 H 13 ) 2 − ( H 21 − u 0 H 23 ) 2 ( H 12 − v 0 H 13 ) 2 − ( H 22 − v 0 H 23 ) 2 ] [ B 1 B 2 ] = [ − H 23 H 13 − ( H 13 2 − H 23 2 ) ] (19) \begin{bmatrix} (H_{11}-u_0H_{13})(H_{21}-u_0H_{23}) &(H_{12}-v_0H_{13})(H_{22}-v_0H_{23}) \\ (H_{11}-u_0H_{13})^2-(H_{21}-u_0H_{23})^2 &(H_{12}-v_0H_{13})^2-(H_{22}-v_0H_{23})^2 \end{bmatrix} \begin{bmatrix} B_1\\B_2 \end{bmatrix} = \begin{bmatrix} -H_{23}H_{13} \\ -(H_{13}^2-H_{23}^2)\end{bmatrix} \tag{19} [(H11​−u0​H13​)(H21​−u0​H23​)(H11​−u0​H13​)2−(H21​−u0​H23​)2​(H12​−v0​H13​)(H22​−v0​H23​)(H12​−v0​H13​)2−(H22​−v0​H23​)2​][B1​B2​​]=[−H23​H13​−(H132​−H232​)​](19)这是典型的形如“Ax=b"的形式，能够容易的求解B1和B2，也就是得到了焦距

f x = 1 / B 1 , f y = 1 / B 2 (20) f_x=\sqrt{1/B_1}, f_y=\sqrt{1/B_2} \tag{20} fx​=1/B1​ ⎡​H11​−u0​H13​H12​−v0​H13​H13​​⎦ ⎤​(21) v = t 1 = [ H 21 − u 0 H 23 H 22 − v 0 H 23 H 23 ] (22) \bf{v}=t_1=\begin{bmatrix} H_{21}-u_0H_{23} \\ H_{22}-v_0H_{23} \\ H_{23} \end{bmatrix}\tag{22} v=t1​=⎣ ⎡​H21​−u0​H23​H22​−v0​H23​H23​​⎦ ⎤​(22) d 1 = t 0 + t 1 = [ ( H 11 − u 0 H 13 ) + ( H 21 − u 0 H 23 ) ( H 12 − v 0 H 13 ) + ( H 22 − v 0 H 23 ) H 13 + H 23 ] (23) \bf{d_1}=t_0+t_1=\begin{bmatrix} (H_{11}-u_0H_{13}) + (H_{21}-u_0H_{23}) \\ (H_{12}-v_0H_{13}) + (H_{22}-v_0H_{23}) \\ H_{13} + H_{23} \end{bmatrix}\tag{23} d1​=t0​+t1​=⎣ ⎡​(H11​−u0​H13​)+(H21​−u0​H23​)(H12​−v0​H13​)+(H22​−v0​H23​)H13​+H23​​⎦ ⎤​(23) d 2 = t 0 − t 1 = [ ( H 11 − u 0 H 13 ) − ( H 21 − u 0 H 23 ) ( H 12 − v 0 H 13 ) − ( H 22 − v 0 H 23 ) H 13 − H 23 ] (24) \bf{d_2}=t_0-t_1=\begin{bmatrix} (H_{11}-u_0H_{13}) - (H_{21}-u_0H_{23}) \\ (H_{12}-v_0H_{13}) - (H_{22}-v_0H_{23}) \\ H_{13} - H_{23} \end{bmatrix}\tag{24} d2​=t0​−t1​=⎣ ⎡​(H11​−u0​H13​)−(H21​−u0​H23​)(H12​−v0​H13​)−(H22​−v0​H23​)H13​−H23​​⎦ ⎤​(24)使用式(21)~(24)的符号带入式（19）

[ h [ 0 ] ⋅ v [ 0 ] h [ 1 ] ⋅ v [ 1 ] d 1 [ 0 ] ⋅ d 2 [ 0 ] d 1 [ 1 ] ⋅ d 2 [ 1 ] ] [ B 1 B 2 ] = [ − h [ 2 ] ⋅ v [ 2 ] − d 1 [ 2 ] ⋅ d 2 [ 2 ] ] (25) \begin{bmatrix} \bf{h}[0]\cdot v[0] &\bf{h}[1]\cdot v[1] \\ \bf{d_1}[0] \cdot \bf{d_2}[0] &\bf{d_1}[1] \cdot \bf{d_2}[1] \end{bmatrix} \begin{bmatrix} B_1\\B_2 \end{bmatrix} = \begin{bmatrix} -\bf{h}[2]\cdot v[2] \\ -\bf{d_1}[2] \cdot \bf{d_2}[2] \end{bmatrix} \tag{25} [h[0]⋅v[0]d1​[0]⋅d2​[0]​h[1]⋅v[1]d1​[1]⋅d2​[1]​][B1​B2​​]=[−h[2]⋅v[2]−d1​[2]⋅d2​[2]​](25)令 A p = [ h [ 0 ] ⋅ v [ 0 ] h [ 1 ] ⋅ v [ 1 ] d 1 [ 0 ] ⋅ d 2 [ 0 ] d 1 [ 1 ] ⋅ d 2 [ 1 ] ] (26) \bf{Ap} = \begin{bmatrix} \bf{h}[0]\cdot v[0] &\bf{h}[1]\cdot v[1] \\ \bf{d_1}[0] \cdot \bf{d_2}[0] &\bf{d_1}[1] \cdot \bf{d_2}[1] \end{bmatrix} \tag{26} Ap=[h[0]⋅v[0]d1​[0]⋅d2​[0]​h[1]⋅v[1]d1​[1]⋅d2​[1]​](26) b p = [ − h [ 2 ] ⋅ v [ 2 ] − d 1 [ 2 ] ⋅ d 2 [ 2 ] ] (27) \bf{bp} = \begin{bmatrix} -\bf{h}[2]\cdot v[2] \\ -\bf{d_1}[2] \cdot \bf{d_2}[2] \end{bmatrix} \tag{27} bp=[−h[2]⋅v[2]−d1​[2]⋅d2​[2]​](27)以上推导过程中与opencv不同的有两点，1是代码中对 h \bf{h} h， v \bf{v} v, d 1 \bf{d_1} d1​和 d 2 \bf{d_2} d2​做了归一化处理，应该是防止系数矩阵元素之间差别太大，导致病态问题；2是 d 1 = ( t 0 + t 1 ) ∗ 0.5 , d 2 = ( t 0 − t 1 ) ∗ 0.5 \bf{d_1}=(t_0+t_1)*0.5, \bf{d_2}=(t_0-t_1)*0.5 d1​=(t0​+t1​)∗0.5,d2​=(t0​−t1​)∗0.5这里的0.5在我推导中目前没有找到合理的解释。

代码注释

CV_IMPL void cvInitIntrinsicParams2D( const CvMat* objectPoints,
                         const CvMat* imagePoints, const CvMat* npoints,
                         CvSize imageSize, CvMat* cameraMatrix,
                         double aspectRatio )
{
    Ptr<CvMat> matA, _b, _allH;

    int i, j, pos, nimages, ni = 0;
    // **a是内参矩阵
    double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
    double H[9] = {0}, f[2] = {0};
    CvMat _a = cvMat( 3, 3, CV_64F, a );
    CvMat matH = cvMat( 3, 3, CV_64F, H );
    CvMat _f = cvMat( 2, 1, CV_64F, f );

    assert( CV_MAT_TYPE(npoints->type) == CV_32SC1 &&
            CV_IS_MAT_CONT(npoints->type) );
    nimages = npoints->rows + npoints->cols - 1;

    if( (CV_MAT_TYPE(objectPoints->type) != CV_32FC3 &&
        CV_MAT_TYPE(objectPoints->type) != CV_64FC3) ||
        (CV_MAT_TYPE(imagePoints->type) != CV_32FC2 &&
        CV_MAT_TYPE(imagePoints->type) != CV_64FC2) )
        CV_Error( CV_StsUnsupportedFormat, "Both object points and image points must be 2D" );

    if( objectPoints->rows != 1 || imagePoints->rows != 1 )
        CV_Error( CV_StsBadSize, "object points and image points must be a single-row matrices" );

    matA.reset(cvCreateMat( 2*nimages, 2, CV_64F ));
    _b.reset(cvCreateMat( 2*nimages, 1, CV_64F ));
    a[2] = (!imageSize.width) ? 0.5 : (imageSize.width)*0.5;
    a[5] = (!imageSize.height) ? 0.5 : (imageSize.height)*0.5;
    _allH.reset(cvCreateMat( nimages, 9, CV_64F ));

    // extract vanishing points in order to obtain initial value for the focal length
    for( i = 0, pos = 0; i < nimages; i++, pos += ni )
    {
        double* Ap = matA->data.db + i*4;
        double* bp = _b->data.db + i*2;
        ni = npoints->data.i[i];
        double h[3], v[3], d1[3], d2[3];
        double n[4] = {0,0,0,0};
        CvMat _m, matM;
        cvGetCols( objectPoints, &matM, pos, pos + ni );
        cvGetCols( imagePoints, &_m, pos, pos + ni );
		// **求解单应性矩阵H
        cvFindHomography( &matM, &_m, &matH );
        memcpy( _allH->data.db + i*9, H, sizeof(H) );
		// **见式(21),(22)
        H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
        H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];

        for( j = 0; j < 3; j++ )
        {
            double t0 = H[j*3], t1 = H[j*3+1];
            // **见式(21),(22),(23),(24)
            h[j] = t0; v[j] = t1;
            d1[j] = (t0 + t1)*0.5;
            d2[j] = (t0 - t1)*0.5;
            n[0] += t0*t0; n[1] += t1*t1;
            n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
        }
		// **求h,v,d1和d2的模
        for( j = 0; j < 4; j++ )
            n[j] = 1./std::sqrt(n[j]);
            
		// **h,v,d1和d2归一化
        for( j = 0; j < 3; j++ )
        {
            h[j] *= n[0]; v[j] *= n[1];
            d1[j] *= n[2]; d2[j] *= n[3];
        }
		// **见式(25),(26),(27)
        Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
        Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
        bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
    }
	// **求解式(19),或者说(25)
    cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
    // **求解式(20)
    a[0] = std::sqrt(fabs(1./f[0]));
    a[4] = std::sqrt(fabs(1./f[1]));
    
	// **若定义CCD传感器横纵分布不垂直程度的系数不为零，修正焦距
    if( aspectRatio != 0 )
    {
        double tf = (a[0] + a[4])/(aspectRatio + 1.);
        a[0] = aspectRatio*tf;
        a[4] = tf;
    }

    cvConvert( &_a, cameraMatrix );
}
           

参考文献

[1] Zhang Z . A flexible new technique for camera calibration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 22.