OpenCV 张正友标定法的实现

　　关于张正友标定法的原理，网上的资料很多，本人虽然看了一些，但觉得还没有到能讲的非常清楚的程度，因此不在这里做太多原理描述。有兴趣了解细节的可以看张大神的原文，或者这篇文章。

　　需要大概知道的是，相机标定中内参、外参和畸变参数的概念。

　　内参有五个，分别是:

　　摄像头拍摄到的物体和实际物体在x,y轴上的映射关系（两个参数）。

　　摄像头中心和图像中心的偏移关系（两个参数）。

　　摄像头和镜头安装非完全垂直，存在一个角度的偏差。（一个参数）

　　外参有六个，分别是x,y,z方向上的平移和旋转。

　　有了上面两种参数，我们基本上知道摄像头拍摄到的图像和现实事物的对应关系了，但“畸变”亦不能忽略。它是由于镜头质量等原因导致的2D点的偏移。举个简单的例子就是用摄像头拍摄一个正方形，图像上会变成一个桶形或者其他的形状。在张氏标定法中张大神用“极大似然法”去计算出畸变的各项参数（如果想加深理解，也可以参考本人之前写过的一篇相关的文章）。

　　到此，就介绍完了相机标定三个最为重要的概念。一般我们要处理摄像头的畸变，只要求内参和畸变参数就可以了，而要做双目标定则需要把外参数也求出来。

　　本篇文章主要介绍用OpenCV自带的张正友标定法相关的函数来对摄像头进行标定（求取摄像头内外参数和畸变参数）并对单张图像进行校正的方法。主要参考的是这篇文章（如同雷同，是我抄的）。

　　需要自行准备标定板，长相如下（因为快递比较慢，自己先做了一个，但因为这个的精度直接影响到后面标定的精确度，建议还是买一块给力一点的）：

　　标定的流程是：角点提取->相机标定

　　一.角点提取

　　会用到比较重要的函数是：

//用于提取标定板的内角点，也就是提取上图中每四个黑白格中间的那些角点
bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners, int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE );

　　有四个参数：

　　第一个“Image”，是拍摄到的棋盘图像，也就是上图那样的图像；

　　第二个“patternSize”，即每个棋盘图上内角点的行列数，一般情况下，行列数不要相同，便于后续标定程序识别标定板的方向，像上面那样的板子就是Size(7, 5)，也就是每行7个角点，每列5个角点；

　　第三个“corners”，用于存储检测到的内角点图像坐标位置，一般用元素是Point2f的向量来表示；

　　第四个“flage”：用于定义棋盘图上内角点查找的不同处理方式，有默认值。

　　另外返回值很重要，它会告诉你是不是真的从图中找到了角点。如果后面想做成自动标定的程序，这个非常有用；

　　例如如果输入的图像如上图所示，而我们的第二个参数是Size(10, 5)，则会返回错误；

//用于在初步提取的角点信息上进一步提取亚像素信息，降低相机标定偏差，该方法专门用来获取棋盘图上内角点的精确位置。而比较普遍的提取亚像素角点的方法是cornerSubPix，这里不做赘述
bool find4QuadCornerSubpix(InputArray img, InputOutputArray corners, Size region_size);

　　有五个参数：

　　第一个“mage”，输入的Mat矩阵，最好是8位灰度图像，检测效率更高；

　　第二个“corners”，初始的角点坐标向量，同时作为亚像素坐标位置的输出，所以需要是浮点型数据，一般用元素是Pointf2f/Point2d的向量来表示。也即输入上面findChessboardCorners函数的第三个参数。

　　第三个“winSize”，大小为搜索窗口的一半；

　　第四个“zeroZone”，死区的一半尺寸，死区为不对搜索区的中央位置做求和运算的区域。它是用来避免自相关矩阵出现某些可能的奇异性。当值为（-1，-1）时表示没有死区；

　　第五个“criteria”，定义求角点的迭代过程的终止条件，可以为迭代次数和角点精度两者的组合；

