第六节、双目视觉之相机标定、图片矫正

双目测距的博客已经很多了，由于我非计算机出身的，对计算机视觉知识很欠缺，在看前辈博客的时候还有点吃力，通过记录对双目视觉的学习可以让我更进一步，同时在前辈的博文上稍微啰嗦一些，可以让更多像我一样的小伙伴可以看得懂。

双目视觉是建立在几何数学的基础上，数学推导是枯燥乏味的。因此这里不去过多的介绍数学原理，只是简要的叙述一下双目视觉的流程。

双目视觉主要包括相机标定、图片矫正、摄像机校正、图片匹配、3D恢复五个部分。

下面我们从相机标定开始说起。

一、三大坐标系

谈到相机标定，我们不得不说起摄相机坐标系、世界坐标系、图像坐标系。

上图是三个坐标的示意简图，通过它大家可以对三个坐标有一个直观的认识。

世界坐标系（X_W,Y_W,Z_W）：目标物体位置的参考系。除了无穷远，世界坐标可以根据运算方便与否自由放置，单位为长度单位如mm。。在双目视觉中世界坐标系主要有三个用途：

1、标定时确定标定物的位置；

2、作为双目视觉的系统参考系，给出两个摄像机相对世界坐标系的关系，从而求出相机之间的相对关系；

3、作为重建得到三维坐标的容器，存放重建后的物体的三维坐标。世界坐标系是将看见中物体纳入运算的第一站。

摄像机坐标系(X_C,Y_C,Z_C)：摄像机站在自己角度上衡量的物体的坐标系。摄像机坐标系的原点在摄像机的光心上，z轴与摄像机光轴平行。它是与拍摄物体发生联系的桥头堡，世界坐标系下的物体需先经历刚体变化转到摄像机坐标系，然后在和图像坐标系发生关系。它是图像坐标与世界坐标之间发生关系的纽带，沟通了世界上最远的距离。单位为长度单位如mm。

图像坐标系(x,y):为了描述成像过程中物体从相机坐标系到图像坐标系的投影透射关系而引入，方便进一步得到像素坐标系下的坐标。单位为长度单位如mm。

像素坐标系(u,v) ：为了描述物体成像后的像点在数字图像上（相片）的坐标而引入，是我们真正从相机内读取到的信息所在的坐标系。单位为像素数目。

(备注：)有很多人把图像坐标系和像素坐标系合在一起，称作三大坐标系，也有人分开，称为四大坐标系。

1、图像坐标系到像素坐标系

讲到这里，你可能会问有了图像坐标系为什么还要建一个像素坐标系？

我们以图像左上角为原点建立以像素为单位的直接坐标系u-v。像素的横坐标u与纵坐标v分别是在其图像数组中所在的列数与所在行数。

由于(u,v)只代表像素的列数与行数，而像素在图像中的位置并没有用物理单位表示出来，所以，我们还要建立以物理单位（如毫米）表示的图像坐标系x-y。将相机光轴与图像平面的交点（一般位于图像平面的中心处，也称为图像的主点（principal point）定义为该坐标系的原点O1，且x轴与u轴平行，y轴与v轴平行，假设(u0,v0)代表O1在u-v坐标系下的坐标，dx与dy分别表示每个像素在横轴x和纵轴y上的物理尺寸，则图像中的每个像素在u-v坐标系中的坐标和在x-y坐标系中的坐标之间都存在如下的关系：

其中，我们假设物理坐标系中的单位为毫米，那么dx的的单位为:毫米/像素。那么x/dx的单位就是像素了，即和u的单位一样都是像素。为了使用方便，可将上式用齐次坐标与矩阵形式表示为：

为了让你更直接的理解这一块内容，我们举个例子。由于被摄像机摄物体的图像经过镜头投影到CCD芯片上(像平面)，我们设CCD的大小为8x8mm,而拍摄到的图像大小为640x400，则dx=8/640mm/像素，dy=8/400mm/像素,u0=320,v0=200。

上面的矩阵公式运用了齐次坐标，初学者可能会感到有些迷惑。大家会问：怎样将普通坐标转换为齐次坐标呢？齐次坐标能带来什么好处呢？

这里对齐次坐标做一个通俗的解释。此处只讲怎么将普通坐标改写为齐次坐标及为什么引入齐次坐标。这里只做一个通俗但不太严谨的表述。力求简单明了。针对齐次坐标的严谨的纯数学推导，可参见“周兴和版的《高等几何》---1.3拓广平面上的齐次坐标”。玉米曾详细读过《高等几何》这本书，但觉得离计算机视觉有点远，是讲纯数学的投影关系的，较为生涩难懂。

齐次坐标可以理解为在原有坐标后面加一个“小尾巴”。将普通坐标转换为齐次坐标，通常就是在增加一个维度，这个维度上的数值为1。如图像坐标系(u,v)转换为(u,v,1)一样。对于无穷远点，小尾巴为0。注意，给零向量增加小尾巴，数学上无意义。

那么，为什么计算机视觉在坐标运算时要加上这个“小尾巴”呢？

1、将投影平面扩展到无穷远点。如对消隐点(vanishing point)的描述。

2、使得计算更加规整

如果用普通坐标来表达的话，会是下面的样子：

这样的运算形式会给后与运算带来一定的麻烦，所以齐次坐标是一个更好的选择。

齐次坐标还有一个重要的性质，伸缩不变性。即：设齐次坐标M，则αM=M。

