手把手教你OpenCV利用张氏标定法进行相机标定（二）

由于刚到德国，办手续、买东西花了快一周的时间，今天刚有课余闲暇时间，就赶紧接着上一讲继续写。
在上一讲 手把手教你OpenCV利用张氏标定法进行相机标定（一）中，我们完成了标定板的制作，得到了不同相机位姿下的标定板图片。这一讲中，我们将利用强大的图像处理工具——OpenCV，教大家如何进行相机的标定。

1.函数介绍

张氏相机标定法简便易用且精度较高，因此相关的数学基础早已被转化为程序封装在了OpenCV的函数库里。其算法将会在下一章详细介绍，这里我们只需知道其流程就可以了。

1.1程序流程

（1）提取角点
（2）提取亚像素角点
（3）画出角点
（4）参数标定
（5）评价标定结果
以上步骤可以简记为“一提二亚三画四标五评”。接下来，我们就每一步所用到的函数进行介绍。

1.2角点提取函数

OpenCV中我们用到的提取角点的函数是findChessboardCorners()，这真正实现了一行代码走天下的方便快捷，之后还有好几个类似函数，我们先来看看提取角点函数的内部结构吧。

C++:
bool cv::findChessboardCorners ( InputArray image,
Size patternSize,
OutputArray corners,
int flags = CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE
)
Python:
retval, corners = cv.findChessboardCorners( image, patternSize[, corners[, flags]] )

注：以上函数摘自OpenCV-3.4.7文档（下同）。该函数属于OpenCV的calib3d库。
文档很贴心的提供了C++和Python的函数写法，方便大家调用。我就以C++为例，讲解一下各参数的意义。
首先，从整体来看，这个函数实现了对图像是否为棋盘格的判断并定位出棋盘的内角点。函数类型为bool型，因此最终会返回0（未检测到或没检测全）或者非0值。
再具体到每一个参数：
image：输入的棋盘格原图。必须是8位灰度图或彩色图像。
patternSize：指棋盘格图片中的每一行每一列的内角点数。例如，前边我们拍照采用的8x8的图在这里就是（7,7）。
corners：输出检测到的角点阵列。通常定义为vector类型。
flags：默认为0。其他取值为文档所给几个数值的结合。由于这几个数值不太常用，这里就不赘述了，有兴趣的同学可以参考官方文档。OpenCV: Camera Calibration and 3D Reconstruction

1.3亚像素角点提取函数

通过上一个步骤我们检测出了角点的位置，但是这个坐标只是一个大概。为了精确地确定其位置，我们还需要对角点进行亚像素化。
那何谓亚像素？
数码摄像机的成像面的分辨率以像素数量来衡量， 像素中心之间的距离有几个至十几个微米不等。为了最大限度利用图像信息来提高分辨率，人们提出了亚像素概念。意思是说，在两个物理像素之间还有像素，称之为亚像素。它是通过插值计算得出来的。
OpenCV提供了两个可以提取亚像素角点信息的函数：cornerSubPix()和find4QuadCornerSubpix(). 两者并无较大差异，这里都列出来供大家参考一下吧。

C++:
void cv::cornerSubPix ( InputArray image,
InputOutputArray corners,
Size winSize,
Size zeroZone,
TermCriteria criteria
)
Python:
corners = cv.cornerSubPix( image, corners, winSize, zeroZone, criteria )

该函数属于OpenCV的imgproc库。
参数说明：
image：输入一个单通道、8位或者浮点型图片。
corners：用来储存角点的初始坐标和精确化后的角点坐标。
winSize：搜索窗口边长的一半。 例如，如果winSize = Size（5,5），则使用（5 * 2 + 1）×（5 * 2 + 1）= 11×11搜索窗口。选择一定的窗口后，它会不断移动，并计算窗口中的亚像素角点。
zeroZone：死区的一般尺寸，死区为不对搜索区的中央位置做求和运算的区域。值（-1，-1）表示没有这个区域大小。
criteria：终止角点优化迭代过程的条件。一般省略。
关于该函数详细的注释见这里：cornerSubPix 这里还有亚像素角点的搜索机制。

C++:
bool cv::find4QuadCornerSubpix ( InputArray img,
InputOutputArray corners,
Size region_size
)
Python:
retval, corners = cv.find4QuadCornerSubpix( img, corners, region_size )