//用于画出求得的角点，以便查看是否标定正确
void drawChessboardCorners( InputOutputArray image, Size patternSize, InputArray corners, bool patternWasFound );  

　　有四个参数：

　　第一个“image”，8位灰度或者彩色图像；

　　第二个“patternSize”，每张标定棋盘上内角点的行列数,即findChessboardCorners的第二个参数；

　　第三个“corners”，角点坐标向量，可用find4QuadCornerSubpix函数的第二个参数输出做输入；

　　第四个“patternWasFound”，标志位，用来指示定义的棋盘内角点是否被完整的探测到，true表示被完整的探测到，函数会用直线依次连接所有的内角点，作为一个整体，false表示有未被探测到的内角点，这时候函数会以（红色）圆圈标记处检测到的内角点；

　　总的查找角点的示例代码如下：

Mat imageInput = imread("1.bmp");
Size board_size = Size(7, 5);//标定板上每行、列的角点数
vector image_points_buf;//缓存每幅图像上检测到的角点
/*提取角点*/
if (!findChessboardCorners(imageInput, board_size, image_points_buf))
{
    cout << "can not find chessboard corners!\n"; //找不到角点  
    return;
}
else
{
    Mat view_gray;
    cvtColor(imageInput, view_gray, CV_RGB2GRAY);
    /*亚像素精确化*/
    find4QuadCornerSubpix(view_gray, image_points_buf, Size(5, 5)); //对粗提取的角点进行精确化  
    drawChessboardCorners(view_gray, board_size, image_points_buf, true); //用于在图片中标记角点  
    imshow("Camera Calibration", view_gray);//显示图片  
    waitKey(0);     
}

　　二.相机标定

　　利用上面获取到的图像角点（理论上需要三张图像，即三组数据，事实上以10~20张为宜，因为这样误差会比较小），便可以用calibrateCamera函数做摄像头标定，计算出摄像头的内参、外参和畸变参数了。当然前面代码在本人只做了一张图像的角点提取，可以改成求多张的，代码如下：

Size board_size = Size(7, 5);//标定板上每行、列的角点数
vector image_points_buf;//缓存每幅图像上检测到的角点
vector<vector> image_points_seq; //保存检测到的所有角点
/* 提取角点 */
char filename[10];
for (size_t image_num = 1; image_num <= 14; image_num++)
{
    sprintf_s(filename, "%d.bmp", image_num);
    Mat imageInput = imread(filename);
    if (!findChessboardCorners(imageInput, board_size, image_points_buf))
    {
        cout << "can not find chessboard corners!\n"; //找不到角点  
        return;
    }
    else
    {
        Mat view_gray;
        cvtColor(imageInput, view_gray, CV_RGB2GRAY);
        /*亚像素精确化*/
        find4QuadCornerSubpix(view_gray, image_points_buf, Size(5, 5)); //对粗提取的角点进行精确化  
        drawChessboardCorners(view_gray, board_size, image_points_buf, true); //用于在图片中标记角点  
        image_points_seq.push_back(image_points_buf);//保存亚像素角点  
        imshow("Camera Calibration", view_gray);//显示图片  
        waitKey(500);//停半秒
    }
    imageInput.release();
}

double calibrateCamera(InputArrayOfArrays objectPoints,  
                       InputArrayOfArrays imagePoints,  
                       Size imageSize,  
                       CV_OUT InputOutputArray cameraMatrix,  
                       CV_OUT InputOutputArray distCoeffs,  
                       OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,  
                       int flags=0, TermCriteria criteria = 
                       TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON));  

　　参数好多，有九个之多。。。

　　第一个“objectPoints”，为世界坐标系中的三维点。在使用时，应该输入一个三维坐标点的向量集合。一般我们假定标定板放在z=0的平面上，然后依据棋盘上单个黑白方块的大小（也可以直接都取10，如果不需要很准确的映射到现实事物的话）可以计算出每个内角点的世界坐标。

　　第二个“imagePoints”，为每一个内角点对应的图像坐标点。也即是上面求得的各张图像的角点集合；

　　第三个“imageSize”，为图像的像素尺寸大小，在计算相机的内参和畸变矩阵时需要使用到该参数；