我们介绍过了像素坐标系之后，我们再次三大坐标系的问题上。我们想知道这三个坐标系有什么样的关系，我们先从下图说起：

图中显示，世界坐标系通过刚体变换到达摄像机坐标系，然后摄像机坐标系通过透视投影变换到达图像坐标系。可以看出，世界坐标与图像坐标的关系建立在刚体变换和透视投影变换的基础上。

2、世界坐标系到摄像机坐标系

首先，让我们来看一下刚体变换是如何将世界坐标系与图像坐标系联系起来的吧。这里，先对刚体变换做一个介绍：

刚体变换(regidbody motion):三维空间中，当物体不发生形变时，对一个几何物体作旋转，平移的运动，称之为刚体变换。

因为世界坐标系和摄像机坐标都是右手坐标系，所以其不会发生形变。我们想把世界坐标系下的坐标转换到摄像机坐标下的坐标，如下图所示，可以通过刚体变换的方式。空间中一个坐标系，总可以通过刚体变换转换到另外一个个坐标系的。

下面看一下，二者之间刚体变换的数学表达：

对应的齐次表达式为：

其中，R为旋转矩阵，t为平移向量，R、T与摄像机无关，所以称这两个参数为摄像机的外参数(extrinsic parameter)，可以理解为两个坐标原点之间的距离，因其受x,y,z三个方向上的分量共同控制，所以其具有三个自由度。

我们假定在世界坐标系中物点所在平面过世界坐标系原点且与Zw轴垂（也即棋盘平面与Xw-Yw平面重合，目的在于方便后续计算），则Zw=0。 $X_{W} - Y_{W}$

3、摄像机坐标系到图像坐标系

首先，让我们来看一下透视投影是如何将图像坐标系与图像坐标系联系起来的吧。这里，先对透视投影做一个介绍：

透视投影(perspective projection): 用中心投影法将形体投射到投影面上，从而获得的一种较为接近视觉效果的单面投影图。有一点像皮影戏。它符合人们心理习惯，即离视点近的物体大，离视点远的物体小，不平行于成像平面的平行线会相交于消隐点(vanish point)

这里我们还是拿针孔成像来说明（除了成像亮度低外，成像效果和透视投影是一样的，但是光路更简单）

下图是针孔-摄像机的基本模型。平面π称为摄像机的像平面，点Oc称 $O_{c}$

如图所示，图像坐标系为o-xy，摄像机坐标系为Ox-x_cy_cz_c $O_{c} - x_{c} y_{c} z_{c}$

$O_{c} - x_{c} y_{c} z_{c}$

它的像点m在图像坐标系中的齐次坐标记为：

根据三角形相似原理，可得：

我们使用矩阵表示为：

注意由于齐次坐标的伸缩不变性，z_c[x,y,1]^T和(x,y,1)^T表示的是同一点。

4、总结

我们已经介绍了各个坐标系之间的转换过程，但是我们想知道的是如何从世界坐标系转换到像素坐标系，因此我们需要把上面介绍到的联系起来：

将三者相乘，可以把这三个过程和在一起，写成一个矩阵：

我们取世界坐标到图像坐标变换矩阵P如下：

P就表示了一个投影相机，有下面公式：

其中：

我们设：

最后用一幅图来总结从世界坐标系到像素坐标系（不考虑畸变）的转换关系：

二、图片矫正

我们在摄像机坐标系到图像坐标系变换时谈到透视投影。摄像机拍照时通过透镜把实物投影到像平面上，但是透镜由于制造精度以及组装工艺的偏差会引入畸变，导致原始图像的失真。因此我们需要考虑成像畸变的问题。

透镜的畸变主要分为径向畸变和切向畸变，还有薄透镜畸变等等，但都没有径向和切向畸变影响显著，所以我们在这里只考虑径向和切向畸变。

径向畸变

顾名思义，径向畸变就是沿着透镜半径方向分布的畸变，产生原因是光线在原理透镜中心的地方比靠近中心的地方更加弯曲，这种畸变在普通廉价的镜头中表现更加明显，径向畸变主要包括桶形畸变和枕形畸变两种。以下分别是枕形和桶形畸变示意图：

它们在真实照片中是这样的：

像平面中心的畸变为0，沿着镜头半径方向向边缘移动，畸变越来越严重。畸变的数学模型可以用主点（principle point）周围的泰勒级数展开式的前几项进行描述，通常使用前两项，即k1和k2，对于畸变很大的镜头，如鱼眼镜头，可以增加使用第三项k3来进行描述，成像仪上某点根据其在径向方向上的分布位置，调节公式为：

式里（x0，y0）是畸变点在像平面的原始位置，（x，y）是畸变较正后新的位置，下图是距离光心不同距离上的点经过透镜径向畸变后点位的偏移示意图，可以看到，距离光心越远，径向位移越大，表示畸变也越大，在光心附近，几乎没有偏移。

切向畸变

切向畸变是由于透镜本身与相机传感器平面（像平面）或图像平面不平行而产生的，这种情况多是由于透镜被粘贴到镜头模组上的安装偏差导致。畸变模型可以用两个额外的参数p1和p2来描述：

下图显示某个透镜的切向畸变示意图，大体上畸变位移相对于左下——右上角的连线是对称的，说明该镜头在垂直于该方向上有一个旋转角度。

