相机标定法-张正有

前言

　　此处“张正友标定”又称“张氏标定”，是指张正友教授于1998年提出的单平面棋盘格的摄像机标定方法。张氏标定法已经作为工具箱或封装好的函数被广泛应用。张氏标定的原文为“A Flexible New Technique forCamera Calibration”。此文中所提到的方法，为相机标定提供了很大便利，并且具有很高的精度。从此标定可以不需要特殊的标定物，只需要一张打印出来的棋盘格。So great! 这样的方法让人肃然起敬。

一 算法描述

1. 打印一张模板并贴在一个平面上
2. 从不同角度拍摄若干张模板图象
3. 检测出图象中的特征点
4. 求出摄像机的内参数和外参数
5. 求出畸变系数
6. 优化求精

二 具体算法

1、标定平面到图像平面的单应性

　　因为张氏标定是一种基于平面棋盘格的标定，所以想要搞懂张氏标定，首先应该从两个平面的单应性(homography)映射开始着手。
单应性(homography)：在计算机视觉中被定义为一个平面到另一个平面的投影映射。首先看一下，图像平面与标定物棋盘格平面的单应性。
由上篇博文中讲到的摄像机模型，肯容易得到：

　　其中m的齐次坐标表示图像平面的像素坐标(u,v,1)，M的齐次坐标表示世界坐标系的坐标点(X,Y,Z,1)。A[R t]即是上面一篇博客推出的P。R表示旋转矩阵、t表示平移矩阵、S表示尺度因子。A表示摄像机的内参数,具体表达式如下：

　　α=f/dx，β=f/dy,因为像素不是规规矩矩的正方形，分别是u，v轴的尺度因子。u0,v0是主点（通常在图像的中心）。γ代表像素点在x,y方向上尺度的偏差。
这里还有一个“梗儿”，就是S。它只是为了方便运算，对于齐次坐标，尺度因子不会改变坐标值的。因为标定物是平面，所以我们可以把世界坐标系构造在Z=0的平面上。然后进行单应性计算。令Z=0可以将上式转换为如下形式：

　　既然，此变化属于单应性变化。那么我们可以给A[r1 r2 t]一个名字：单应性矩阵。并记H= A[r1 r2 t]。
那么现在就有：

　　大家可以分析一下，H是一个三3*3的矩阵，并且有一个元素是作为齐次坐标。因此，H有8个未知量待解。(x,y)作为标定物的坐标，可以由设计者人为控制，是已知量。(u,v)是像素坐标，我们可以直接通过摄像机获得。对于一组对应的(x,y)-à(u,v)我们可以获得两组方程。
现在有8个未知量需要求解，所以我们至少需要八个方程。所以需要四个对应点。四点即可算出，图像平面到世界平面的单应性矩阵H。

这也是张氏标定采用四个角点的棋盘格作为标定物的一个原因。
在这里，我们可以将单应性矩阵写成三个列向量的形式，即：

2、利用约束条件求解内参矩阵A

　　从上面可知，应用4个点我们可以获得单应性矩阵H。但是，H是内参阵和外参阵的合体。我们想要最终分别获得内参和外参。所以需要想个办法，先把内参求出来。然后外参也就随之解出了。我们可以仔细的“观摩”一下下面的式子。

从中可以得出下面两个约束条件，这两个约束条件都是围绕着旋转向量来的。
　　1、r1,r2正交 得：r1r2=0。这个很容易理解，因为r1,r2分别是绕x,y轴旋转的。应用高中立体几何中的两垂直平面上（两个旋转向量分别位于y-z和x-z平面）直线的垂直关系即可轻松推出。
　　2、旋转向量的模为1，即|r1|=|r2|=1。这个也很容易理解，因为旋转不改变尺度嘛。如果不信可以回到上一篇博客，找到个方向的旋转矩阵化行列式算一下。
通过上面的式子可以将r1,r2代换为h1,h2与A的组合进行表达。即 r1=A-1h1，r2=A-1h2.根据两约束条件，可以得到下面两个式子：

