opencv相机标定原理-封闭解计算

相机标定原理——opencv内参解析解推导

封闭解估计五个内在参数 cvInitIntrinsicParams2D（）

源代码

/* 2.1 封闭解估计参数 */
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[9]
    double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
    // 相机外参H[9]，焦距f[2]
    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 );

    // 图像数量
    nimages = npoints->rows + npoints->cols - 1;

    matA.reset(cvCreateMat( 2*nimages, 2, CV_64F ));
    _b.reset(cvCreateMat( 2*nimages, 1, CV_64F ));
    //  a[2]， a[5]：初始化光心坐标
    a[2] = (!imageSize.width) ? 0.5 : (imageSize.width - 1)*0.5;
    a[5] = (!imageSize.height) ? 0.5 : (imageSize.height - 1)*0.5;
    // 所有图像的 单应性矩阵H
    _allH.reset(cvCreateMat( nimages, 9, CV_64F ));

    // 获得焦距的初始值
    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: 计算 图像点与对象点的 单应性矩阵H（投影变换矩阵）*/
        cvFindHomography( &matM, &_m, &matH );
        memcpy( _allH->data.db + i*9, H, sizeof(H) );

        /* 根据H求解相机两个焦距内参 */
        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];
            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];
        }

        for( j = 0; j < 4; j++ )
            n[j] = 1./std::sqrt(n[j]);

        for( j = 0; j < 3; j++ )
        {
            h[j] *= n[0]; v[j] *= n[1];
            d1[j] *= n[2]; d2[j] *= n[3];
        }

        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];
    }

    /*
        cvSolve : 求解线性系统或者最小二乘法问题
        求解两个焦距参数
    */
    cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
    a[0] = std::sqrt(fabs(1./f[0]));
    a[4] = std::sqrt(fabs(1./f[1]));
    if( aspectRatio != 0 )
    {
        double tf = (a[0] + a[4])/(aspectRatio + 1.);
        a[0] = aspectRatio*tf;
        a[4] = tf;
    }

    cvConvert( &_a, cameraMatrix );
}

opencv中封闭解 计算原理及步骤：

（1） 通过对象点和图像点由cvFindHomography（）函数计算单应性矩阵H；

（2） 有：
$$\mathbf{H}=\mathbf{A}[\mathbf{r}1,r2,\mathbf{t}]=\left[\begin{array}{lll}h11& h12& h13\\ h21& h22& h23\\ h31& h32& h33\end{array}\right]$$
此时，A中设定倾斜系数c=0，主点u0, v0由图像大小得到，因此现在的相机内参只有两个焦距未知量：
$$A=\left[\begin{array}{ccc}\alpha & 0& u_0\\ 0& \beta & v_0\\ 0& 0& 1\end{array}\right]$$
由上面两式有：
$$\left[\begin{array}{ccc}\alpha & 0& u_0\\ 0& \beta & v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{lll}{r}_{11}& r_{12}& t_1\\ r_{21}& r_{22}& t_2\\ r_{31}& r_{32}& t_3\end{array}\right]=\left[\begin{array}{ccc}{H}_0& H_1& H_2\\ H_3& H_4& H_5\\ H_6& H_7& H_8\end{array}\right]$$
展开有得到以下9项（求解焦距只需要这几项）：
$$\begin{array}{rl}\alpha r_{11}+u_0{r}_{31}& =H_0\\ \alpha r_{12}+u_0{r}_{32}& =H_1\\ \alpha t_1+u_0{t}_3& =H_2\\ \beta r_{21}+v_0{r}_{31}& =H_3\\ \beta r_{22}+v_0{r}_{32}& =H_4\\ \beta t_2+v_0{t}_3& =H_5\\ r_{31}& =H_6\\ r_{32}& =H_7\\ t_3& =H_8\end{array}$$
将下面三项带入上面四项有：
$$\begin{array}{rl}\alpha r_{11}& =H_0-u_0{H}_6\\ \alpha r_{12}& =H_1-u_0{H}_7\\ \alpha t_1& =H_2-u_0{H}_8\\ \beta r_{21}& =H_3-v_0{H}_6\\ \beta r_{22}& =H_4-v_0{H}_7\\ \beta t_2& =H_5-v_0{H}_8\\ r_{31}& =H_6\\ r_{32}& =H_7\\ t_3& =H_8\end{array}\text{\hspace{1em}(a)}$$
上式就是以下代码：

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];