径向畸变和切向畸变模型中一共有5个畸变参数，在Opencv中他们被排列成一个5*1的矩阵，依次包含k1、k2、p1、p2、k3，经常被定义为Mat矩阵的形式，如Mat distCoeffs=Mat（1,5，CV_32FC1，Scalar::all（0））；这5个参数就是相机标定中需要确定的相机的5个畸变系数。求得这5个参数后，就可以校正由于镜头畸变引起的图像的变形失真，下图显示根据镜头畸变系数校正后的效果：

三、张氏标定法

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

上文中我们已经得到了像素坐标系和世界坐标系下的坐标映射关系，我们 $u 、 v$

其中，u,v表示 $u 、 v$

$u 、 v$

我们如果求取H的值？

大家可以分析一下，H是一个三3*3的矩阵，并且有一个元素是作为齐次坐标。因此，H有8个未知量待解。

(x_w,y_w)作为标定物的空间坐标，可以由设计者人为控制，是已知量。(u,v)是像素坐标，我们可以直接通过摄像机获得。对于一组对应(x_w,y_w)-(u,v)我们可以获得两组方程。

现在有8个未知量需要求解，所以我们至少需要八个方程。所以需要四个对应点。四点即可算出，图像平面到世界平面的单应性矩阵H。

这也是张氏标定采用四个角点的棋盘格作为标定物的一个原因。张氏标定就是利用一张打印的棋盘格，然后对每个角点进行标记其在像素坐标系的像素点坐标，以及在世界坐标系的坐标，张氏标定证明通过4组以上的点就可以求解出H矩阵的值，但是为了减少误差，具有更强的鲁棒性，我们一般会拍摄许多张照片，选取大量的角点进行标定。具体过程如下：

打印一张棋盘格标定图纸，将其贴在平面物体的表面.
拍摄一组不同方向棋盘格的图片，可以通过移动相机来实现，也可以移动标定图片来实现。
对于每张拍摄的棋盘图片，检测图片中所有棋盘格的特征点（角点，也就是下图中黑白棋盘交叉点，中间品红色的圆圈内就是一个角点）。我们定义打印的棋盘图纸位于世界坐标系z_w=0的平面上，世界坐标系的原点位于棋盘图纸的固定一角（比如下图中黄色点）。像素坐标系原点位于图片左上角。

因为棋盘标定图纸中所有角点的空间坐标是已知的，这些角点对应在拍摄的标定图片中的角点的像素坐标也是已知的，如果我们得到这样的N>=4个匹配点对（越多计算结果越鲁棒），就可以根据LM等优化方法得到单应性矩阵H。

张正友教授从数学推导上证明了张氏标定算法的可行性。但在实际标定过程中，一般使用最大似然估计进行优化。假设我们拍摄了n张标定图片，每张图片里有m个棋盘格角点。三维空间点X_j(x_w,y_w,z_w)经过相机内参M，外参R，t变换后得到的二维像素为x’(u,v)，假设噪声是独立同分布的，我们通过最小化x_ij(实际值：棋盘格角点在像素坐标系下的实际值), x’(估计值)的位置来求解上述最大似然估计问题：

现在我们来考虑透镜畸变的影响，由于径向畸变的影响相对较明显，所以主要考虑径向畸变参数，根据经验，通常只考虑径向畸变的前两个参数k1,k2就可以（增加更多的参数会使得模型变的复杂且不稳定）。实际求解中，通常把k1,k2也作为参数加入上述函数一起进行优化，待优化函数如下所示：

极大似然估计是一种估计总体未知参数的方法。它主要用于点估计问题。所谓点估计是指用一个估计量的观测值来估计未知参数的真值。说穿了就一句话：就是在参数空间中选取使得样本取得观测值的概率最大的参数。

这里我没有过多的介绍张氏标定法的数学推导过程，感兴趣的童鞋可以看一下博客最后给出来的链接。

四、使用opencv实现单目标定

相机标定的目的：获取摄像机的内参和外参矩阵（同时也会得到每一幅标定图像的选择和平移矩阵），内参和外参系数可以对之后相机拍摄的图像就进行矫正，得到畸变相对很小的图像。

相机标定的输入：标定图像上所有内角点的图像坐标，标定板图像上所有内角点的空间三维坐标（一般情况下假定图像位于Z=0平面上）。

相机标定的输出：摄像机的内参、外参系数。

这三个基础的问题就决定了使用Opencv实现张正友法标定相机的标定流程、标定结果评价以及使用标定结果矫正原始图像的完整流程：

1. 准备标定图片
2. 对每一张标定图片，提取角点信息
3.对每一张标定图片，进一步提取亚像素角点信息
4. 在棋盘标定图上绘制找到的内角点（非必须，仅为了显示）
5. 相机标定
6. 对标定结果进行评价
7. 查看标定效果——利用标定结果对棋盘图进行矫正

准备标定图片

标定图片需要使用标定板在不同位置、不同角度、不同姿态下拍摄，最少需要3张，以10~20张为宜。标定板需要是黑白相间的矩形构成的棋盘图，制作精度要求较高。

这里我们使用OpenCV提供的sample程序中的标定图片，图片位于opencv(C++版本)的安装路径：opencv\sources\samples\data下：

我们先创建一个C++控制台项目，并把标定图片按如下格式存放：

sample文件夹下有两个文件夹left和right，分别对应左摄像头和右摄像头拍摄到的标定板图片：

filename.txt存放标定图片的路径，内容如下：

关于OpenCV提供的用于相机标定的API函数可以查看博客双目视觉标定程序讲解，单目标定的代码如下：

