OPENCV3.0 單目攝像頭標定(使用官方自帶的標定圖片)

// opencv_test.cpp : 定義控制檯應用程序的入口點。  
//  
  
#include "stdafx.h"  
#include <opencv2/opencv.hpp>  
#include <highgui.hpp>  
#include "cv.h"  
#include <cv.hpp>  
#include <iostream>  
  
using namespace std;  
using namespace cv;  
  
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);   //  
      
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);  
    /*輸出數據*/  
    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;  
}  
  
  
int _tmain(int argc, _TCHAR* argv[])  
{  
    Mat img;  
    int goodFrameCount = 0;  
    namedWindow("chessboard");  
    cout << "按Q退出 ..." << endl;  
    while (goodFrameCount < frameNumber)  
    {  
        char filename[100];  
        sprintf_s(filename,"image\\left%02d.jpg", 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 << "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");  
    return 0;  
}