该函数与角点提取函数findChessboardCorners()一样同属calib3d库。这可能也是一般相机标定用这个函数用的比cornerSubPix()多的原因。
参数说明（基本同上一个函数一样）：
img：同样也是输入图片矩阵，最好是8位灰度图。
corners：初始的角点坐标，同时也会作为亚像素角点坐标的输出。
region_size：角点搜索窗口的大小。
最终的结果储存在参数“corners”里面。
该函数的链接：find4QuadCornerSubpix

1.4角点绘制函数

drawChessboardCorners()这个函数顾名思义，就是让程序在照片上画出找到的角点，可以说是编程过程中的检验，可视化的角点也会让动手实践变得更有激励性。
如果代码出了bug，大家也可以通过这一步来判断是之前的步骤出了问题还是之后的出了问题。
下面我们来看看函数组成吧：

C++:
void cv::drawChessboardCorners ( InputOutputArray image,
Size patternSize,
InputArray corners,
bool patternWasFound
)
Python:
image = cv.drawChessboardCorners( image, patternSize, corners, patternWasFound )

该函数同样属于calib3d库。其实呢，想必大家也发现了，calib就是calibration（标定）的缩写，因此OpenCV的这个库就是用来完成相机标定、3D重建等工作的。
参数说明：
image：目标图片，必须是一个8位的彩色图（三通道）。
patternSize：每一幅棋盘格图片中，每行和每列角点的个数。patternSize = cv::Size(points_per_row,points_per_column)。
corners：检测到的角点阵列，也就是上面角点检测函数findChessboardCorners（）里的corners。
patternWasFound：标志位，用来判断是否检测到所有的棋盘内角点。也就是findChessboardCorners这个函数的返回值。
链接在这儿：drawChessboardCorners

1.5相机标定函数

前面铺垫了那么多，其实为的就是给这个函数提供参数。由OpenCV calib3d库提供的calibrateCamera()函数由九个参数组成，仅用一句话完成对相机的标定（求得内参外参）。
OpenCV文档中提供了两种calibrateCamera()函数，我们这里用的是第二种，两者区别是第一种多输出了内外偏差和视差参数。

C++:
double cv::calibrateCamera ( InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints,
Size imageSize,
InputOutputArray cameraMatrix,
InputOutputArray distCoeffs,
OutputArrayOfArrays rvecs,
OutputArrayOfArrays tvecs,
int flags = 0,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON)
)
Python:
retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv.calibrateCamera( objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs[, rvecs[, tvecs[, flags[, criteria]]]] )

参数说明如下：
objectPoints：一系列角点的三维坐标。它是校准图案坐标空间中校准图案点的向量的向量，这话说的非常拗口，简单来说这就是个三维坐标点，通过两层的向量来表示。申明方式如下：std::vector(std::vector(cv::Vec3f))objectPoints.
imagePoints:角点投影到标定图案平面上的二维坐标点。
imageSize：图片的尺寸大小，用以初始化相机内参。
cameraMatrix：相机的内参矩阵，即$$A=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$
此处内参矩阵的参数是否需要初始化与标志位选取值有关。
distCoeffs：相机的畸变参数矩阵，有5个畸变参数：k1,k2,p1,p2,k3。
rvecs：旋转向量
tvecs：平移向量
flags：表示标定时采用的算法，默认为0。其他选项详见官方文档calibrateCamera
criteria：迭代的终止条件，通常忽略。
这个函数解决的就是一个优化问题——重投影误差，一般使用的算法叫Bundle Adjustment。本着前两讲尽量少些公式的原则，详细的数学推导就留到下一讲好了，这里你只需知道OpenCV默默地帮你完成了一个计算量很大的优化迭代。

1.6重投影函数

完成了相机标定只算完成了标定工作的90%，还有剩下的10%就是对结果的评定。我们对标定结果评价时，就是计算投影点与检测到的亚像素角点坐标的差值。由于是二维的，所以分别对x和y坐标求差值，再求平方根，即求L2范数。
对空间中的三维坐标点进行反向投影由projectPoints() 完成。

C++:
void cv::fisheye::projectPoints ( InputArray objectPoints,
OutputArray imagePoints,
InputArray rvec,
InputArray tvec,
InputArray K,
InputArray D,
double alpha = 0,
OutputArray jacobian = noArray()
)
Python:
imagePoints, jacobian = cv.fisheye.projectPoints( objectPoints, rvec, tvec, K, D[, imagePoints[, alpha[, jacobian]]] )