/*************************************************************************************
*
*   Description：相机标定，张氏标定法  单目标定
*   Author     ：JNU
*   Data       ：2018.7.22
*
************************************************************************************/
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <fstream>
#include <vector>

using namespace cv;
using namespace std;

void main(char *args)
{
    //保存文件名称
    std::vector<std::string>  filenames;

    //需要更改的参数
    //左相机标定，指定左相机图片路径，以及标定结果保存文件
    string infilename = "sample/left/filename.txt";        //如果是右相机把left改为right
    string outfilename = "sample/left/caliberation_result.txt";

    //标定所用图片文件的路径,每一行保存一个标定图片的路径  ifstream 是从硬盘读到内存
    ifstream fin(infilename);
    //保存标定的结果  ofstream 是从内存写到硬盘
    ofstream fout(outfilename);

    /*
    1.读取毎一幅图像，从中提取出角点，然后对角点进行亚像素精确化、获取每个角点在像素坐标系中的坐标
    像素坐标系的原点位于图像的左上角
    */
    std::cout << "开始提取角点......" << std::endl;;
    //图像数量
    int imageCount = 0;
    //图像尺寸
    cv::Size imageSize;
    //标定板上每行每列的角点数
    cv::Size boardSize = cv::Size(9, 6);
    //缓存每幅图像上检测到的角点
    std::vector<Point2f>  imagePointsBuf;
    //保存检测到的所有角点
    std::vector<std::vector<Point2f>> imagePointsSeq;
    char filename[100];
    if (fin.is_open())
    {
        //读取完毕？
        while (!fin.eof())
        {
            //一次读取一行
            fin.getline(filename, sizeof(filename) / sizeof(char));
            //保存文件名
            filenames.push_back(filename);
            //读取图片
            Mat imageInput = cv::imread(filename);
            //读入第一张图片时获取图宽高信息
            if (imageCount == 0)
            {
                imageSize.width = imageInput.cols;
                imageSize.height = imageInput.rows;
                std::cout << "imageSize.width = " << imageSize.width << std::endl;
                std::cout << "imageSize.height = " << imageSize.height << std::endl;
            }

            std::cout << "imageCount = " << imageCount << std::endl;
            imageCount++;

            //提取每一张图片的角点
            if (cv::findChessboardCorners(imageInput, boardSize, imagePointsBuf) == 0)
            {
                //找不到角点
                std::cout << "Can not find chessboard corners!" << std::endl;
                exit(1);
            }
            else
            {
                Mat viewGray;
                //转换为灰度图片
                cv::cvtColor(imageInput, viewGray, cv::COLOR_BGR2GRAY);
                //亚像素精确化   对粗提取的角点进行精确化
                cv::find4QuadCornerSubpix(viewGray, imagePointsBuf, cv::Size(5, 5));
                //保存亚像素点
                imagePointsSeq.push_back(imagePointsBuf);
                //在图像上显示角点位置
                cv::drawChessboardCorners(viewGray, boardSize, imagePointsBuf, true);
                //显示图片
                //cv::imshow("Camera Calibration", viewGray);
                cv::imwrite("test.jpg", viewGray);
                //等待0.5s
                //waitKey(500);
            }
        }        
        
        //计算每张图片上的角点数 54
        int cornerNum = boardSize.width * boardSize.height;

        //角点总数
        int total = imagePointsSeq.size()*cornerNum;
        std::cout << "total = " << total << std::endl;

        for (int i = 0; i < total; i++)
        {
            int num = i / cornerNum;
            int p = i%cornerNum;
            //cornerNum是每幅图片的角点个数，此判断语句是为了输出，便于调试
            if (p == 0)
            {                                        
                std::cout << "\n第 " << num+1 << "张图片的数据 -->: " << std::endl;
            }
            //输出所有的角点
            std::cout<<p+1<<":("<< imagePointsSeq[num][p].x;
            std::cout << imagePointsSeq[num][p].y<<")\t";
            if ((p+1) % 3 == 0)
            {
                std::cout << std::endl;
            }
        }

        std::cout << "角点提取完成!" << std::endl;

        /*
        2.摄像机标定 世界坐标系原点位于标定板左上角(第一个方格的左上角)
        */
        std::cout << "开始标定" << std::endl;
        //棋盘三维信息，设置棋盘在世界坐标系的坐标
        //实际测量得到标定板上每个棋盘格的大小
        cv::Size squareSize = cv::Size(26, 26);
        //毎幅图片角点数量
        std::vector<int> pointCounts;
        //保存标定板上角点的三维坐标
        std::vector<std::vector<cv::Point3f>> objectPoints;
        //摄像机内参数矩阵 M=[fx γ u0,0 fy v0,0 0 1]
        cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F, Scalar::all(0));
        //摄像机的5个畸变系数k1,k2,p1,p2,k3
        cv::Mat distCoeffs = cv::Mat(1, 5, CV_64F, Scalar::all(0));
        //每幅图片的旋转向量
        std::vector<cv::Mat> tvecsMat;
        //每幅图片的平移向量
        std::vector<cv::Mat> rvecsMat;

        //初始化标定板上角点的三维坐标
        int i, j, t;
        for (t = 0; t < imageCount; t++)
        {
            std::vector<cv::Point3f> tempPointSet;
            //行数
            for (i = 0; i < boardSize.height; i++)
            {
                //列数
                for (j = 0; j < boardSize.width; j++)
                {
                    cv::Point3f realPoint;
                    //假设标定板放在世界坐标系中z=0的平面上。
                    realPoint.x = i*squareSize.width;
                    realPoint.y = j*squareSize.height;
                    realPoint.z = 0;
                    tempPointSet.push_back(realPoint);
                }
            }
            objectPoints.push_back(tempPointSet);
        }

        //初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板
        for (i = 0; i < imageCount; i++)
        {
            pointCounts.push_back(boardSize.width*boardSize.height);
        }
        //开始标定
        cv::calibrateCamera(objectPoints, imagePointsSeq, imageSize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);
        std::cout << "标定完成" << std::endl;
        //对标定结果进行评价
        std::cout << "开始评价标定结果......" << std::endl;
        //所有图像的平均误差的总和
        double totalErr = 0.0;
        //每幅图像的平均误差
        double err = 0.0;
        //保存重新计算得到的投影点
        std::vector<cv::Point2f> imagePoints2;
        std::cout << "每幅图像的标定误差:" << std::endl;
        fout << "每幅图像的标定误差:" << std::endl;
        for (i = 0; i < imageCount; i++)
        {
            std::vector<cv::Point3f> tempPointSet = objectPoints[i];
            //通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点imagePoints2（在像素坐标系下的点坐标）
            cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, imagePoints2);
            //计算新的投影点和旧的投影点之间的误差
            std::vector<cv::Point2f> tempImagePoint = imagePointsSeq[i];
            cv::Mat tempImagePointMat = cv::Mat(1, tempImagePoint.size(), CV_32FC2);
            cv::Mat imagePoints2Mat = cv::Mat(1, imagePoints2.size(), CV_32FC2);
            for (int j = 0; j < tempImagePoint.size(); j++)
            {
                imagePoints2Mat.at<cv::Vec2f>(0, j) = cv::Vec2f(imagePoints2[j].x, imagePoints2[j].y);
                tempImagePointMat.at<cv::Vec2f>(0, j) = cv::Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
            }
            //Calculates an absolute difference norm or a relative difference norm.
            err = cv::norm(imagePoints2Mat, tempImagePointMat, NORM_L2);
            totalErr += err /= pointCounts[i];
            std::cout << "  第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
            fout<<  "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;

        }
        //每张图像的平均总误差
        std::cout << "  总体平均误差:" << totalErr / imageCount << "像素" << std::endl;
        fout << "总体平均误差:" << totalErr / imageCount << "像素" << std::endl;
        std::cout << "评价完成!" << std::endl;
        //保存标定结果
        std::cout << "开始保存标定结果....." << std::endl;
        //保存每张图像的旋转矩阵
        cv::Mat rotationMatrix = cv::Mat(3, 3, CV_32FC1, Scalar::all(0));
        fout << "相机内参数矩阵:" << std::endl;
        fout << cameraMatrix << std::endl << std::endl;
        fout << "畸变系数:" << std::endl;
        fout << distCoeffs << std::endl << std::endl;

        for (int i = 0; i < imageCount; i++)
        {
            fout << "第" << i + 1 << "幅图像的旋转向量:" << std::endl;
            fout << tvecsMat[i] << std::endl;
            //将旋转向量转换为相对应的旋转矩阵
            cv::Rodrigues(tvecsMat[i], rotationMatrix);
            fout << "第" << i + 1 << "幅图像的旋转矩阵:" << std::endl;
            fout << rotationMatrix << std::endl;
            fout << "第" << i + 1 << "幅图像的平移向量:" << std::endl;
            fout << rvecsMat[i] << std::endl;
        }
        std::cout << "保存完成" << std::endl;

        /************************************************************************
        显示定标结果
        *************************************************************************/
        cv::Mat mapx = cv::Mat(imageSize, CV_32FC1);
        cv::Mat mapy = cv::Mat(imageSize, CV_32FC1);
        cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
        std::cout << "显示矫正图像" << endl;
        for (int i = 0; i != imageCount; i++)
        {
            std::cout << "Frame #" << i + 1 << "..." << endl;
            //金酸图片校正映射矩阵mapx、mapy
            initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
            //读取一张图片
            Mat imageSource = imread(filenames[i]);
            Mat newimage = imageSource.clone();
            //另一种不需要转换矩阵的方式
            //undistort(imageSource,newimage,cameraMatrix,distCoeffs);
            //进行校正
            remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
            imshow("原始图像", imageSource);
            imshow("矫正后图像", newimage);
            waitKey();
        }

        //释放资源
        fin.close();
        fout.close();
        system("pause");        
    }
}

