python张正友标定 张正友标定法原理

张正友标定法

相机标定的目的

当我们拿到一张图片，进行识别之后，得到的两部分之间的距离为多少像素，但是这多少像素究竟对应实际世界中的多少米呢？这就需要利用相机标定的结果来将像素坐标转换到物理坐标来计算距离（仅仅利用单目相机标定的结果，是无法直接从像素坐标转化到物理坐标的，因为透视投影丢失了一个维度的坐标，所以测距其实需要双目相机）。

相机标定的第一个目的就是获得相机的内参矩阵和外参矩阵。

相机标定的第二个目的就是获得相机的畸变参数进而对拍摄的图片进行去畸变处理。

简介

张正友标定法利用如下图所示的棋盘格标定板，在得到一张标定板的图像之后，可以利用相应的图像检测算法得到每一个角点的像素坐标\((u,v)\)

张正友标定法将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标\(W = 0\),由于标定板的世界坐标系是人为事先定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到每一个角点在世界坐标系下的物理坐标\((U,V,W = 0)\)

我们将利用这些信息：每一个角点的像素坐标\((u,v)\)、每一个角点在世界坐标系下的物理坐标\((U,V,W = 0)\)来进行相机的标定，获得相机的内外参矩阵、畸变参数。

求解内外参数矩阵

将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标\(W = 0\)，因此，原单点无畸变的成像模型可以化为下式。

\[Z \left( \begin{matrix} u\\v\\1 \end{matrix} \right) =\left( \begin{matrix} f_u & -f_ucot\theta & u_0\\ 0 & \frac{f_v}{sin\theta} & v_0\\ 0 & 0 & 1 \end{matrix} \right) \left( \begin{matrix} R_1 & R_2 & T \end{matrix} \right)\left( \begin{matrix} U\\V\\1 \end{matrix} \right)=A \left( \begin{matrix} R_1 & R_2 & T \end{matrix} \right)\left( \begin{matrix} U\\V\\1 \end{matrix} \right) \]

其中，\(R_1,R_2\)为旋转矩阵\(R\)的前两列，为了方便把内参矩阵记为\(A\)。

• 对于不同的图片，内参矩阵\(A\)为定值；
• 对于同一张图片，内参矩阵\(A\)，外参矩阵\((R_1\quad R_2\quad T)\)为定值；
• 对于同一组图片上的单点，内参矩阵\(A\)，外参矩阵\((R_1\quad R_2\quad T)\)，尺度因子\(Z\)为定值

将\(A(R_1\quad R_2\quad T)\)记为矩阵\(H\)，\(H\)即为内参矩阵和外参矩阵的积，记矩阵H的三列为\((H_1,H_2,H_3)\)，则有

\[\left( \begin{matrix} u\\v\\1 \end{matrix} \right) = \frac{1}{Z}H \left( \begin{matrix} U\\V\\1 \end{matrix} \right) =\frac{1}{Z}\left[ \begin{matrix} H_{11} & H_{12} & H_{13}\\ H_{12} & H_{22} & H_{23}\\ H_{13} & H_{32} & H_{33}\\ \end{matrix} \right] \left( \begin{matrix} U\\V\\1 \end{matrix} \right) \]

利用上式，消去尺度因子\(Z\)，可得

\[u = \frac{H_{11}U+H_{12}V+H_{13}}{H_{31}U+H_{32}V+H_{33}}\\ v = \frac{H_{21}U+H_{22}V+H_{23}}{H_{31}U+H_{32}V+H_{33}} \]

此时，尺度因子\(Z\)已经被消去，因此上式对于同一张图片上所有的角点均成立。

\((u,v)\)是像素坐标系下的标定板角点的坐标，\((U,V)\)是世界坐标系下标定板角点的坐标。

通过图像识别算法，可以得到标定板角点的像素坐标\((u,v)\)，又由于标定板的世界坐标系是人为定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到世界坐标系下的\((U,V)\)

