張氏相機標定法和畸變矯正opencv代碼

原理部分可見上一篇博客，這一部分主要是關於opencv實現：
這部分代碼參考網上教程張氏標定法，但我覺得部分地方可能存在問題，後續會繼續看一下官方代碼

完整代碼和棋盤圖片下載可從這裏下載

基本思路爲:

1. 檢測代標定圖像的內角點findChessboardCorners
2. 利用find4QuadCornerSubpix尋找更精細的像素級座標
3. 根據測量的標定板的格子尺寸得到真實世界座標系中內角點座標
4. 利用calibrateCamera得到相機的矩陣，畸變矩陣，和每個圖像的的旋轉平移向量
5. 利用projectPoints重投影，從而計算誤差
6. 利用undistort進行畸變矯正，畸變矯正中只需要攝像機的內參

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include<opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include<iostream>
#include<fstream>

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<Point2f> per_img_point;//每張圖檢測的角點數量
    vector<vector<Point2f> > img_point;

    string filename;
    cout<<"尋找角點"<<endl;
    while(getline(fin, filename))
    {
        cout<<filename<<endl;
        img_count++;
        Mat img = cv::imread(filename);
        if(img.empty())
        {
            cout<<"can't read the image"<<endl;
            return -2;
        }
        //獲取圖像尺寸
        if (img_count==1)
        {
            img_size.width = img.cols;
            img_size.height = img.rows;
        }
		// 像素座標系
        if(findChessboardCorners(img, board_size, per_img_point)==0)
        {
            cout<<"can not find corners"<<endl;
            return -1;
        }
        else
        {
            Mat img_gray;
            cvtColor(img, img_gray, CV_RGB2GRAY);//轉換爲灰度圖來通過亞像素精確化尋找角點座標
            // Size(5, 5)角點窗口的尺寸
            find4QuadCornerSubpix(img_gray, per_img_point, Size(5, 5));
            img_point.push_back(per_img_point);

            //在圖像顯示角點位置
            drawChessboardCorners(img_gray, board_size, per_img_point, true);
            imshow("corners", img_gray);
            waitKey(500);
        }

    }

    cout<<"圖像數量"<<img_count<<endl;

    int corner_number = img_point.size();//所有的角點數
    int per_corner_number = board_size.width * board_size.height;//每張圖的角點數

    cout<<"開始標定"<<endl;
    Size square_size = Size(10, 10);//每個棋盤格大小
    vector<vector<Point3f> > object_points; //世界座標系中的三維座標

    Mat camereaMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//攝像機的內參矩陣
    vector<int> point_count; //每幅圖中角點數量
    Mat distCoffeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); //攝像機的畸變係數

    vector<Mat> rotaMatrix; //旋轉向量，最後需要轉換爲旋轉矩陣
    vector<Mat> transMatrix; //平移向量


    // 初始化標定板上角點的世界座標
    for(int num=0; num<img_count; num++)
    {
        vector<Point3f> temp;
        for(int i=0; i<board_size.height; i++)
        {
            for(int j=0; j<board_size.width; j++)
            {
                Point3f realPoints;
                // 假設世界座標系中z=0座標很奇怪
                realPoints.x = i*square_size.width;
                realPoints.y = j*square_size.height;
                realPoints.z = 0;
                temp.push_back(realPoints);
            }
        }
        object_points.push_back(temp);
    }

    //標定圖像中角點數量理論是這麼多
    for(int i=0; i<img_count; i++)
    {
        point_count.push_back(board_size.width*board_size.height);
    }

    //開始標定
    calibrateCamera(object_points, img_point, img_size, camereaMatrix, distCoffeffs, rotaMatrix, transMatrix, 0);
    cout<<"標定完成"<<endl;
    cout<<"評價標定結果"<<endl;

    double total_err;
    double per_err;
    vector<Point2f> new_point;//重投影之後的角點座標
    fout<<"每幅圖的標定誤差 \n";
    cout<<"每幅圖的標定誤差"<<endl;

    for(int i=0; i<img_count; i++)
    {
        vector<Point3f> temp_points = object_points[i];
        //得到新投影之後點的座標
        projectPoints(temp_points, rotaMatrix[i], transMatrix[i], camereaMatrix, distCoffeffs, new_point);
        //計算新舊投影點的誤差
        vector<Point2f> 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<origin_points.size(); j++)
        {
            new_point_matrix.at<Vec2f>(0, j) = Vec2f(new_point[j].x, new_point[j].y);
            new_origin_matrix.at<Vec2f>(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<<"第"<<i+1<<"幅圖的平均誤差"<<per_err<<"像素"<<endl;
        fout<<"第"<<i+1<<"幅圖的平均誤差"<<per_err<<"像素"<<endl;
    }
    cout<<"總體平均誤差爲"<<total_err/img_count<<endl;
    fout<<"總體平均誤差爲"<<total_err/img_count<<endl;
    cout<<"評價完成"<<endl;


    cout<<"保存標定結果"<<endl;
    fout<<"相機內參矩陣"<<endl;
    fout<<camereaMatrix<<endl;
    fout<<"畸變係數"<<endl;
    fout<<distCoffeffs<<endl;
    Mat rota_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//旋轉矩陣

    //保存旋轉矩陣和平移向量
    for(int i=0; i<img_count; i++){
        fout<<"第"<<i+1<<"幅圖的旋轉向量"<<endl;
        fout<<rotaMatrix[i]<<endl;
        fout<<"第"<<i+1<<"幅圖的旋轉矩陣"<<endl;
        Rodrigues(rotaMatrix[i], rota_matrix);
        fout<<rota_matrix<<endl;
        fout<<"第"<<i+1<<"幅圖的平移向量"<<endl;
        fout<<transMatrix[i]<<endl<<endl;
    }

    cout<<"完成保存"<<endl;
    fout<<endl;

    //圖像矯正
    Mat src = imread("1_d.jpg");
    Mat distoration = src.clone();
    undistort(src, distoration, camereaMatrix, distCoffeffs);
    imwrite("undisotation.jpg", distoration);

    return 0;





}