此时H经过更新（根据代码）为：
$$\begin{array}{rl}\alpha r_{11}& =H_0\\ \alpha r_{12}& =H_1\\ \alpha t_1& =H_2\\ \beta r_{21}& =H_3\\ \beta r_{22}& =H_4\\ \beta t_2& =H_5\\ r_{31}& =H_6\\ r_{32}& =H_7\\ t_3& =H_8\end{array}\text{\hspace{1em}(b)}$$
即
$$\begin{array}{rl}{r}_{11}& =H_0/\alpha \\ r_{12}& =H_1/\alpha \\ t_1& =H_2/\alpha \\ r_{21}& =H_3/\beta \\ r_{22}& =H_4/\beta \\ t_2& =H_5/\beta \\ r_{31}& =H_6\\ r_{32}& =H_7\\ t_3& =H_8\end{array}$$

（3） 在（a）中，我们发现6个方程有8个未知量，要想解出焦距还需要两个方程，这两个方程就是前面的两个约束：
$$\begin{array}{c}{r}_1^T{r}_2=0\\ r_1^T{r}_1=r_2^T{r}_2=1\end{array}$$
结合式（b）：
$$\begin{array}{rl}{r}_1:& \\ r_{11}& =H_0/\alpha \\ r_{21}& =H_3/\beta \\ r_{31}& =H_6\\ r_2:& \\ r_{12}& =H_1/\alpha \\ r_{22}& =H_4/\beta \\ r_{32}& =H_7\end{array}$$
则有两个新约束方程：
$$\begin{array}{rl}& \frac{H_0{H}_1}{{\alpha }^2}+\frac{H_3{H}_4}{{\beta }^2}+H_6{H}_7=0\\ & \frac{H_0^2}{{\alpha }^2}+\frac{H_3^2}{{\beta }^2}+H_6^2=\frac{H_1^2}{{\alpha }^2}+\frac{H_4^2}{{\beta }^2}+H_7^2\\ & \Rightarrow \frac{H_0^2-H_1^2}{{\alpha }^2}+\frac{H_3^2-H_4^2}{{\beta }^2}+\left(H_6^2-H_7^2\right)=0\end{array}\text{\hspace{1em}(C)}$$
根据（C）结合所有图像则可以得到方程组求解出两个焦距。代码为（代码中n是为了单位化向量r）：

    for( j = 0; j < 3; j++ )
    {
        double t0 = H[j*3], t1 = H[j*3+1];
        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];
    }

    for( j = 0; j < 4; j++ )
        n[j] = 1./std::sqrt(n[j]);

    for( j = 0; j < 3; j++ )
    {
        h[j] *= n[0]; v[j] *= n[1];
        d1[j] *= n[2]; d2[j] *= n[3];
    }

    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];
}

/*
cvSolve : 求解线性系统或者最小二乘法问题
求解两个焦距参数
*/
cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
a[0] = std::sqrt(fabs(1./f[0]));
a[4] = std::sqrt(fabs(1./f[1]));

将代码中的公式罗列出：
$$\begin{array}{rl}& j=0\\ & t0=H_0,t1=H_1;\\ & h[0]=t0=H_0;v[0]=t1=H_1;\\ & d1[0]=(t0+t1)\ast 0.5=\frac{H_0+H_1}{2};\\ & d2[0]=(t0-t1)\ast 0.5=\frac{H_0-H_1}{2};\\ & n[0]+=t0\ast t0=H_0^2;\\ & n[1]+=t1\ast t1=H_1^2;\\ & n[2]+=d1[0]\ast d1[0]={\left(\frac{H_0+H_1}{2}\right)}^2;\\ & n[3]+=d2[0]\ast d2[0]={\left(\frac{H_0-H_1}{2}\right)}^2;\end{array}$$