这里的\(H\)是齐次矩阵，有8个独立未知元素。每一个标定板角点可以提供两个约束方程（\(u,U,V\)的对应关系提供了两个约束方程），因此当一张图片上的标定板角点数量等于4时，即可求得该图片对应的矩阵\(H\)，当一张图片上的标定板角点数量大于4时，利用最小二乘法回归最佳的矩阵\(H\)

求解内参矩阵

已知矩阵\(H = A(R1 \quad R2 \quad T)\)，接下来需要求解相机的内参矩阵\(A\)。

利用\(R1,R2\)作为旋转矩阵\(R\)的两列，存在单位正交的关系，即：

\[R_1^TR_2 = 0\\ R_1^TR_1 = R_2^TR_2 = 1 \]

由\(H\)和\(R1,R2\)的关系可知：

\[R_1 = A^{-1}H_1\\ R_2 = A^{-1}H_2 \]

代入可得：

\[H_1^TA^{-T}A^{-1}H_2 = 0\\ H_1^{T}A^{-T}A^{-1}H_1 = H_2^TA^{-T}A^{-1}H_2 = 1 \]

述两个约束方程中均存在矩阵\(A^{-T}A^{-1}\)，因此，记\(A^{-T}A^{-1} = B\) ，则\(B\)为对称阵。

先求解矩阵\(B\)，通过矩阵B再求解相机的内参矩阵\(A\)。

为了简便，我们记相机内参矩阵\(A\)为：

\[A = \left( \begin{matrix} f_u & -f_ucot\theta & u_0\\ 0 & \frac{f_v}{sin\theta} & v_0\\ 0 & 0 & 1 \end{matrix} \right) = \left[ \begin{matrix} \alpha & \gamma & u_0\\ 0 & \beta & v_0\\ 0 & 0 & 1 \end{matrix} \right] \]

则：

\[A^{-1} = \left[ \begin{matrix} \frac{1}{\alpha} & -\frac{\gamma}{\alpha\beta} & \frac{\gamma v_0-\beta u_0}{\alpha \beta}\\ 0 & \frac{1}{\beta} & -\frac{v_0}{\beta}\\ 0 & 0 & 1 \end{matrix} \right] \]

则用矩阵\(A\)表示矩阵\(B\)得

注意：由于\(B\)为对称阵，上式出现了两次\(B_{12},B_{13},B_{23}\)

这里我们可以使用\(B= A^{-T}A^{-1}\)将前面通过\(R_1,R_2\)单位正交得到的约束方程化为：

\[H_1^TBH_2 = 0\\ H_1^TBH_1 = H_2^TBH_2 =1 \]

因此，为了求解矩阵\(B\)，必须计算\(H_i^{T}BH_{j}\)，则

\[\begin{eqnarray} H_i^{T}BH_{j} &=& \left[ \begin{matrix} H_{1i} & H_{2i} & H_{3i} \end{matrix} \right] \left[ \begin{matrix} B_{11} & B_{12} & B_{13}\\ B_{12} & B_{22} & B_{23}\\ B_{13} & B_{32} & B_{33}\\ \end{matrix} \right]\left[ \begin{matrix} H_{1j}\\H_{2j}\\H_{3j} \end{matrix} \right]\\ &=&\left[ \begin{matrix} H_{1i}H_{1j} & H_{1i}H_{2j} + H_{2i}H_{1j} & H_{2i}H_{2j} & H_{1i}H_{3j} + H_{3i}H_{1j} & H_{2i}H_{3j} + H_{3i}H_{2j} & H_{3i}H_{3j} \end{matrix} \right] \left[ \begin{matrix} B_{11}\\B_{12}\\B_{22}\\B_{13}\\B_{23}\\B_{33} \end{matrix} \right] \end{eqnarray} \]

记：

\[v_{ij} = \left[ \begin{matrix} H_{1i}H_{1j} & H_{1i}H_{2j} + H_{2i}H_{1j} & H_{2i}H_{2j} & H_{1i}H_{3j} + H_{3i}H_{1j} & H_{2i}H_{3j} + H_{3i}H_{2j} & H_{3i}H_{3j} \end{matrix} \right]^{T} \\ b = \left[ \begin{matrix} B_{11} & B_{12} & B_{22} & B_{13} & B_{23} & B_{33} \end{matrix} \right]^T \]