运行后，把相机内部参数和外部参数保存在caliberation_result.txt文件中，内容如下：

每幅图像的标定误差:
第1幅图像的平均误差：0.0644823像素
第2幅图像的平均误差：0.0769712像素
第3幅图像的平均误差：0.057877像素
第4幅图像的平均误差：0.0596713像素
第5幅图像的平均误差：0.0625956像素
第6幅图像的平均误差：0.0658863像素
第7幅图像的平均误差：0.0568134像素
第8幅图像的平均误差：0.0643699像素
第9幅图像的平均误差：0.058048像素
第10幅图像的平均误差：0.0565483像素
第11幅图像的平均误差：0.0590138像素
第12幅图像的平均误差：0.0569968像素
第13幅图像的平均误差：0.0698826像素
总体平均误差:0.0622428像素
相机内参数矩阵:
[530.5277314196954, 0, 338.8371277433631;
 0, 530.5883296858968, 231.5390118666163;
 0, 0, 1]

畸变系数:
[-0.2581406917163123, -0.11124480187392, 0.0004630258905514519, -0.0009475605555950018, 0.413646790569884]

第1幅图像的旋转向量:
[-75.22204622827574;
 -109.7328226714255;
 412.7511174854986]
第1幅图像的旋转矩阵:
[0.9927105083879407, -0.1161407096490343, -0.03220531164846807;
 0.1168004495051158, 0.9929655913965856, 0.01941621224214358;
 0.02972375365863362, -0.02303627280285992, 0.999292664139887]
