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

背景介绍

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

算法原理

opencv中对内参矩阵的定义为

式中的s表示CCD传感器横纵分布不垂直程度的系数，也就是传感器理想是矩形，但其实可能是具有剪切变形，是直角边不垂直的平行四边形。而Cx和Cy表示主点在图像中的位置，包含了主点偏差。在标定初始时，通常是考虑所有畸变量为零，也就是说此时的内参矩阵为
[fx0u00fyv0001](1)\begin{bmatrix} f_x & 0 & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1\end{bmatrix}\tag{1} ⎣⎡fx000fy0u0v01⎦⎤(1)式中，u0,v0u_0, v_0u0,v0为图像宽度和高度的一半，单位为像素，含义是从像素坐标系转换到像平面坐标的偏移量。cvInitIntrinsicParams2D就是用于估计式（1）中焦距的值，前提是用于平面标定板。
参考张正友的文献【1】假设世界坐标系在平面标定板上，Z轴垂直于标定平面，那么标定板上点的Z=0。根据投影仪方程表达为λ[uv1]=A[r1r2r3t][XY01]=A[r1r2t][XY1](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[r1r2r3t]⎣⎡XY01⎦⎤=A[r1r2t]⎣⎡XY1⎦⎤(2)式中，A\bf{A}A表示式（1）的内参矩阵， r1,r2,r3,t\bf{r_1}, \bf{r_2},\bf{r_3} ,\bf{t}r1,r2,r3,t是外参矩阵中的列向量。由于标定板上三维点和图像上二维点各自都近似在平面上，利用平面单应性关系有λ[uv1]=H[XY1](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=[H1,H2,H3]=λA[r1r2t](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[r1r2t](4)式中，H1，H2和H3表示单应性矩阵的三列，特别注意的是，后面的符号中，H12H_{12}H12不是表示的H矩阵第1行2列的元素，而是列向量H1\bf{H_1}H1中的第二个元素，在矩阵中的位置是第1列第2行，这里和常规的表示不太一样。
根据式（4）可以得到外参矩阵中的列向量r1、r2，根据旋转矩阵单位正交的性质可知
r1T⋅r2=0(5)\bf{r_1}^T \cdot \bf{r_2}=0 \tag{5} r1T⋅r2=0(5)r1T⋅r1=r2T⋅r2=1(6)\bf{r_1}^T \cdot \bf{r_1}= \bf{r_2}^T \cdot \bf{r_2} = 1\tag{6} r1T⋅r1=r2T⋅r2=1(6)根据式（4）得到r1和r2，并带入上面两个式子得到
(A−1H1)T⋅(A−1H2)=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−1H1)T⋅(A−1H1)=(A−1H2)T⋅(A−1H2)(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)进一步简化得到H1TA−TA−1H2=0(9)\bf{H_1^T } \bf{A}^{-T} \bf{A}^{-1}\bf{H_2}=0\tag{9} H1TA−TA−1H2=0(9)H1TA−TA−1H1=H2TA−TA−1H2(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} H1TA−TA−1H1=H2TA−TA−1H2(10)式中，A−T\bf{A}^{-T}A−T表示(A−1)T(\bf{A}^{-1})^T(A−1)T. 接下来就是利用式（9）和（10）来解算焦距fx,fyf_x,f_yfx,fy。为了和代码对应，接下来的推导过程可能比较慢，公式比较长，这一段其实就是上述两个式子进一步带入具体的元素，来构建Ax=bAx=bAx=b的解算式。
文献【1】中令

在所有畸变量初始为零的情况下，上式简化为B=[1α20−u0α201β2−v0β2−u0α2−v0β2u02α2+v02β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=⎣⎡α210−α2u00β21−β2v0−α2u0−β2v0α2u02+β2v02+1⎦⎤(11)论文中α,β\alpha,\betaα,β对应我这里的fx,fyf_x,f_yfx,fy，为了后面推导表达方便，令B1=1α2,B2=1β2B_1=\frac{1}{\alpha^2},B_2=\frac{1}{\beta^2}B1=α21,B2=β21, 下面分两部分分别对式（9）和式（10）详细展开

式（9）的展开

[H11H12H13][B10−u0B10B2−v0B2−u0B1−v0B2u02B1+v02B2+1][H21H22H23]=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} [H11H12H13]⎣⎡B10−u0B10B2−v0B2−u0B1−v0B2u02B1+v02B2+1⎦⎤⎣⎡H21H22H23⎦⎤=0(12)[H11B1−u0H13B1H12B2−v0H13B2−u0H11B1−v0H12B2+u02H13B1+v02H13B2+H13][H21H22H23]=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} [H11B1−u0H13B1H12B2−v0H13B2−u0H11B1−v0H12B2+u02H13B1+v02H13B2+H13]⎣⎡H21H22H23⎦⎤=0(13)(H21H11B1−u0H21H13B1)+(H22H12B2−v0H22H13B2)+(−u0H23H11B1−v0H23H12B2+u02H23H13B1+v02H23H13B2+H23H13)=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} (H21H11B1−u0H21H13B1)+(H22H12B2−v0H22H13B2)+(−u0H23H11B1−v0H23H12B2+u02H23H13B1+v02H23H13B2+H23H13)=0(14)B1(H21H11−u0H21H13−u0H23H11+u02H23H13)+B2(H22H12−v0H22H13−v0H23H12+v02H23H13)+H23H13=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(H21H11−u0H21H13−u0H23H11+u02H23H13)+B2(H22H12−v0H22H13−v0H23H12+v02H23H13)+H23H13=0(15)B1(H11−u0H13)(H21−u0H23)+B2(H12−v0H13)(H22−v0H23)+H23H13=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−u0H13)(H21−u0H23)+B2(H12−v0H13)(H22−v0H23)+H23H13=0(16)

