三维重建学习(4)：张正友相机标定程序实现（OpenCV）

前言

在前面的博客中（ 三维重建学习(3)：张正友相机标定推导），推到了张正友相机标定的数学原理，并给出了标定流程。OpenCV中已经封装好了一系列函数，我们使用这些函数可以更快捷地实现张正友相机标定。

程序流程

1. 准备好一系列用来相机标定的图片；
2. 对每张图片提取角点信息；
3. 由于角点信息不够精确，进一步提取亚像素角点信息；
4. 在图片中画出提取出的角点；
5. 相机标定；
6. 对标定结果评价，计算误差；
7. 使用标定结果对原图片进行矫正；

上面这个流程大概看一遍有了个大概的认识就足够了。整个程序中都不涉及太深的数学，因为那些比较“恶心”的数学部分OpenCV都已经实现好了，我们直接调用就可以了。
关于其中的内外参矩阵等的参数不做赘述，前面的博客已经介绍过了： 三维重建学习(3)：张正友相机标定推导。

流程说明

1. 准备图片

标定图片需要使用标定板在不同位置、不同角度、不同姿态下拍摄。在前面的博客中进行推导时，我们分析得知至少要有3张图片，才能有唯一解。通常以10~20张为佳。
通常都会购买专门的标定板，如果精度要求不高，也可以自己打印。
下面的图片摘自：http://blog.csdn.net/dcrmg/article/details/52939318

网上也有别人已经拍好的照片，下面是下载链接：https://pan.baidu.com/s/1mhG3mHU
程序中就直接使用这些现成的图片来测试了，省事。

2. 提取角点信息

OpenCV中自带了提取棋盘格中内角点的函数：findChessboardCorners()。
下面是函数原型：

CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners,
                                         int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE );

函数说明：
这个函数的功能是确定输入图像中是否有棋盘格图案，并检测棋盘格的内角点。如果所有的内角点都找到了，那么函数返回一个非0值；如果没有找到所有的内角点，就会返回0。
参数说明：

• image：输入的棋盘格图像，必须是8位的灰度或彩色图像。
• patternSize：每一幅棋盘格图片中，每行和每列角点的个数；如果用前面给出的那副图片，每行每列对应的角点数就是4和6。另外为了便于辨别方向，每行每列对应的角点数不能相同。
• corners：输出的角点坐标。通常用cv::Point2f 向量来保存，vector points。
• flags：默认为0，也可为其他参数。决定了内角点的不同查找方式。（默认写0即可，下面是官方文档中的给出的可选取值）
  
  • CALIB_CB_ADAPTIVE_THRESH 使用自适应阈值（通过平均图像亮度计算得到）将图像转换为黑白图，而不是一个固定的阈值。
  • CALIB_CB_NORMALIZE_IMAGE 在使用固定阈值或者自适应阈值进行二值化之前，先使用equalizeHist()来均衡化图像亮度。
  • CALIB_CB_FILTER_QUADS 使用其他的准则（如轮廓面积，周长，方形形状）来去除在轮廓检测阶段检测到的错误方块。

3. 提取亚像素角点信息

有两个函数可以实现提取亚像素角点信息：cornerSubPix、find4QuadCornerSubpix。在提取棋盘格角点时两者的效果差不多，随便使用哪一个都行。

void cornerSubPix(InputArray image, InputOutputArray corners, Size winSize, Size zeroZone, TermCriteria criteria)

参数说明：

• image：输入的图像；
• corners：初始的角点坐标，同时也会作为亚像素角点坐标的输出；通常用cv::Point2f/Point2d 向量来保存，vector points。
• winSize：大小为搜索窗口的一半。
• zeroZone：死区的一般尺寸，死区为不对搜索区的中央位置做求和运算的区域。
• criteria：迭代的终止条件。

CV_EXPORTS bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size )

参数说明：

• img：输入图像，最好是8位灰度图像，检测效率更高；
• corners：初始的角点坐标，同时也会作为亚像素角点坐标的输出；通常用cv::Point2f/Point2d 向量来保存，vector points。
• region_size：角点搜索窗口的大小。

采用这两个函数都可实现亚像素角点检测，精度差不多，后面程序中采用find4QuadCornerSubpix函数。

4. 画出角点

使用drawChessboardCorners函数。函数功能很简单，就是在图片中画出检测到的角点。

CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize, InputArray corners, bool patternWasFound )

参数说明：

• image：图像，8位灰度或彩色图像。
• patternSize：每一幅棋盘格图片中，每行和每列角点的个数。
• corners：初始的角点坐标，同时也会作为亚像素角点坐标的输出；通常用cv::Point2f/Point2d 向量来保存，vector points。
• patternWasFound：标志位，用来只是是否检测倒所有的棋盘内角点。true表示完整地检测到了所有内角点，函数会用直线将角点依次连接起来；false表示没有完整检测到所有内角点，函数会用红色圆圈标出检测到的内角点。