第1幅图像的平移向量:
[-1.985720132175791;
 -2.010141521348128;
 0.1175016759367312]
第2幅图像的旋转向量:
[-57.88571684656549;
 88.73102475029921;
 365.4767680110305]
第2幅图像的旋转矩阵:
[-0.880518198944593, 0.2965025784551226, -0.36982958548071;
 -0.4330747951156081, -0.8203927789645991, 0.3733656519530371;
 -0.192701642865192, 0.4889191233652108, 0.8507785655767596]
第2幅图像的平移向量:
[-2.431974050326802;
 -0.2015324617416875;
 0.2103186188188722]
第3幅图像的旋转向量:
[-38.96229403649615;
 -101.619482335263;
 328.7991741655258]
第3幅图像的旋转矩阵:
[0.7229826652152683, -0.6501194230369263, -0.2337537199455046;
 0.6686409526220074, 0.7435854196067706, -1.49985835111166e-05;
 0.1738256088007802, -0.1562864662674188, 0.9722958388199968]
第3幅图像的平移向量:
[1.726707502757928;
 2.49410066154742;
 -0.5169212442744683]
第4幅图像的旋转向量:
[-99.94408740929534;
 -67.11904896100746;
 341.7035262057663]
第4幅图像的旋转矩阵:
[-0.4166240767662854, 0.8762113538151707, -0.2422355095852507;
 -0.7194830230098562, -0.4806860756468779, -0.5012834290895748;
 -0.5556694685325433, -0.03456240912595265, 0.8306845861192869]
第4幅图像的平移向量:
[-2.144507828065959;
 -2.137658756455213;
 0.3861555312888436]
第5幅图像的旋转向量:
[63.1817601794685;
 -117.2855578733511;
 327.5340459209377]
第5幅图像的旋转矩阵:
[-0.1237680939389874, -0.9830519969136794, -0.1352413778646805;
 0.8454470843144938, -0.03311262698003439, -0.5330316890754268;
 0.5195196690663707, -0.1803117447603135, 0.8352167312468426]
第5幅图像的平移向量:
[-0.3394208745634724;
 -2.941274925899604;
 0.7239987875443074]
第6幅图像的旋转向量:
[176.6380486063267;
 -65.02048705679623;
 345.2669628180993]
第6幅图像的旋转矩阵:
[-0.4823787195065527, 0.3144101256594393, 0.8175922234525194;
 -0.5902636261183672, -0.8063068742380883, -0.03818476447485269;
 0.6472245534965549, -0.5010144682933011, 0.5745301383843724]
第6幅图像的平移向量:
[0.144403698794371;
 -2.686413562533621;
 -0.08279238304814077]
第7幅图像的旋转向量:
[23.37912628758978;
 -71.28708027930361;
 401.7783087659996]
第7幅图像的旋转矩阵:
[0.950756682549477, -0.3056521783663705, -0.05136610212392408;
 0.3046663933949521, 0.9520979509442887, -0.02622747687825021;
 0.05692204602107398, 0.009286423831555549, 0.9983354361181394]
第7幅图像的平移向量:
[0.4433620069430767;
 -2.778035766165631;
 0.1565310822654871]
第8幅图像的旋转向量:
[84.53413910746443;
 -88.75268154189268;
 326.4489757550855]
第8幅图像的旋转矩阵:
[-0.882333219506006, -0.1387045774185431, 0.4497211691251699;
 -0.1080922696912742, -0.870309912144045, -0.4804963247068739;
 0.4580438308602738, -0.4725692510383723, 0.7529104541603049]
第8幅图像的平移向量:
[0.3026042878663719;
 -2.832559861959414;
 0.5197600078874884]
第9幅图像的旋转向量:
[-66.87955552666558;
 -81.79728232518671;
 287.3798612501427]
第9幅图像的旋转矩阵:
[-0.06408698919457989, 0.997286705569611, 0.03622270986668297;
 -0.8668814706204128, -0.03765202403427882, -0.4970903750638435;
 -0.4943777641752957, -0.06325782149453277, 0.8669423708118097]
第9幅图像的平移向量:
[1.918018245182696;
 2.198445482038513;
 0.6398190872020209]
第10幅图像的旋转向量:
[51.38889872566385;
 -112.4792732922813;
 348.8614284720838]
第10幅图像的旋转矩阵:
[0.8410751829508221, 0.5075468667660225, 0.1870527055678015;
 -0.521221221444936, 0.852916565973049, 0.0293559159998552;
 -0.1446408481020841, -0.1221863720908967, 0.9819111546039054]
第10幅图像的平移向量:
[0.2388869800501047;
 2.534868757127185;
 0.05816455567725017]
第11幅图像的旋转向量:
[55.25157597573984;
 -103.974863603741;
 332.3331998859927]