　　大家从上面两个式子是不是看出一点端倪了。式子中，h1,h2是通过单应性求解出来的那么未知量就仅仅剩下，内参矩阵A了。内参阵A包含5个参数：α，β，u0，v0，γ。那么如果我们想完全解出这五个未知量，则需要3个单应性矩阵。3个单应性矩阵在2个约束下可以产生6个方程。这样可以解出全部的五个内参了。大家想一下，我们怎样才能获得三个不同的单应性矩阵呢？答案就是，用三幅标定物平面的照片。我们可以通过改变摄像机与标定板间的相对位置来获得三张不同的照片。（当然也可以用两张照片，但这样的话就要舍弃掉一个内参了γ=0）
　　到这里，大家应该就明白我们在张氏标定法时为什么要不断变换标定板的方位了吧。当然这只是一个原因。第二个原因，玉米会在讲极大似然时讲到。
　　下面在对我们得到的方程做一些数学上的变化，这些变化都是简单的运算变化了，相信大家动动笔，一算就可以算出。这些变化都是为了运算方便的，所以也没什么物理意义。
首先令：

　　很容易发现B是一个对称阵，所以B的有效元素只剩下六个（因为有三对对称的元素是相等的，所以只要解得下面的6个元素就可以得到完整的B了），让这六个元素构成向量b。
　　

接下来在做一步纯数学化简：

可以计算得：

利用约束条件可以得到下面，方程组：

　　这个方程组的本质和前面那两个用h和A组成的约束条件方程组是一样的。在此重复一遍解释：如果我们想完全解出这五个未知量，则需要3个单应性矩阵。3个单应性矩阵在2个约束下可以产生6个方程。这样可以解出全部的五个内参了。大家想一下，我们怎样才能获得三个不同的单应性矩阵呢？答案就是，用三幅标定物平面的照片。我们可以通过改变摄像机与标定板间的相对位置来获得三张不同的照片。（当然也可以用两张照片，但这样的话就要舍弃掉一个内参了γ=0）
　　通过至少含一个棋盘格的三幅图像，应用上述公式我们就可以估算出B了。得到B后，我们通过cholesky分解 ，就可以轻松地得到摄像机的内参阵A。
　　
3、基于内参阵估算外参阵

通过上面的运算，我们已经获得了摄像机的内参阵。那么对于外参阵，我们很容易通过下面的公式解得：

对上面公式进行化简，可以得到：

至此，玉米已经将张氏标定的主体数学框架已经讲完了。介于篇幅关系（怕太长大机会读的昏昏欲睡，哈哈）。但其实我们做了这么多推导，仅仅是为后面的极大似然参数估计提供初值。但当然这个初值也是不可或缺的，因为没有这个初值，就无法估计出更为准确的参数。玉米将张氏标定中用于提高标定精度的极大似然算法，放到下一篇博客中进行讲解。

上文中内容引自本链接

三 张正有opencv实现

void cvCalibrateCamera2( const CvMat* object_points, const CvMat* image_points,
                         const CvMat* point_counts, CvSize image_size,
                         CvMat* intrinsic_matrix, CvMat* distortion_coeffs,
                         CvMat* rotation_vectors=NULL,
                         CvMat* translation_vectors=NULL,
                         int flags=0 );

object_points
定标点的世界坐标，为3xN或者Nx3的矩阵，这里N是所有视图中点的总数。
image_points
定标点的图像坐标，为2xN或者Nx2的矩阵，这里N是所有视图中点的总数。
point_counts
向量，指定不同视图里点的数目，1xM或者Mx1向量，M是视图数目。
image_size
图像大小，只用在初始化内参数时。
intrinsic_matrix
输出内参矩阵(A)

$$\begin{bmatrix}fx & 0 & cx\\ 0 & fy & cy \\ 0&0&1\end{bmatrix}$$ ，如果指定CV_CALIB_USE_INTRINSIC_GUESS和（或）CV_CALIB_FIX_ASPECT_RATION，fx、 fy、 cx和cy部分或者全部必须被初始化。
distortion_coeffs
输出大小为4x1或者1x4的向量，里面为形变参数[k1, k2, p1, p2]。
rotation_vectors
输出大小为3xM或者Mx3的矩阵，里面为旋转向量（旋转矩阵的紧凑表示方式，具体参考函数cvRodrigues2）
translation_vectors
输出大小为3xM或Mx3的矩阵，里面为平移向量。