这里需要注意的是，projectPoints()也有两种，我们这里用的是第二种。第一种是仿射变换，第二种才是我们这的旋转+平移变换。projectPoints
参数说明：
objectPoints：对象点的数组，大小为1xN / Nx1 ，3通道（vector （Point3f）），其中N为图中的点数。
imagePoints：若干张图片对应的若干的内角点的坐标。
rvecs：旋转向量
tvecs：平移向量
K：即上文的矩阵A，相机的内参矩阵。
D：即上文的畸变参数矩阵。
alpha：偏斜系数，这里取0。
jacobian：可选择是否计算雅可比行列式，一般不用。

到此为止，标定程序所要用到的几大函数就介绍完了。

2.编程前注意事项

需要在此说明的是，为了方便程序的调用及结果的显示，事先需要创建两个文本文档“calibimage.txt”和“calibration_result.txt”。第一个文档里存着所有图片的标号，因为程序是按照这个标号顺序来依次读取图片的。像这样：

切记：文档开头一定要顶格写，不能有空格或者空行，否则读取指令会出现问题！
接下来就可以开始愉（ku）快（bi）的敲代码了。

3.完整代码展示

#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

int main(int argc, char **argv) {
    
    ifstream fin("calibimage.txt");            //读取标定图片的路径，与cpp程序在同一路径下
    if (!fin)                                  //检测是否读取到文件
    {
        cerr<<"没有找到文件"<<endl;
    }
    ofstream fout("calibration_result.txt");   //输出结果保存在此文本文件下
    //依次读取每一幅图片，从中提取角点
    cout<<"开始提取角点……"<<endl;
    int image_count = 0;                       //图片数量
    Size image_size;                           //图片尺寸
    Size board_size = Size(7,7);               //标定板每行每列角点个数，共7*7个角点
    vector<Point2f> image_points_buf;          //缓存每幅图检测到的角点
    vector<vector<Point2f>> image_points_seq;  //用一个二维数组保存检测到的所有角点
    string filename;                           //申明一个文件名的字符串
    
    while (getline(fin,filename))              //逐行读取，将行读入字符串   
    {
        image_count++;
        cout<<"image_count = "<<image_count<<endl;
        //读入图片
        Mat imageInput=imread(filename);
        if(image_count == 1)
        {
            image_size.height = imageInput.rows;//图像的高对应着行数
            image_size.width = imageInput.cols; //图像的宽对应着列数
            cout<<"image_size.width = "<<image_size.width<<endl;
            cout<<"image_size.height = "<<image_size.height<<endl;
        }
        //角点检测
        if (findChessboardCorners(imageInput, board_size, image_points_buf) == 0)
        {
            cout<<"can not find the corners "<<endl;
            exit(1);
        }
        else
        {
             Mat view_gray;                      //存储灰度图的矩阵
             cvtColor(imageInput, view_gray, CV_RGB2GRAY);//将RGB图转化为灰度图
             //亚像素精确化（两种方法）
             find4QuadCornerSubpix(view_gray, image_points_buf, Size(5,5));
             //cornerSubPix(view_gray,image_points_buf,Size(5,5));
             image_points_seq.push_back(image_points_buf);//保存亚像素角点
             //在图中画出角点位置
             drawChessboardCorners(view_gray, board_size, image_points_buf, true);//将角点连线
             imshow("Camera calibration", view_gray);
             waitKey(100);                         //等待按键输入
        }
    }
    //输出图像数目
    int total = image_points_seq.size();
    cout<<"total = "<<total<<endl;
    int CornerNum = board_size.width*board_size.height;//一幅图片中的角点数
    //以第一幅图片为例，下同
    cout<<"第一副图片的角点数据:"<<endl;
    for (int i=0; i<CornerNum; i++)
    {
        cout<<"x= "<<image_points_seq[0][i].x<<" ";
        cout<<"y= "<<image_points_seq[0][i].y<<" ";
        cout<<endl;
    }
    cout<<"角点提取完成!\n";
    
    //开始相机标定
    cout<<"开始标定……"<<endl;
    Size square_size = Size(10,10);              //每个小方格实际大小
    vector<vector<Point3f>> object_points;         //保存角点的三维坐标
    Mat cameraMatrix = Mat(3,3,CV_32FC1,Scalar::all(0));//内参矩阵3*3
    Mat distCoeffs = Mat(1,5,CV_32FC1,Scalar::all(0));//畸变矩阵1*5
    vector<Mat> rotationMat;                       //旋转矩阵
    vector<Mat> translationMat;                    //平移矩阵
    //初始化角点三维坐标
    int i,j,t;
    for (t=0; t<image_count; t++)
    {
        vector<Point3f> tempPointSet;
        for (i=0; i<board_size.height; i++)       //行
        {
            for (j=0;j<board_size.width;j++)      //列
            {
                Point3f realpoint;
                realpoint.x = i*square_size.width;
                realpoint.y = j*square_size.height;
                realpoint.z = 0;
                tempPointSet.push_back(realpoint);
            }
        }
        object_points.push_back(tempPointSet);
    }
    vector<int> point_counts;
    for (i=0; i<image_count; i++)
	{
		point_counts.push_back(board_size.width*board_size.height);
	}
    //标定
    calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs,rotationMat, translationMat,0);   //拥有八个参数的标定函数，不过一句话搞定
    cout<<"标定完成！"<<endl;
    
    //对标定结果进行评价
    double total_err = 0.0;                      //所有图像平均误差总和
    double err = 0.0;                            //每幅图像的平均误差
    vector<Point2f> image_pointsre;              //重投影点
    cout<<"\t每幅图像的标定误差：\n";
    fout<<"每幅图像的标定误差：\n";
    for (i=0; i<image_count; i++)
    {
        vector<Point3f> tempPointSet = object_points[i];
        //通过之前标定得到的相机内外参，对三维点进行重投影
        projectPoints(tempPointSet, image_pointsre, rotationMat[i], translationMat[i], cameraMatrix, distCoeffs);
        //计算两者之间的误差
        vector<Point2f> tempImagePoint = image_points_seq[i];
        Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);//变为1*20的矩阵
        Mat image_pointsreMat = Mat(1, image_pointsre.size(), CV_32FC2);
        for (int j = 0 ; j < tempImagePoint.size(); j++)
        {
            tempImagePointMat.at<Vec2f>(0,j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
            image_pointsreMat.at<Vec2f>(0,j) = Vec2f(image_pointsre[j].x, image_pointsre[j].y);
        }
        err = norm(image_pointsreMat, tempImagePointMat, NORM_L2);
        total_err += err/=  point_counts[i];
        cout<<"第"<<i+1<<"幅图像的平均误差为： "<<err<<"像素"<<endl;
        fout<<"第"<<i+1<<"幅图像的平均误差为： "<<err<<"像素"<<endl;
    }
    cout<<"总体平均误差为： "<<total_err/image_count<<"像素"<<endl;
    fout<<"总体平均误差为： "<<total_err/image_count<<"像素"<<endl;
    cout<<"评价完成！"<<endl;
    
    //将标定结果写入txt文件
    cout<<"开始保存结果……"<<endl;
    Mat rotate_Mat = Mat(3,3,CV_32FC1, Scalar::all(0));//保存旋转矩阵
    fout<<"相机内参数矩阵："<<endl;
    fout<<cameraMatrix<<endl<<endl;
    fout<<"畸变系数：\n";   
	fout<<distCoeffs<<endl<<endl<<endl; 
    for (int i=0; i<image_count; i++)
    {
        Rodrigues(rotationMat[i], rotate_Mat); //将旋转向量通过罗德里格斯公式转换为旋转矩阵
        fout<<"第"<<i+1<<"幅图像的旋转矩阵为："<<endl;
        fout<<rotate_Mat<<endl;
        fout<<"第"<<i+1<<"幅图像的平移向量为："<<endl;
        fout<<translationMat[i]<<endl<<endl;
    }
    cout<<"保存完成"<<endl;
    fout<<endl;
    
    return 0;
}

4.运行结果

在Ubuntu下用cmake完成C++的编译后，执行程序：

最终求得的相机内参矩阵和畸变系数分别为：

还记得上一讲中一开始我们进行理论计算得到的$f_x=1124.50$，$f_y=1260$，$c_x=720$，$c_y=540$。比照上面的结果有一些误差，不过基本还是接近的。可能是计算所采取的数据不太精确所致。

最后，附上整个工程的全部代码文件：张氏相机标定法源码+标定板制作代码（附标定图片）

参考链接：
https://blog.csdn.net/hongbin_xu/article/details/78988450