第11幅图像的旋转矩阵:
[0.7603104175748064, -0.6302201082550355, -0.1573235013538499;
 0.6075084686586226, 0.7756458925501082, -0.1711926104661106;
 0.2299163531271294, 0.0345841657577196, 0.9725957053388442]
第11幅图像的平移向量:
[-0.02801590475009446;
 -3.011578659457537;
 0.5796308944847007]
第12幅图像的旋转向量:
[37.20265745451167;
 -92.46700742075161;
 299.3885458741333]
第12幅图像的旋转矩阵:
[0.1968247409885918, -0.9604756585987335, -0.1968413843024444;
 0.9041946443200382, 0.2554459280495449, -0.3423148010616344;
 0.3790673640894628, -0.1106069034112951, 0.9187350251296783]
第12幅图像的平移向量:
[-0.4442257873668548;
 -2.891665626351126;
 -0.7306268697464358]
第13幅图像的旋转向量:
[49.15686896201693;
 -109.7597615043953;
 322.2472823512488]
第13幅图像的旋转矩阵:
[-0.02527960043733595, 0.888126856668879, 0.4589026348422781;
 -0.9835935284565535, 0.05992383782219021, -0.170155530145356;
 -0.1786189031992861, -0.4556751256368033, 0.8720409779911538]
第13幅图像的平移向量:
[0.2685697410235677;
 2.70549028727733;
 0.2575020268614151]

下面在附上一份来自于其他博客的源码：

/*************************************************************************************
*
*   Description：相机标定，张氏标定法  单目标定，一次只能标定一个相机
                 OPENCV3.0 单目摄像头标定(使用官方自带的标定图片)
                 https://blog.csdn.net/zc850463390zc/article/details/48946855
*   Author     ：JNU
*   Data       ：2018.7.22
*
************************************************************************************/
#include <opencv2/opencv.hpp>
#include <highgui.hpp>
#include "cv.h"
#include <cv.hpp>
#include <iostream>

using namespace std;
using namespace cv;

//程序运行之前需要更改的参数

//使用官方标定图片集？
//#define   SAMPLE  
#define  MY_DATA

#ifdef SAMPLE

/* 官方数据集  */
const int imageWidth = 640;                                //摄像头的分辨率
const int imageHeight = 480;
const int boardWidth = 9;                                //横向的角点数目
const int boardHeight = 6;                                //纵向的角点数据
const int boardCorner = boardWidth * boardHeight;        //总的角点数据
const int frameNumber = 13;                                //相机标定时需要采用的图像帧数
const int squareSize = 20;                                //标定板黑白格子的大小 单位mm
const Size boardSize = Size(boardWidth, boardHeight);
const char imageFilePathFormat[] = "sample/right%02d.jpg";          //用于标定的图片路径，格式化字符串sample/left%02d.bmp表明图片路径为 sample/left01.bmp - sample/leftxx.bmp

#elif defined  MY_DATA
//自己的数据
const int imageWidth = 1600;                                //摄像头的分辨率
const int imageHeight = 1200;
const int boardWidth = 9;                                    //横向的角点数目
const int boardHeight = 6;                                    //纵向的角点数据
const int boardCorner = boardWidth * boardHeight;           //总的角点数据
const int frameNumber = 10;                                         //相机标定时需要采用的图像帧数
const int squareSize = 30;                                   //标定板黑白格子的大小 单位mm
const Size boardSize = Size(boardWidth, boardHeight);
Size imageSize = Size(imageWidth, imageHeight);
const char imageFilePathFormat[] = "image/right/%d.bmp";

#endif // SAMPLE



Mat intrinsic;                                            //相机内参数
Mat distortion_coeff;                                    //相机畸变参数
vector<Mat> rvecs;                                        //旋转向量
vector<Mat> tvecs;                                        //平移向量
vector<vector<Point2f>> corners;                        //各个图像找到的角点的集合 和objRealPoint 一一对应
vector<vector<Point3f>> objRealPoint;                    //各副图像的角点的实际物理坐标集合


vector<Point2f> corner;                                    //某一副图像找到的角点

Mat rgbImage, grayImage;

/*计算标定板上模块的实际物理坐标*/
void calRealPoint(vector<vector<Point3f>>& obj, int boardwidth, int boardheight, int imgNumber, int squaresize)
{
    //    Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));
    vector<Point3f> imgpoint;
    for (int rowIndex = 0; rowIndex < boardheight; rowIndex++)
    {
        for (int colIndex = 0; colIndex < boardwidth; colIndex++)
        {
            //    imgpoint.at<Vec3f>(rowIndex, colIndex) = Vec3f(rowIndex * squaresize, colIndex*squaresize, 0);
            imgpoint.push_back(Point3f(rowIndex * squaresize, colIndex * squaresize, 0));
        }
    }
    for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++)
    {
        obj.push_back(imgpoint);
    }
}

/*设置相机的初始参数 也可以不估计*/
void guessCameraParam(void)
{
    /*分配内存*/
    intrinsic.create(3, 3, CV_64FC1);
    distortion_coeff.create(5, 1, CV_64FC1);

    /*
    fx 0 cx
    0 fy cy
    0 0  1
    */
    intrinsic.at<double>(0, 0) = 256.8093262;   //fx        
    intrinsic.at<double>(0, 2) = 160.2826538;   //cx
    intrinsic.at<double>(1, 1) = 254.7511139;   //fy
    intrinsic.at<double>(1, 2) = 127.6264572;   //cy

    intrinsic.at<double>(0, 1) = 0;
    intrinsic.at<double>(1, 0) = 0;
    intrinsic.at<double>(2, 0) = 0;
    intrinsic.at<double>(2, 1) = 0;
    intrinsic.at<double>(2, 2) = 1;

    /*
    k1 k2 p1 p2 p3
    */
    distortion_coeff.at<double>(0, 0) = -0.193740;  //k1
    distortion_coeff.at<double>(1, 0) = -0.378588;  //k2
    distortion_coeff.at<double>(2, 0) = 0.028980;   //p1
    distortion_coeff.at<double>(3, 0) = 0.008136;   //p2
    distortion_coeff.at<double>(4, 0) = 0;          //p3
}