5. 相机标定

这里的标定函数是calibrateCamera，也是相机标定的核心了。

CV_EXPORTS_W double 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) );

参数说明：

• objectPoints：一系列点的三维坐标，即若干张图片中对应的若干个点的三维坐标。在使用时应该建立一个二维的vector，即vector> objectPoints。我们需要根据棋盘格每个黑白格子的长宽，计算出各个内角点的三维坐标。通常我们会取z=0，而只计算x和y 坐标。
• imagePoints：若干张图片对应的若干的内角点的坐标，通常采用vector> image_points表示。
• imageSize：图像的像素尺寸大小。
• cameraMatrix：相机的内参矩阵，对应推导时的内参矩阵 A ，大小为 3×3 。
• distCoeffs：相机的畸变参数矩阵，有5个畸变参数： k1,k2,p1,p2,k3 ，，矩阵大小为 1×5 。
• rvecs：旋转向量，罗德里格旋转向量，是相机外参；因为有若干张图片，所以通常使用Mat类型的vector 表示，vector rvecs。
• tvecs：位移向量，与旋转向量一样，也是相机外参，通常使用Mat类型的vector 表示，vector tvecs。
• flags：表示标定时采用的算法。默认为0，其他有：
  
  • 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：迭代的终止条件。

这个函数解决的就是我们以前推导的极大似然优化问题：

∑i=1n∑j=1m∥mij−m^(A,k1,k2,Ri,ti,Mj)∥

其中： m^(A,ki,kj,Ri,ti,Mj) 表示的是点 Mj 在第 i 幅图像上的投影。
A 是内参矩阵，对应函数中的参数 cameraMatrix； m 是角点坐标，对应参数中的 imagePoints； M 是角点对应的三维点坐标（在世界坐标系中）； R 是旋转矩阵，当然旋转矩阵可以使用Rodrigue公式转换成等价的旋转向量，即参数中的 rvecs； t 是平移矩阵，对应参数中的 tvecs；其他还有一些畸变系数，根据情况可能考虑进去也可能忽略不计。
通过类比，不难理解这些参数的意义。

6. 评价标定结果

我们在进行相机标定时，本身要解决的是一个优化问题，而优化的对象就是角点与三维点投影到图像点坐标之间的差值，通过不断迭代，尽可能地最小化这个差值。我们对标定结果评价时，就是计算投影点与检测到的亚像素角点坐标的差值。由于是二维的，所以分别对 x 和 y 坐标求差值，再求平方根，即求L2范数。

先考虑如何对空间中的三维坐标点进行反向投影，使用函数projectPoints 实现：

CV_EXPORTS_W void projectPoints( InputArray objectPoints,
                                 InputArray rvec, InputArray tvec,
                                 InputArray cameraMatrix, InputArray distCoeffs,
                                 OutputArray imagePoints,
                                 OutputArray jacobian = noArray(),
                                 double aspectRatio = 0 );

参数说明：

• objectPoints：一系列点的三维坐标，即若干张图片中对应的若干个点的三维坐标。在使用时应该建立一个二维的vector，即vector> objectPoints。我们需要根据棋盘格每个黑白格子的长宽，计算出各个内角点的三维坐标。通常我们会取z=0，而只计算x和y 坐标。
• rvecs：旋转向量，罗德里格旋转向量，是相机外参；因为有若干张图片，所以通常使用Mat类型的vector 表示，vector rvecs。
• tvecs：位移向量，与旋转向量一样，也是相机外参，通常使用Mat类型的vector 表示，vector tvecs。
• cameraMatrix：相机的内参矩阵，对应推导时的内参矩阵 A ，大小为 3×3 。
• distCoeffs：相机的畸变参数矩阵，有5个畸变参数： k1,k2,p1,p2,k3 ，，矩阵大小为 1×5 。
• imagePoints：若干张图片对应的若干的内角点的坐标，通常采用vector> image_points表示。
• 另外两个参数这里不会用到，跳过。

7. 矫正图像

使用前面求得的内参和外参以及畸变参数数据，可以对图像进行畸变矫正。
使用initUndistortRectifyMap和remap两个函数来实现。
initUndistortRectifyMap用来计算畸变映射，remap把求得的映射应用到图像上。

CV_EXPORTS_W void initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,
                           InputArray R, InputArray newCameraMatrix,
                           Size size, int m1type, OutputArray map1, OutputArray map2 );

参数说明：

• cameraMatrix：相机的内参矩阵；
• distCoeffs：相机的畸变参数构成的矩阵；
• R：可选的输入，是第一和第二相机坐标之间的旋转矩阵；
• newCameraMatrix：校正后的内参矩阵；
• size：摄像机采集的无失真的图像尺寸；
• m1type：定义map1的数据类型，可以是CV_32FC1或者CV_16SC2；
• map1和map2：分别对应 X 和 Y 坐标的重映射参数。