则上式化为：

\[H_i^TBH_j = v_{ij}b \]

此时，通过\(R_1\quad R_2\)单位正交得到的约束方程可化为：

\[v_{12}^Tb = 0\\ v_{11}^Tb = v_{22}^Tb = 1 \]

即：

\[\left[ \begin{matrix} v_{12}^T\\v_{11}^T-v_{22}^T \end{matrix} \right]b = vb = 0 \]

其中，矩阵\(v = \left[\begin{matrix}v_{12}^T\\v_{11}^T-v_{22}^T\end{matrix}\right]\)

由于矩阵\(H\)已知，矩阵\(v\)又全部由矩阵\(H\)的元素构成，因此矩阵\(v\)已知

此时，我们只要求解出向量\(b\)，即可得到矩阵\(B\)。每张标定板图片可以提供一个\(vb=0\)的约束关系，该约束关系含有两个约束方程。但是，向量\(b\)有6个未知元素。因此，单张图片提供的两个约束方程不足以解出向量\(b\)。因此只取3张标定板照片，得到3个\(vb=0\)的约束关系，即6个方程，即可求解向量\(b\)。

当标定板图片的个数大于3时（事实上一般需要15到20张标定板图片），可采用最小二乘拟合最佳的向量\(b\)，并得到矩阵\(B\)。

根据矩阵\(B\)的元素和相机内参\(\alpha,\beta,\gamma,u_0,v_0\)的对应关系（上式），可得到：

\[\begin{align} & v_0 = \frac{B_{12}B_{13}-B_{11}B_{23}}{B_{11}B_{22}-B_{12}^2}\\ & \alpha = \sqrt{\frac{1}{B_{11}}}\\ & \beta = \sqrt{\frac{B_{11}}{B_{11}B_{22}-B_{12}^2}}\\ & \gamma = -B_{12}\alpha^2\beta\\ & u_0 = \frac{\gamma v_0}{\beta} - B_{13}\alpha^2 \end{align} \]

即可求得相机的内参矩阵 \(A = \left(\begin{matrix}f_u & -f_ucot\theta & u_0\\0 & \frac{f_v}{sin\theta} & v_0\\0 & 0 & 1\end{matrix}\right) =\left[\begin{matrix}\alpha & \gamma & u_0\\0 & \beta & v_0\\0 & 0 & 1\end{matrix}\right]\)

求解外参矩阵

对于同一个相机，相机的内参矩阵取决于相机的内部参数，无论标定板和相机的位置关系是怎么样的，相机的内参矩阵不变。这也正是在第2部分“求解内参矩阵”中，我们可以利用不同的图片（标定板和相机位置关系不同）获取的矩阵\(H\)，共同求解相机内参矩阵\(A\)的原因

但是，外参矩阵反映的是标定板和相机的位置关系。对于不同的图片，标定板和相机的位置关系已经改变，此时每一张图片对应的外参矩阵都是不同的。

在关系：\(A(R_1\quad R_2\quad T) = H\)中，已经求解得到了矩阵\(H\)，（对于同一张图片相同，对于不同的图片不同），矩阵\(A\)（对于不同的图片都相同）

通过公式：\((R_1\quad R_2\quad T) = A^{-1}H\)即可求得每一张图片对应的外参矩阵\((R_1\quad R_2\quad T)\)

虽然完整的外参矩阵为\(\left(\begin{matrix}R & T\\0 & 1\end{matrix}\right)\)，但由于张正友标定板将世界坐标系的原点选取在棋盘格上，则棋盘格上任一点的物理坐标 \(W=0\)，将旋转矩阵的\(R\)的第三列\(R_3\)消掉，因此\(R_3\)在坐标转化中并没有作用，但是\(R_3\)要使得\(R\)满足旋转矩阵的性质，即列与列之间单位正交，因此可以通过向量\(R_1,R_2\)的叉乘，即\(R_3=R_1\times R_2\)，计算得到\(R_3\)