void outputCameraParam(void)
{
    /*保存数据*/
    //cvSave("cameraMatrix.xml", &intrinsic);
    //cvSave("cameraDistoration.xml", &distortion_coeff);
    //cvSave("rotatoVector.xml", &rvecs);
    //cvSave("translationVector.xml", &tvecs);

    /*保存数据*/
    /*输出数据*/
    FileStorage fs("intrinsics.yml", FileStorage::WRITE);
    if (fs.isOpened())
    {
        fs << "intrinsic" << intrinsic << "distortion_coeff" << distortion_coeff ;
        fs.release();
    }
    else
    {
        cout << "Error: can not save the intrinsics!!!!!" << endl;
    }

    fs.open("extrinsics.yml", FileStorage::WRITE);
    if (fs.isOpened())
    {
        fs << "rvecs" << rvecs << "tvecs" << tvecs;
        fs.release();
    }
    else
    {    
        cout << "Error: can not save the extrinsics parameters\n";
    }

    /*输出数据*/
    cout << "fx :" << intrinsic.at<double>(0, 0) << endl << "fy :" << intrinsic.at<double>(1, 1) << endl;
    cout << "cx :" << intrinsic.at<double>(0, 2) << endl << "cy :" << intrinsic.at<double>(1, 2) << endl;

    cout << "k1 :" << distortion_coeff.at<double>(0, 0) << endl;
    cout << "k2 :" << distortion_coeff.at<double>(1, 0) << endl;
    cout << "p1 :" << distortion_coeff.at<double>(2, 0) << endl;
    cout << "p2 :" << distortion_coeff.at<double>(3, 0) << endl;
    cout << "p3 :" << distortion_coeff.at<double>(4, 0) << endl;
}


void main(char *args)
{
    Mat img;
    int goodFrameCount = 0;
    namedWindow("chessboard");
    cout << "按Q退出 ..." << endl;
    while (goodFrameCount < frameNumber)
    {
        char filename[100];
        //sprintf_s(filename, "image/right/%d.bmp", goodFrameCount + 1);
        sprintf_s(filename, imageFilePathFormat, goodFrameCount + 1);
        //    cout << filename << endl;
        rgbImage = imread(filename, CV_LOAD_IMAGE_COLOR);
        cvtColor(rgbImage, grayImage, CV_BGR2GRAY);
        imshow("Camera", grayImage);

        bool isFind = findChessboardCorners(rgbImage, boardSize, corner, 0);
        if (isFind == true)    //所有角点都被找到 说明这幅图像是可行的
        {
            /*
            Size(5,5) 搜索窗口的一半大小
            Size(-1,-1) 死区的一半尺寸
            TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1)迭代终止条件
            */
            cornerSubPix(grayImage, corner, Size(5, 5), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1));
            drawChessboardCorners(rgbImage, boardSize, corner, isFind);
            imshow("chessboard", rgbImage);
            corners.push_back(corner);
            //string filename = "res\\image\\calibration";
            //filename += goodFrameCount + ".jpg";
            //cvSaveImage(filename.c_str(), &IplImage(rgbImage));        //把合格的图片保存起来
            goodFrameCount++;
            cout << "The image is good" << endl;
        }
        else
        {
            cout << goodFrameCount+1 <<" The image is bad please try again" << endl;
        }
        //    cout << "Press any key to continue..." << endl;
        //    waitKey(0);

        if (waitKey(10) == 'q')
        {
            break;
        }
        //    imshow("chessboard", rgbImage);
    }

    /*
    图像采集完毕 接下来开始摄像头的校正
    calibrateCamera()
    输入参数 objectPoints  角点的实际物理坐标
    imagePoints   角点的图像坐标
    imageSize       图像的大小
    输出参数
    cameraMatrix   相机的内参矩阵
    distCoeffs       相机的畸变参数
    rvecs           旋转矢量(外参数)
    tvecs           平移矢量(外参数）
    */

    /*设置实际初始参数 根据calibrateCamera来 如果flag = 0 也可以不进行设置*/
    guessCameraParam();
    cout << "guess successful" << endl;
    /*计算实际的校正点的三维坐标*/
    calRealPoint(objRealPoint, boardWidth, boardHeight, frameNumber, squareSize);
    cout << "cal real successful" << endl;
    /*标定摄像头*/
    calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distortion_coeff, rvecs, tvecs, 0);
    cout << "calibration successful" << endl;
    /*保存并输出参数*/
    outputCameraParam();
    cout << "out successful" << endl;

    /*显示畸变校正效果*/
    Mat cImage;
    undistort(rgbImage, cImage, intrinsic, distortion_coeff);
    imshow("Corret Image", cImage);
    cout << "Correct Image" << endl;
    cout << "Wait for Key" << endl;
    waitKey(0);
    system("pause");
}

View Code