式（10）的展开

和式（9）的推导相似，对式（16）可以直接替换下标，结果为
B1(H11−u0H13)(H11−u0H13)+B2(H12−v0H13)(H12−v0H13)+H13H13−B1(H21−u0H23)(H21−u0H23)−B2(H22−v0H23)(H22−v0H23)−H23H23=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−u0H13)(H11−u0H13)+B2(H12−v0H13)(H12−v0H13)+H13H13−B1(H21−u0H23)(H21−u0H23)−B2(H22−v0H23)(H22−v0H23)−H23H23=0(17)B1[(H11−u0H13)2−(H21−u0H23)2]+B2(H12−v0H13)2−(H22−v0H23)2+H132−H232=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−u0H13)2−(H21−u0H23)2]+B2(H12−v0H13)2−(H22−v0H23)2+H132−H232=0(18)
式（9）和式（10）推导出的最终形式，联合式（16）和（18）并写出矩阵形式
[(H11−u0H13)(H21−u0H23)(H12−v0H13)(H22−v0H23)(H11−u0H13)2−(H21−u0H23)2(H12−v0H13)2−(H22−v0H23)2][B1B2]=[−H23H13−(H132−H232)](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−u0H13)(H21−u0H23)(H11−u0H13)2−(H21−u0H23)2(H12−v0H13)(H22−v0H23)(H12−v0H13)2−(H22−v0H23)2][B1B2]=[−H23H13−(H132−H232)](19)这是典型的形如“Ax=b"的形式，能够容易的求解B1和B2，也就是得到了焦距
fx=1/B1,fy=1/B2(20)f_x=\sqrt{1/B_1}, f_y=\sqrt{1/B_2} \tag{20} fx=1/B1,fy=1/B2(20)为了和Opencv代码中符号一致，各变量整理为h=t0=[H11−u0H13H12−v0H13H13](21)\bf{h} = t_0=\begin{bmatrix} H_{11}-u_0H_{13} \\ H_{12}-v_0H_{13} \\ H_{13} \end{bmatrix}\tag{21} h=t0=⎣⎡H11−u0H13H12−v0H13H13⎦⎤(21)v=t1=[H21−u0H23H22−v0H23H23](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−u0H23H22−v0H23H23⎦⎤(22)d1=t0+t1=[(H11−u0H13)+(H21−u0H23)(H12−v0H13)+(H22−v0H23)H13+H23](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−u0H13)+(H21−u0H23)(H12−v0H13)+(H22−v0H23)H13+H23⎦⎤(23)d2=t0−t1=[(H11−u0H13)−(H21−u0H23)(H12−v0H13)−(H22−v0H23)H13−H23](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−u0H13)−(H21−u0H23)(H12−v0H13)−(H22−v0H23)H13−H23⎦⎤(24)使用式(21)~(24)的符号带入式（19）
[h[0]⋅v[0]h[1]⋅v[1]d1[0]⋅d2[0]d1[1]⋅d2[1]][B1B2]=[−h[2]⋅v[2]−d1[2]⋅d2[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]][B1B2]=[−h[2]⋅v[2]−d1[2]⋅d2[2]](25)令Ap=[h[0]⋅v[0]h[1]⋅v[1]d1[0]⋅d2[0]d1[1]⋅d2[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)bp=[−h[2]⋅v[2]−d1[2]⋅d2[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, d1\bf{d_1}d1和d2\bf{d_2}d2做了归一化处理，应该是防止系数矩阵元素之间差别太大，导致病态问题；2是d1=(t0+t1)∗0.5,d2=(t0−t1)∗0.5\bf{d_1}=(t_0+t_1)*0.5, \bf{d_2}=(t_0-t_1)*0.5d1=(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 lengthfor( 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 );// **求解单应性矩阵HcvFindHomography( &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.