　　第四个“cameraMatrix”为相机的内参矩阵。输入一个Mat cameraMatrix即可，如Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0));

　　第五个“distCoeffs“为畸变矩阵。输入一个Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0));即可。

　　第六个“rvecs”为旋转向量；应该输入一个Mat类型的vector，即vectorrvecs;

　　第七个“tvecs”为位移向量，和rvecs一样，应该为vector tvecs；

　　第八个“flags”为标定时所采用的算法。有如下几个参数（直接不写则依据下面参数描述中没设参数的情况进行）：

CV_CALIB_USE_INTRINSIC_GUESS：使用该参数时，在cameraMatrix矩阵中应该有fx,fy,u0,v0的估计值。否则的话，将初始化(u0,v0）图像的中心点，使用最小二乘估算出fx，fy。 
CV_CALIB_FIX_PRINCIPAL_POINT：在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置，光轴点将保持在中心或者某个输入的值。 
CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只将fy作为可变量，进行优化计算。当CV_CALIB_USE_INTRINSIC_GUESS没有被设置，fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到。 
CV_CALIB_ZERO_TANGENT_DIST：设定切向畸变参数（p1,p2）为零。 
CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6：对应的径向畸变在优化中保持不变。 
CV_CALIB_RATIONAL_MODEL：计算k4，k5，k6三个畸变参数。如果没有设置，则只计算其它5个畸变参数。

　　第九个“criteria“是最优迭代终止条件设定。

　　在使用该函数进行标定运算之前，需要对棋盘上每一个内角点的空间坐标系的位置坐标进行初始化，标定的结果是生成相机的内参矩阵cameraMatrix、相机的5个畸变系数distCoeffs，另外每张图像都会生成属于自己的平移向量和旋转向量。

　　具体的实现代码如下：

Size image_size;//图像的尺寸
Size board_size = Size(7, 5);     //标定板上每行、列的角点数
vector image_points_buf;  //缓存每幅图像上检测到的角点
vector<vector> image_points_seq; //保存检测到的所有角点
/*提取角点*/
char filename[10];
for (size_t image_num = 1; image_num <= IMGCOUNT; image_num++)
{
    sprintf_s(filename, "%d.bmp", image_num);
    Mat imageInput = imread(filename);
    if (!findChessboardCorners(imageInput, board_size, image_points_buf))
    {
        cout << "can not find chessboard corners!\n";//找不到角点  
        return;
    }
    else
    {
        Mat view_gray;
        cvtColor(imageInput, view_gray, CV_RGB2GRAY);
        /*亚像素精确化*/
        find4QuadCornerSubpix(view_gray, image_points_buf, Size(5, 5));//对粗提取的角点进行精确化  
        drawChessboardCorners(view_gray, board_size, image_points_buf, true);//用于在图片中标记角点  
        image_points_seq.push_back(image_points_buf);//保存亚像素角点  
        imshow("Camera Calibration", view_gray);//显示图片  
        //waitKey(500);//停半秒
    }
    image_size.width = imageInput.cols;
    image_size.height = imageInput.rows;
    imageInput.release();
}
/*相机标定*/
vector<vector> object_points; //保存标定板上角点的三维坐标,为标定函数的第一个参数
Size square_size = Size(10, 10);//实际测量得到的标定板上每个棋盘格的大小，这里其实没测，就假定了一个值，感觉影响不是太大，后面再研究下
for (int t = 0; tvector tempPointSet;
    for (int i = 0; ifor (int j = 0; j//假设标定板放在世界坐标系中z=0的平面上
            realPoint.x = i*square_size.width;
            realPoint.y = j*square_size.height;
            realPoint.z = 0;
            tempPointSet.push_back(realPoint);
        }
    }
    object_points.push_back(tempPointSet);
}
//内外参数对象
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//摄像机内参数矩阵
vector<int> point_counts;// 每幅图像中角点的数量  
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));//摄像机的5个畸变系数：k1,k2,p1,p2,k3
vector tvecsMat;//每幅图像的旋转向量
vector rvecsMat;//每幅图像的平移向量
calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);//摄像头标定