此时，相机的内参矩阵和外参矩阵均已得到

标定相机的畸变参数

张正友标定法仅仅考虑了畸变模型中影响较大的径向畸变。

径向畸变公式（2阶）如下：

\[\hat{x} = x(1+k_1r^2+k_2r^4)\\ \hat{y} = y(1+k_1r^2+k_2r^4)\\ \]

其中，\((x,y),(\hat{x},\hat{y})\)分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标，\(r\)为图像像素点到图像中心点的距离，即\(r^2 = x^2 + y^2\)

图像坐标和像素坐标的转化关系为：

\[\left( \begin{matrix} u\\v\\1 \end{matrix} \right) =\left( \begin{matrix} f_u & -f_ucot\theta & u_0\\ 0 & \frac{f_v}{sin\theta} & v_0\\ 0 & 0 & 1 \end{matrix} \right) \left( \begin{matrix} x\\y\\1 \end{matrix} \right) \]

其中，\((u,v)\)为理想的无畸变的像素坐标。由于\(\theta\)接近90度，则上式近似为

\[u = \frac{x}{dX} + u_0\\ v = \frac{y}{dY} + v_0 \]

同理可得畸变后的像素坐标\((\hat{u},\hat{v})\)的表达式为

\[\hat{u} = \frac{\hat{x}}{dX} + u_0\\ \hat{v} = \frac{\hat{y}}{dY} + v_0 \]

代入径向畸变公式（2阶）则有：

\[\hat{u} - u_0 = (u - u_0)(1+k_1r^2+k_2r^4)\\ \hat{v} - v_0 = (v - v_0)(1+k_1r^2+k_2r^4) \]

化简得：

\[\hat{u} = u + (u - u_0)(k_1r^2+k_2r^4)\\ \hat{v} = v + (v - v_0)(k_1r^2+k_2r^4) \]

即为：

\[\left[ \begin{matrix} (u-u_0)r^2 & (u-u_0)r^4\\ (v-v_0)r^2 & (v-v_0)r^4 \end{matrix} \right] \left[ \begin{matrix} k_1\\k_2 \end{matrix} \right] =\left[ \begin{matrix} \hat{u}-u\\ \hat{v}- v \end{matrix} \right] \]

上式中的\(\hat{u}, \hat{v}, u,v\)可以通过识别标定板的角点获得，每一个角点可以构造两个上述等式。

有m幅图像，每幅图像上有n个标定板角点，则将得到的所有等式组合起来，可以得到个mn未知数为的\(k = [k_1,k_2]^T\)的约束方程，将约束方程系数矩阵记为\(D\)，等式右端非齐次项记为\(d\)，可将其记作矩阵形式

\[Dk = d \]

则使用最小二乘法可求得：

\[k = \left[ \begin{matrix} k_1\\k_2 \end{matrix} \right] = (D^TD)^{-1}D^Td \]

此时，相机的畸变矫正参数已经标定好

步骤

1. 准备一个张正友标定法的棋盘格，棋盘格大小已知，用相机对其进行不同角度的拍摄，得到一组图像；
2. 对图像中的特征点如标定板角点进行检测，得到标定板角点的像素坐标值，根据已知的棋盘格大小和世界坐标系原点，计算得到标定板角点的物理坐标值；
3. 求解内参矩阵与外参矩阵
   根据物理坐标值和像素坐标值的关系，求出\(H\)矩阵，进而构造\(v\)矩阵，求解\(B\)矩阵，利用\(B\)矩阵求解相机内参数\(A\)，最后求解每张照片对应的相机外参矩阵\(\left(\begin{matrix}R & T\\0 & 1\end{matrix}\right)\)
4. 求解畸变参数
   利用\(\hat{u}, \hat{v}, u,v\)构造\(D\)矩阵，计算径向畸变参数
5. 利用L-M（Levenberg-Marquardt）算法对上述参数进行优化

代码

#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");        
    }
}