张氏相机标定法和畸变矫正opencv代码

原理部分可见上一篇博客，这一部分主要是关于opencv实现：
这部分代码参考网上教程张氏标定法，但我觉得部分地方可能存在问题，后续会继续看一下官方代码

完整代码和棋盘图片下载可从这里下载

基本思路为:

1. 检测代标定图像的内角点findChessboardCorners
2. 利用find4QuadCornerSubpix寻找更精细的像素级坐标
3. 根据测量的标定板的格子尺寸得到真实世界坐标系中内角点坐标
4. 利用calibrateCamera得到相机的矩阵，畸变矩阵，和每个图像的的旋转平移向量
5. 利用projectPoints重投影，从而计算误差
6. 利用undistort进行畸变矫正，畸变矫正中只需要摄像机的内参

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

using namespace std;
using namespace cv;

int main()
{
    ifstream fin("img.txt");
    ofstream fout("result.txt");

    int img_count = 0;
    Size img_size;//图像尺寸
    Size board_size = Size(4, 6);//行列上的内角点数量
    vector per_img_point;//每张图检测的角点数量
    vector > img_point;

    string filename;
    cout<<"寻找角点"< > object_points; //世界坐标系中的三维坐标

    Mat camereaMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//摄像机的内参矩阵
    vector point_count; //每幅图中角点数量
    Mat distCoffeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); //摄像机的畸变系数

    vector rotaMatrix; //旋转向量，最后需要转换为旋转矩阵
    vector transMatrix; //平移向量


    // 初始化标定板上角点的世界坐标
    for(int num=0; num temp;
        for(int i=0; i new_point;//重投影之后的角点坐标
    fout<<"每幅图的标定误差 \n";
    cout<<"每幅图的标定误差"< temp_points = object_points[i];
        //得到新投影之后点的坐标
        projectPoints(temp_points, rotaMatrix[i], transMatrix[i], camereaMatrix, distCoffeffs, new_point);
        //计算新旧投影点的误差
        vector origin_points = img_point[i];
        Mat new_point_matrix = Mat(1, new_point.size(), CV_32FC2);
        Mat new_origin_matrix = Mat(1, origin_points.size(), CV_32FC2);

        for(int j=0; j(0, j) = Vec2f(new_point[j].x, new_point[j].y);
            new_origin_matrix.at(0, j) = Vec2f(origin_points[j].x, origin_points[j].y);

            per_err = norm(new_point_matrix, new_origin_matrix, NORM_L2)/origin_points.size();
            total_err += per_err;
        }
        cout<<"第"<