　　到此我们已经完成了标定的过程，得到了摄像头的各个参数，后面就可以用这些得到的参数来做摄像头的矫正了。

　　矫正可以使用下面的函数：

void undistort( InputArray src, OutputArray dst,InputArray cameraMatrix,InputArray distCoeffs,InputArray newCameraMatrix=noArray() );  

　　有五个参数：

　　第一个“src”，输入参数，代表畸变的原始图像；

　　第二个“dst”，矫正后的输出图像，跟输入图像具有相同的类型和大小；

　　第三个“cameraMatrix”为之前求得的相机的内参矩阵；

　　第四个“distCoeffs”为之前求得的相机畸变矩阵；

　　第五个“newCameraMatrix”，默认跟cameraMatrix保持一致；

　　具体代码如下：

/*用标定的结果矫正图像*/
for (size_t image_num = 1; image_num <= IMGCOUNT; image_num++)
{
    sprintf_s(filename, "%d.bmp", image_num);
    Mat imageSource = imread(filename);
    Mat newimage = imageSource.clone();
    undistort(imageSource, newimage, cameraMatrix, distCoeffs);
    imshow("source", imageSource);//显示图片 
    imshow("drc", newimage);//显示图片  
    waitKey(500);//停半秒
    imageSource.release();
    newimage.release();
}

　　到此就完成了摄像头的标定和图像的矫正的整个流程，如果想要知道标定的效果如何评定，可以参考上文提到过的参考文章，本文的很多函数说明基本是照搬的，只是做了一些代码上的拆分。

　　如果想要把标定的结果保存下载后面直接用，则可以如下代码保存：

/*保存内参和畸变系数，以便后面直接矫正*/
ofstream fout("caliberation_result.txt");//保存标定结果的文件
fout << "相机内参数矩阵：" << endl;
fout << cameraMatrix << endl << endl;
fout << "畸变系数：\n";
fout << distCoeffs << endl << endl << endl;
fout.close();

　　读取标定文件文件代码如下：

char read[100];
double getdata;
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//摄像机内参数矩阵 
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));//摄像机的5个畸变系数：k1,k2,p1,p2,k3
ifstream fin("caliberation_result.txt");//读取保存标定结果的文件，以供矫正
fin >> read;
fin.seekg(3, ios::cur);
for (size_t j = 0; j < 3; j++)
    for (size_t i = 0; i < 3; i++)  
    {
        fin >> getdata;
        cameraMatrix.at<float>(j, i) = getdata;
        fin >> read;
    }
fin >> read;
fin.seekg(3, ios::cur);
for (size_t i = 0; i < 5; i++)
{
    fin >> getdata;
    distCoeffs.at<float>(i) = getdata;
    fin >> read;
}   
fin.close();

　　本文的全部代码如下：

#include "opencv2/core/core.hpp"  
#include "opencv2/imgproc/imgproc.hpp"  
#include "opencv2/calib3d/calib3d.hpp"  
#include "opencv2/highgui/highgui.hpp"  
#include   
#include   

#define IMGCOUNT 20

using namespace cv;
using namespace std;