而标定板的角点坐标由cvFindChessboardCorners()函数获得。
函数cvFindChessboardCorners试图确定输入图像是否是棋盘模式，并确定角点的位置。如果所有角点都被检测到且它们都被以一定顺序排布（一行一行地，每行从左到右），函数返回非零值，否则在函数不能发现所有角点或者记录它们地情况下，函数返回0。

int i,j,t;
    ifstream fin("calibdata.txt"); /* 定标所用图像文件的路径 *//*ifstream类，读取文件数据*/
    ofstream fout("caliberation_result.txt");  /* 保存定标结果的文件 *//*ofstream类，向文件中写入数据*/

    /************************************************************************
     Step1：读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
    *************************************************************************/

    cout<<"开始提取角点………………";

    int image_count=0;  /* 图像数量 */
    CvSize image_size;  /* 图像的尺寸*/

    CvSize board_size = cvSize(4,6);    /* 定标板上每行、列的角点数 */

    CvPoint2D32f* image_points_buf = new CvPoint2D32f[board_size.width*board_size.height];   /* 缓存每幅图像上检测到的角点，角点位置以像素坐标保存 */
    Seq<CvPoint2D32f> image_points_seq;  /* 保存检测到的所有角点 */

    string filename;

    int count= -1 ;//用于存储角点个数。 

    while (getline(fin,filename))
    {
        image_count++;

        if(image_count==1)
        {
            GetDlgItem(IDC_STATIC1)->ShowWindow(SW_SHOW);
        }
        if(image_count==2)
        {
            GetDlgItem(IDC_STATIC2)->ShowWindow(SW_SHOW);
        }
        if(image_count==3)
        {
            GetDlgItem(IDC_STATIC3)->ShowWindow(SW_SHOW);
        }
        if(image_count==4)
        {
            GetDlgItem(IDC_STATIC4)->ShowWindow(SW_SHOW);
        }
        if(image_count==5)
        {
            GetDlgItem(IDC_STATIC5)->ShowWindow(SW_SHOW);
        }
        if(image_count==6)
        {
            GetDlgItem(IDC_STATIC6)->ShowWindow(SW_SHOW);
        }
        if(image_count==7)
        {
            GetDlgItem(IDC_STATIC7)->ShowWindow(SW_SHOW);
        }
        if(image_count==8)
        {
            GetDlgItem(IDC_STATIC8)->ShowWindow(SW_SHOW);
        }
        if(image_count==9)
        {
            GetDlgItem(IDC_STATIC9)->ShowWindow(SW_SHOW);
        }
        if(image_count==10)
        {
            GetDlgItem(IDC_STATIC10)->ShowWindow(SW_SHOW);
        }
        if(image_count==11)
        {
            GetDlgItem(IDC_STATIC11)->ShowWindow(SW_SHOW);
        }
        if(image_count==12)
        {
            GetDlgItem(IDC_STATIC12)->ShowWindow(SW_SHOW);
        }

        cout<<"image_count = "<<image_count<<endl;
        Image<uchar> view(filename);

        if (image_count == 1) 
        {
            image_size.width = view.size().width;
            image_size.height = view.size().height;

            cout<<"image_size.width = "<<image_size.width<<endl;/*输出图像宽和高*/
            cout<<"image_size.height = "<<image_size.height<<endl;

        }
        /*角点检测失败*/
//board_size 每行和每列有多少角点，事先设定。image_points_buf 保存角点位置，count指向角点数目指针
        if (0 == cvFindChessboardCorners( view.cvimage, board_size,
            image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH ))
        {
            cout<<"can not find chessboard corners!\n";
            exit(1);
        } 
        /*角点检测成功*/
        else 
        {

            Image<uchar> view_gray(view.size(),8,1);
            rgb2gray(view,view_gray);

            cvFindCornerSubPix( view_gray.cvimage, image_points_buf, count, cvSize(11,11),
                cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));//对粗提取的角点进行精确化输出image_points_buf

            image_points_seq.push_back(image_points_buf,count);//将检测到的每一幅图像的角点坐标都放入seq中

            cvDrawChessboardCorners( view.cvimage, board_size, image_points_buf, count, 1);//用于在图片中标记角点
            cvNamedWindow("角点检测",0);
            view.show("角点检测");//显示图片
            cvMoveWindow("角点检测",270,200);
            cvResizeWindow("角点检测",448,336);
            cvWaitKey(1000);//用于暂停，单位是毫秒
            view.close();
        }
    }

     /************************************************************************
     Step2：输出检测到的角点数据（利用包装后的数据结构（seq类）进行操作）
    *************************************************************************/      
       /*用于测试 not-delete!!*/                                                         
        int total = image_points_seq.length();
        cout<<"total = "<<total<<endl;//所有角点的总数量

        for (int ii=0 ; ii<total ;ii++)
        {
            if (0 == ii%24)// 24 是每幅图片的角点个数。此判断语句是为了输出 图片号，便于控制台观看 
            {   
                int i = -1;
                i = ii/24;
                int j=i+1;
                cout<<"--> 第 "<<j <<"图片角点的数据 --> : "<<endl;

            }       
            if (0 == ii%2)  // 此判断语句，格式化输出，便于控制台查看
            {
                cout<<endl;
            }
            else
            {
                cout.width(10);
            }

            cout<<" -->"<<image_points_seq[ii].x;   //输出所有的角点
            cout<<" -->"<<image_points_seq[ii].y;

        }

    delete []image_points_buf;
    cout<<"角点提取完成！\n";


    GetDlgItem(IDC_STATIC13)->ShowWindow(SW_SHOW);
     /************************************************************************
     Step3：摄像机标定
    *************************************************************************/      

    cout<<"开始定标………………";

    /*棋盘三维信息*/

    CvSize square_size = cvSize(10,10);  /* 实际测量得到的定标板上每个棋盘格的大小 */
    Matrix<double> object_points(1,board_size.width*board_size.height*image_count,3); /* 保存定标板上角点的三维坐标 *//*3表示3维*/
    Matrix<double> image_points(1,image_points_seq.cvseq->total,2); /* 保存提取的所有角点 */
    Matrix<int> point_counts(1,image_count,1); /* 每幅图像中角点的数量 */

    /*内外参数*/

    Matrix<double> intrinsic_matrix(3,3,1); /* 摄像机内参数矩阵 */
    Matrix<double> distortion_coeffs(1,4,1); /* 摄像机的4个畸变系数：k1,k2,p1,p2 */
    Matrix<double> rotation_vectors(1,image_count,3); /* 每幅图像的旋转向量 */
    Matrix<double> translation_vectors(1,image_count,3); /* 每幅图像的平移向量 */


    /* 初始化定标板上角点的三维坐标 */


    for (t=0;t<image_count;t++) {
        for (i=0;i<board_size.height;i++) {
            for (j=0;j<board_size.width;j++) {
                /* 假设定标板放在世界坐标系中z=0的平面上 分三维赋值  共有3*6*4*12=864个数据*/
                object_points(0,t*board_size.height*board_size.width+i*board_size.width+j,0) = i*square_size.width;
                object_points(0,t*board_size.height*board_size.width+i*board_size.width+j,1) = j*square_size.height;
                object_points(0,t*board_size.height*board_size.width+i*board_size.width+j,2) = 0;
            }
        }
    }
    cvSave("objectt_points.xml",object_points.cvmat);/*保存成xml格式，便于调用*/

    /* 将角点的存储结构转换成矩阵形式 */

    for (i=0;i<image_points_seq.cvseq->total;i++) {
        image_points(0,i,0) = image_points_seq[i].x;
        image_points(0,i,1) = image_points_seq[i].y;
    }


    cvSave("imagee_points.xml",image_points.cvmat);/*保存成xml格式，便于调用*/

    /* 初始化每幅图像中的角点数量，这里我们假设每幅图像中都可以看到完整的定标板 */

    for (i=0;i<image_count;i++)
        point_counts(0,i) = board_size.width*board_size.height;

    /* 开始定标 */

    cvCalibrateCamera2(object_points.cvmat,
                       image_points.cvmat,
                       point_counts.cvmat,
                       image_size,
                       intrinsic_matrix.cvmat,
                       distortion_coeffs.cvmat,
                       rotation_vectors.cvmat,
                       translation_vectors.cvmat,
                       0);
    cout<<"定标完成！\n";

    /************************************************************************
    Step4：标定误差计算
    *************************************************************************/

    cout<<"开始评价定标结果………………\n";

    double total_err = 0.0; /* 所有图像的平均误差的总和 */
    double err = 0.0; /* 每幅图像的平均误差 */
    Matrix<double> image_points2(1,point_counts(0,0,0),2); /* 保存重新计算得到的投影点 */

    cout<<"\t每幅图像的定标误差：\n";
    fout<<"每幅图像的定标误差：\n"; /*将标定误差结果写入Text中*/

    for (i=0;i<image_count;i++) {
        /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
        cvProjectPoints2(object_points.get_cols(i*point_counts(0,0,0),(i+1)*point_counts(0,0,0)-1).cvmat,
                        rotation_vectors.get_col(i).cvmat,
                        translation_vectors.get_col(i).cvmat,
                        intrinsic_matrix.cvmat,
                        distortion_coeffs.cvmat,
                        image_points2.cvmat,
                        0,0,0,0);
        /* 计算新的投影点和旧的投影点之间的误差*/
        err = cvNorm(image_points.get_cols(i*point_counts(0,0,0),(i+1)*point_counts(0,0,0)-1).cvmat,
                    image_points2.cvmat,
                    CV_L1);
        total_err += err/=point_counts(0,0,0);
        cout<<"\t\t第"<<i+1<<"幅图像的平均误差："<<err<<"像素"<<'\n';
        fout<<"\t第"<<i+1<<"幅图像的平均误差："<<err<<"像素"<<'\n';
    }

    cout<<"\t总体平均误差："<<total_err/image_count<<"像素"<<'\n';
    fout<<"总体平均误差："<<total_err/image_count<<"像素"<<'\n'<<'\n';
    cout<<"评价完成！\n";

    /************************************************************************
    Step5：保存定标结果
    *************************************************************************/

    cout<<"开始保存定标结果………………";

    Matrix<double> rotation_vector(3,1); /* 保存每幅图像的旋转向量 */
    Matrix<double> rotation_matrix(3,3); /* 保存每幅图像的旋转矩阵 */

    fout<<"相机内参数矩阵：\n";
    fout<<intrinsic_matrix<<'\n';

    fout<<"畸变系数：\n";
    fout<<distortion_coeffs<<'\n';/*保存成txt格式，便于观看*/


    cvSave("Intrinsics.xml",intrinsic_matrix.cvmat);/*保存成xml格式，便于调用*/
    cvSave("Distortion.xml",distortion_coeffs.cvmat);

    for (i=0;i<image_count;i++) 
    {
        fout<<"第"<<i+1<<"幅图像的旋转向量：\n";
        fout<<rotation_vectors.get_col(i);

        /* 将旋转向量转换为相对应的旋转矩阵 */
        for (j=0;j<3;j++) 
        {
            rotation_vector(j,0,0) = rotation_vectors(0,i,j);
        }
        cvRodrigues2(rotation_vector.cvmat,rotation_matrix.cvmat);

        fout<<"第"<<i+1<<"幅图像的旋转矩阵：\n";
        fout<<rotation_matrix;

        fout<<"第"<<i+1<<"幅图像的平移向量：\n";
        fout<<translation_vectors.get_col(i)<<'\n';

    }

实际操作中要注意以下几点：
1、棋盘的排放
算法是基于2D模型的，如果棋盘摆放的不平整，肯定会造成很大的影像。
2、图像的数目
一般要大于10个最好
3、图片的角度
这里注意的是图片的角度是45度最好，但是太大的角度对于角点提取的精度影像比较大，所以保持在45度以内比较好
4、要保证标定板的完整