$$\begin{array}{rl}& j=1\\ & t0=H_3,t1=H_4;\\ & h[1]=t0=H_3;v[1]=t1=H_4;\\ & d1[1]=(t0+t1)\ast 0.5=\frac{H_3+H_4}{2};\\ & d2[1]=(t0-t1)\ast 0.5=\frac{H_3-H_4}{2};\\ & n[0]+=t0\ast t0=H_3^2+H_0^2;\\ & n[1]+=t1\ast t1=H_4^2+H_1^2;\\ & n[2]+=d1[1]\ast d1[1]={\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2;\\ & n[3]+=d2[1]\ast d2[1]={\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2;\end{array}$$

$$\begin{array}{rl}& j=2\\ & t0=H_6,t1=H_7;\\ & h[1]=t0=H_6;v[1]=t1=H_7;\\ & d1[1]=(t0+t1)\ast 0.5=\frac{H_6+H_7}{2};\\ & d2[1]=(t0-t1)\ast 0.5=\frac{H_6-H_7}{2};\\ & n[0]+=t0\ast t0=H_6^2+H_3^2+H_0^2;\\ & n[1]+=t1\ast t1=H_7^2+H_4^2+H_1^2;\\ & n[2]+=d1[1]\ast d1[1]={\left(\frac{H_6+H_7}{2}\right)}^2+{\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2;\\ & n[3]+=d2[1]\ast d2[1]={\left(\frac{H_6-H_7}{2}\right)}^2+{\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2;\end{array}$$

上述式子就是以下代码：

    for( j = 0; j < 3; j++ )
    {
        double t0 = H[j*3], t1 = H[j*3+1];
        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];
    }

变量n开根号求倒数
$$\begin{array}{rl}& n[0]=\frac{1}{\sqrt{n[0]}}=\frac{1}{\sqrt{H_6^2+H_3^2+H_0^2}};\\ & n[1]=\frac{1}{\sqrt{n[1]}}=\frac{1}{\sqrt{H_7^2+H_4^2+H_1^2}};\\ & n[2]=\frac{1}{\sqrt{n[2]}}=\frac{1}{\sqrt{{\left(\frac{H_6+H_7}{2}\right)}^2+{\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2}};\\ & n[3]=\frac{1}{\sqrt{n[3]}}=\frac{1}{\sqrt{{\left(\frac{H_6-H_7}{2}\right)}^2+{\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2}};\end{array}$$
代码如下：

for( j = 0; j < 4; j++ )
	n[j] = 1./std::sqrt(n[j]);

求h，v，d1，d2
$$\begin{array}{rl}& j=0\\ & h[0]=n[0]h[0]=\frac{1}{\sqrt{H_6^2+H_3^2+H_0^2}}\ast H_0\\ & v[0]=n[1]v[0]=\frac{1}{\sqrt{H_7^2+H_4^2+H_1^2}}\ast H_1\\ & d1[0]=n[2]d1[0]=\frac{1}{\sqrt{{\left(\frac{H_6+H_7}{2}\right)}^2+{\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2}}\ast \frac{H_0+H_1}{2}\\ & d2[0]=n[3]d2[0]=\frac{1}{\sqrt{{\left(\frac{H_6-H_7}{2}\right)}^2+{\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2}}\ast \frac{H_0-H_1}{2}\end{array}$$

$$\begin{array}{rl}& j=1\\ & h[1]=n[0]h[1]=\frac{1}{\sqrt{H_6^2+H_3^2+H_0^2}}\ast H_3\\ & v[1]=n[1]v[1]=\frac{1}{\sqrt{H_7^2+H_4^2+H_1^2}}\ast H_4\\ & d1[1]=n[2]d1[1]=\frac{1}{\sqrt{{\left(\frac{H_6+H_7}{2}\right)}^2+{\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2}}\ast \frac{H_3+H_4}{2}\\ & d2[1]=n[3]d2[1]=\frac{1}{\sqrt{{\left(\frac{H_6-H_7}{2}\right)}^2+{\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2}}\ast \frac{H_3-H_4}{2}\end{array}$$

$$\begin{array}{rl}& j=2\\ & h[2]=n[0]h[2]=\frac{1}{\sqrt{H_6^2+H_3^2+H_0^2}}\ast H_6\\ & v[2]=n[1]v[2]=\frac{1}{\sqrt{H_7^2+H_4^2+H_1^2}}\ast H_7\\ & d1[2]=n[2]d1[2]=\frac{1}{\sqrt{{\left(\frac{H_6+H_7}{2}\right)}^2+{\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2}}\ast \frac{H_6+H_7}{2}\\ & d2[2]=n[3]d2[2]=\frac{1}{\sqrt{{\left(\frac{H_6-H_7}{2}\right)}^2+{\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2}}\ast \frac{H_6-H_7}{2}\end{array}$$

上述式子代码如下：

for( j = 0; j < 3; j++ )
{
    h[j] *= n[0]; v[j] *= n[1];
    d1[j] *= n[2]; d2[j] *= n[3];
}

计算Ap，Bp
$$\begin{array}{rl}& Ap[0]=h[0]\ast v[0]=\frac{H_0{H}_1}{\sqrt{H_6^2+H_3^2+H_0^2}\sqrt{H_7^2+H_4^2+H_1^2}}\\ & Ap[1]=h[1]\ast v[1]=\frac{H_3{H}_4}{\sqrt{H_6^2+H_3^2+H_0^2}\sqrt{H_7^2+H_4^2+H_1^2}}\\ & Ap[2]=d1[0]\ast d2[0]=\frac{\left(H_0+H_1\right)\left(H_0-H_1\right)}{4\sqrt{{\left(\frac{H_6+H_7}{2}\right)}^2+{\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2}\sqrt{{\left(\frac{H_6-H_7}{2}\right)}^2+{\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2}}\\ & Ap[3]=d1[1]\ast d2[1]=\frac{\left(H_3+H_4\right)\left(H_3-H_4\right)}{4\sqrt{{\left(\frac{H_6+H_7}{2}\right)}^2+{\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2}\sqrt{{\left(\frac{H_6-H_7}{2}\right)}^2+{\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2}}\\ & bp[0]=-h[2]\ast v[2]=-\frac{H_6{H}_7}{\sqrt{H_6^2+H_3^2+H_0^2}\sqrt{H_7^2+H_4^2+H_1^2}}\\ & bp[1]=-d1[2]\ast d2[2]=-\frac{\left(H_6+H_7\right)\left(H_6-H_7\right)}{4\sqrt{{\left(\frac{H_6+H_7}{2}\right)}^2+{\left(\frac{H_3+H_4}{2}\right)}^2+{\left(\frac{H_0+H_1}{2}\right)}^2}\sqrt{{\left(\frac{H_6-H_7}{2}\right)}^2+{\left(\frac{H_3-H_4}{2}\right)}^2+{\left(\frac{H_0-H_1}{2}\right)}^2}}\end{array}$$
代码如下：

for( j = 0; j < 3; j++ )
{
    h[j] *= n[0]; v[j] *= n[1];
    d1[j] *= n[2]; d2[j] *= n[3];
}

结合式（C），相当于用最小二乘法求解系数a，b
$$\begin{array}{rl}& a{H}_0{H}_1+b{H}_3{H}_4=-H_6{H}_7\\ & a\left(H_0^2-H_1^2\right)+b\left(H_3^2-H_4^2\right)=-\left(H_6^2-H_7^2\right)\end{array}$$
即
$$\begin{array}{rl}& a[0]=\mathrm{std}::\mathrm{sqrt}(fabs(1./f[0]))=\sqrt{\left|\frac{1}{a}\right|}\\ & a[4]=\mathrm{std}::\mathrm{sqrt}(fabs(1./f[1]))=\sqrt{\left|\frac{1}{b}\right|}\end{array}$$

对应代码如下：

/*
cvSolve : 求解线性系统或者最小二乘法问题
求解两个焦距参数
*/
cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
a[0] = std::sqrt(fabs(1./f[0]));
a[4] = std::sqrt(fabs(1./f[1]));