void main()
{
    Size image_size;//图像的尺寸
    Size board_size = Size(9, 6);     //标定板上每行、列的角点数
    vector image_points_buf;  //缓存每幅图像上检测到的角点
    vector<vector> image_points_seq; //保存检测到的所有角点
    /*提取角点*/
    char filename[10];
    for (size_t image_num = 1; image_num <= IMGCOUNT; image_num++)
    {
        sprintf_s(filename, "%d.bmp", image_num);
        Mat imageInput = imread(filename);
        if (!findChessboardCorners(imageInput, board_size, image_points_buf))
        {
            cout << "can not find chessboard corners!\n";//找不到角点  
            return;
        }
        else
        {
            Mat view_gray;
            cvtColor(imageInput, view_gray, CV_RGB2GRAY);
            /*亚像素精确化*/
            find4QuadCornerSubpix(view_gray, image_points_buf, Size(5, 5));//对粗提取的角点进行精确化  
            drawChessboardCorners(view_gray, board_size, image_points_buf, true);//用于在图片中标记角点  
            image_points_seq.push_back(image_points_buf);//保存亚像素角点  
            imshow("Camera Calibration", view_gray);//显示图片  
            waitKey(500);//停半秒
        }
        image_size.width = imageInput.cols;
        image_size.height = imageInput.rows;
        imageInput.release();
    }
    /*相机标定*/
    vector<vector> object_points; //保存标定板上角点的三维坐标,为标定函数的第一个参数
    Size square_size = Size(10, 10);//实际测量得到的标定板上每个棋盘格的大小，这里其实没测，就假定了一个值，感觉影响不是太大，后面再研究下
    for (int t = 0; tvector tempPointSet;
        for (int i = 0; ifor (int j = 0; j//假设标定板放在世界坐标系中z=0的平面上
                realPoint.x = i*square_size.width;
                realPoint.y = j*square_size.height;
                realPoint.z = 0;
                tempPointSet.push_back(realPoint);
            }
        }
        object_points.push_back(tempPointSet);
    }
    //内外参数对象
    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//摄像机内参数矩阵
    vector<int> point_counts;// 每幅图像中角点的数量  
    Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));//摄像机的5个畸变系数：k1,k2,p1,p2,k3
    vector tvecsMat;//每幅图像的旋转向量
    vector rvecsMat;//每幅图像的平移向量
    calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);//相机标定
    /*用标定的结果矫正图像*/
    for (size_t image_num = 1; image_num <= IMGCOUNT; image_num++)
    {
        sprintf_s(filename, "%d.bmp", image_num);
        Mat imageSource = imread(filename);
        Mat newimage = imageSource.clone();
        undistort(imageSource, newimage, cameraMatrix, distCoeffs);
        imshow("source", imageSource);//显示图片 
        imshow("drc", newimage);//显示图片  
        sprintf_s(filename, "%d_d.bmp", image_num);
        imwrite(filename, newimage);//显示图片  
        waitKey(500);//停半秒
        imageSource.release();
        newimage.release();
    }
    /*保存内参和畸变系数，以便后面直接矫正*/
    ofstream fout("caliberation_result.txt");//保存标定结果的文件
    fout << "相机内参数矩阵：" << endl;
    fout << cameraMatrix << endl << endl;
    fout << "畸变系数：\n";
    fout << distCoeffs << endl << endl << endl;
    fout.close();

    ///*读取之前标定好的数据直接矫正*/
    //char read[100];
    //double getdata;
    //Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//摄像机内参数矩阵 
    //Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));//摄像机的5个畸变系数：k1,k2,p1,p2,k3
    //ifstream fin("caliberation_result.txt");//读取保存标定结果的文件，以供矫正
    //fin >> read;
    //fin.seekg(3, ios::cur);
    //for (size_t j = 0; j < 3; j++)
    //  for (size_t i = 0; i < 3; i++)  
    //  {
    //      fin >> getdata;
    //      cameraMatrix.at(j, i) = getdata;
    //      fin >> read;
    //  }
    //fin >> read;
    //fin.seekg(3, ios::cur);
    //for (size_t i = 0; i < 5; i++)
    //{
    //  fin >> getdata;
    //  distCoeffs.at(i) = getdata;
    //  fin >> read;
    //} 
    //fin.close();

    //char filename[10];
    //for (size_t image_num = 1; image_num <= IMGCOUNT; image_num++)
    //{
    //  sprintf_s(filename, "%d.bmp", image_num);
    //  Mat imageSource = imread(filename);
    //  Mat newimage = imageSource.clone();
    //  undistort(imageSource, newimage, cameraMatrix, distCoeffs);
    //  imshow("source", imageSource);//显示图片 
    //  imshow("drc", newimage);//显示图片  
    //  sprintf_s(filename, "%d_d.bmp", image_num);
    //  imwrite(filename, newimage);//显示图片  
    //  waitKey(500);//停半秒
    //  imageSource.release();
    //  newimage.release();
    //}
}