CV_EXPORTS_W void remap( InputArray src, OutputArray dst,
                         InputArray map1, InputArray map2,
                         int interpolation, int borderMode = BORDER_CONSTANT,
                         const Scalar& borderValue = Scalar());

参数说明：

• src：输入图像，原始有畸变的图像；
• dst：输出图像，校正后的图像；
• map1： X 坐标的映射；
• map2： Y 坐标的映射；
• interpolation：图像的插值方式；
• borderMode：边界填充方式；

完整程序实现

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

using namespace std;
using namespace cv;

int main()
{
    ifstream inImgPath("calibdata.txt");    //标定所用图像文件的路径
    vector<string> imgList;
    vector<string>::iterator p;
    string temp;
    if (!inImgPath.is_open())
    {
        cout << "没有找到文件" << endl;
    }
    //读取文件中保存的图片文件路径，并存放在数组中
    while (getline(inImgPath, temp))
    {
        imgList.push_back(temp);
    }

    ofstream fout("caliberation_result.txt");   //保存标定结果的文件

    cout << "开始提取角点......" << endl;
    cv::Size image_size;//保存图片大小
    cv::Size pattern_size = cv::Size(4, 6);//标定板上每行、每列的角点数；测试图片中的标定板上内角点数为4*6
    vector corner_points_buf;//建一个数组缓存检测到的角点，通常采用Point2f形式
    vector::iterator corner_points_buf_ptr;
    vector<vector> corner_points_of_all_imgs;
    int image_num = 0;
    string filename;
    while(image_num < imgList.size())
    {
        filename = imgList[image_num++];
        cout << "image_num = " << image_num << endl;
        cout << filename.c_str() << endl;
        cv::Mat imageInput = cv::imread(filename.c_str());
        if (image_num == 1)
        {
            image_size.width = imageInput.cols;
            image_size.height = imageInput.rows;
            cout << "image_size.width = " << image_size.width << endl;
            cout << "image_size.height = " << image_size.height << endl;
        }

        if (findChessboardCorners(imageInput, pattern_size, corner_points_buf) == 0)
        {
            cout << "can not find chessboard corners!\n";   //找不到角点
            exit(1);
        }
        else
        {
            cv::Mat gray;
            cv::cvtColor(imageInput, gray, CV_RGB2GRAY);
            cv::find4QuadCornerSubpix(gray, corner_points_buf, cv::Size(5, 5));
            corner_points_of_all_imgs.push_back(corner_points_buf);
            cv::drawChessboardCorners(gray, pattern_size, corner_points_buf, true);
            cv::imshow("camera calibration", gray);
            cv::waitKey(100);
        }
    }

    int total = corner_points_of_all_imgs.size();
    cout << "total=" << total << endl;
    int cornerNum = pattern_size.width * pattern_size.height;//每张图片上的总的角点数
    for (int i = 0; i < total;i++)
    {
        cout << "--> 第" << i + 1 << "幅图片的数据 -->:" << endl;
        for (int j = 0;j < cornerNum;j++)
        {
            cout << "-->" << corner_points_of_all_imgs[i][j].x;
            cout << "-->" << corner_points_of_all_imgs[i][j].y;
            if ((j + 1) % 3 == 0)
            {
                cout << endl;
            }
            else
            {
                cout.width(10);
            }
        }
        cout << endl;
    }

    cout << endl << "角点提取完成" << endl;

    //摄像机标定
    cout << "开始标定………………" << endl;
    cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));//内外参矩阵，H——单应性矩阵
    cv::Mat distCoefficients = cv::Mat(1, 5, CV_32FC1, cv::Scalar::all(0));//摄像机的5个畸变系数：k1,k2,p1,p2,k3
    vector tvecsMat;//每幅图像的平移向量，t
    vector rvecsMat;//每幅图像的旋转向量（罗德里格旋转向量）
    vector<vector> objectPoints;//保存所有图片的角点的三维坐标
                                             //初始化每一张图片中标定板上角点的三维坐标
    int i, j, k;
    for (k = 0;k < image_num;k++)//遍历每一张图片
    {
        vector tempCornerPoints;//每一幅图片对应的角点数组
        //遍历所有的角点
        for (i = 0;i < pattern_size.height;i++)
        {
            for (j = 0;j < pattern_size.width;j++)
            {
                cv::Point3f singleRealPoint;//一个角点的坐标
                singleRealPoint.x = i * 10;
                singleRealPoint.y = j * 10;
                singleRealPoint.z = 0;//假设z=0
                tempCornerPoints.push_back(singleRealPoint);
            }
        }
        objectPoints.push_back(tempCornerPoints);
    }

    cv::calibrateCamera(objectPoints, corner_points_of_all_imgs, image_size, cameraMatrix, distCoefficients, rvecsMat, tvecsMat, 0);
    cout << "标定完成" << endl;

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

    cout << endl << "相机相关参数：" << endl;
    fout << "相机相关参数：" << endl;
    cout << "1.内外参数矩阵:" << endl;
    fout << "1.内外参数矩阵:" << endl;
    cout << "大小：" << cameraMatrix.size() << endl;
    fout << "大小：" << cameraMatrix.size() << endl;
    cout << cameraMatrix << endl;
    fout << cameraMatrix << endl;

    cout << "2.畸变系数：" << endl;
    fout << "2.畸变系数：" << endl;
    cout << "大小：" << distCoefficients.size() << endl;
    fout << "大小：" << distCoefficients.size() << endl;
    cout << distCoefficients << endl;
    fout << distCoefficients << endl;

    cout << endl << "图像相关参数：" << endl;
    fout << endl << "图像相关参数：" << endl;
    cv::Mat rotation_Matrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));//旋转矩阵
    for (i = 0;i < image_num;i++)
    {
        cout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
        fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
        cout << rvecsMat[i] << endl;
        fout << rvecsMat[i] << endl;
        cout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
        fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
        cv::Rodrigues(rvecsMat[i], rotation_Matrix);//将旋转向量转换为相对应的旋转矩阵
        cout << rotation_Matrix << endl;
        fout << rotation_Matrix << endl;
        cout << "第" << i + 1 << "幅图像的平移向量：" << endl;
        fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
        cout << tvecsMat[i] << endl;
        fout << tvecsMat[i] << endl;
    }

    cout << "结果保存完毕" << endl;

    //对标定结果进行评价
    cout << "开始评价标定结果......" << endl;

    //计算每幅图像中的角点数量，假设全部角点都检测到了
    int corner_points_counts;
    corner_points_counts = pattern_size.width * pattern_size.height;

    cout << "每幅图像的标定误差：" << endl;
    fout << "每幅图像的标定误差：" << endl;
    double err = 0;//单张图像的误差
    double total_err = 0;//所有图像的平均误差
    for (i = 0;i < image_num;i++)
    {
        vector image_points_calculated;//存放新计算出的投影点的坐标
        vector tempPointSet = objectPoints[i];
        cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoefficients, image_points_calculated);

        //计算新的投影点与旧的投影点之间的误差
        vector image_points_old = corner_points_of_all_imgs[i];
        //将两组数据换成Mat格式
        cv::Mat image_points_calculated_mat = cv::Mat(1, image_points_calculated.size(), CV_32FC2);
        cv::Mat image_points_old_mat = cv::Mat(1, image_points_old.size(), CV_32FC2);
        for (j = 0;j < tempPointSet.size();j++)
        {
            image_points_calculated_mat.at(0, j) = cv::Vec2f(image_points_calculated[j].x, image_points_calculated[j].y);
            image_points_old_mat.at(0, j) = cv::Vec2f(image_points_old[j].x, image_points_old[j].y);
        }
        err = cv::norm(image_points_calculated_mat, image_points_old_mat, cv::NORM_L2);
        err /= corner_points_counts;
        total_err += err;
        cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
        fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
    }
    cout << "总体平均误差：" << total_err / image_num << "像素" << endl;
    fout << "总体平均误差：" << total_err / image_num << "像素" << endl;
    cout << "评价完成" << endl;

    fout.close();

    cv::Mat mapx = cv::Mat(image_size, CV_32FC1);
    cv::Mat mapy = cv::Mat(image_size, CV_32FC1);
    cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
    cout << "保存矫正图像" << endl;
    string imageFileName;
    std::stringstream StrStm;
    for (int i = 0;i < image_num;i++)
    {
        cout << "Frame #" << i + 1 << endl;
        cv::initUndistortRectifyMap(cameraMatrix, distCoefficients, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
        cv::Mat src_image = cv::imread(imgList[i].c_str(), 1);
        cv::Mat new_image = src_image.clone();
        cv::remap(src_image, new_image, mapx, mapy, cv::INTER_LINEAR);
        imshow("原始图像", src_image);
        imshow("矫正后图像", new_image);

        StrStm.clear();
        imageFileName.clear();
        StrStm << i + 1;
        StrStm >> imageFileName;
        imageFileName += "_d.jpg";
        cv::imwrite(imageFileName, new_image);

        cv::waitKey(200);
    }
    cout << "保存结束" << endl;

    cv::waitKey(0);

    return 0;
}

运行结果

完整工程：http://download.csdn.net/download/hongbin_xu/10191899

参考链接：
http://blog.csdn.net/dcrmg/article/details/52939318