相機標定程序詳解<2>

#ifndef _CAMERACALIBRATE_H_
#define _CAMERACALIBRATE_H_

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

using namespace cv;
using namespace std;
class CameraCalibrate
{
private:
    vector<vector<Point3f>> objectPoints;    // 角點的世界坐標系坐標
    vector<vector<Point2f>> imagePoints;     // 角點的像素坐標系坐標
    Mat cameraMatrix;                        // 內參矩陣
    Mat distCoeffs;                          // 畸變矩陣
    vector<Mat> rvecs, tvecs;                // 旋轉矩陣隊列和平移矩陣隊列，每一幅標定圖像都有對應的一個旋轉矩陣和平移矩陣
    vector<double> calibrateErrs;            // 保存矯正偏差

    int flag;
    cv::Mat map1, map2;
    bool mustInitUndistort;

public:
    CameraCalibrate() :flag(0), mustInitUndistort(true) {};
    int addChessboardPoints(const vector<string>& filelist, Size& boardSize);
    void addPoints(const std::vector<cv::Point2f>& imageCorners, const std::vector<cv::Point3f>& objectCorners);
    double CalibCamera(Size imageSize);
    Mat remap(Mat & image);
    void setCalibrationFlag(bool radial8CoeffEnabled = false, bool tangentialParamEnabled = false);
    void computeCalibrateError();
    Mat getCameraMatrix() { return cameraMatrix; }
    Mat getDistCoeffs() { return distCoeffs; }
    vector<double>getCalibrateErrs() { return calibrateErrs; }
    ~CameraCalibrate() {};
};

#endif

#include "CameraCalibrate.h"
int CameraCalibrate::addChessboardPoints(const vector<string>& filelist, Size& boardSize)
{
    vector<Point2f> imageCorners;
    vector<Point3f> objectCorners;

    for (int i = 0; i < boardSize.height; i++)
    {
        for (int j = 0; j < boardSize.width; j++)
        {
            objectCorners.push_back(Point3f(i, j, 0.0f));// 保存世界坐標系坐標
        }
    }

    Mat image, grayImage;
    int success = 0;
    for (int i = 0; i < filelist.size(); i++)
    {
        image = imread(filelist[i]);
        cvtColor(image, grayImage, COLOR_BGR2GRAY);
        bool found = findChessboardCorners(grayImage, boardSize, imageCorners);
        cornerSubPix(grayImage, imageCorners, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 30, 0.1));// 計算亞像素級別角點信息
        //如果角點數目滿足要求，那么將它加入數據
        if (imageCorners.size() == boardSize.area())
        {
            addPoints(imageCorners, objectCorners);// 保存像素坐標系坐標
            success++;
        }

        drawChessboardCorners(image, boardSize, imageCorners, found);
        imshow("Corners on Chessboard", image);// 顯示角點信息
        cv::waitKey(100);
    }    
    return success;
}

void CameraCalibrate::addPoints(const vector<Point2f>& imageCorners, const vector<Point3f>& objectCorners)
{ 
    imagePoints.push_back(imageCorners);    
    objectPoints.push_back(objectCorners);
}

double CameraCalibrate::CalibCamera(Size imageSize)
{
    mustInitUndistort = true;
    double ret = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flag);
    return ret;
}

Mat CameraCalibrate::remap(Mat & image)
{
    Mat undistorted;
    if (mustInitUndistort)//每次標定只需要初始化一次
    {
        //計算無畸變和修正轉換映射。 
        initUndistortRectifyMap(cameraMatrix,distCoeffs, cv::Mat(), cv::Mat(), image.size(), CV_32FC1, map1, map2);// 首先計算得到用於矯正計算的兩個矩陣map1和map2
        mustInitUndistort = false;
    }
    cv::remap(image, undistorted, map1, map2, cv::INTER_LINEAR);// 矯正運算
    return undistorted;
}

void CameraCalibrate::setCalibrationFlag(bool radial8CoeffEnabled , bool tangentialParamEnabled )
{
    flag = 0;
    if (!tangentialParamEnabled)
        flag += CALIB_ZERO_TANGENT_DIST;
    if (radial8CoeffEnabled)
        flag += CALIB_RATIONAL_MODEL;
}

void CameraCalibrate::computeCalibrateError()
{
    int image_count = rvecs.size();    
    for (int i = 0; i < image_count; i++) 
    {
        std::vector<cv::Point2f> result_image_points;// 保存反向映射后的像素坐標系坐標        
        cv::projectPoints(objectPoints[i], rvecs[i], tvecs[i],cameraMatrix, distCoeffs, result_image_points);// 將世界坐標系坐標反向投影到像素坐標系中        
        cv::Mat original_image_points_Mat = cv::Mat(1, objectPoints[i].size(), CV_32FC2);        
        cv::Mat result_image_points_Mat = cv::Mat(1, result_image_points.size(), CV_32FC2);         /* 使用二階笵數計算矩陣間的差異作為矯正偏差 */        
        for(int j = 0; j < objectPoints[i].size(); j++)        
        {            
            original_image_points_Mat.at<cv::Vec2f>(0,j) = cv::Vec2f(imagePoints[i][j].x, imagePoints[i][j].y);            
            result_image_points_Mat.at<cv::Vec2f>(0,j) = cv::Vec2f(result_image_points[j].x, result_image_points[j].y);     
        }        
        double err = cv::norm(original_image_points_Mat, result_image_points_Mat, cv::NORM_L2);        
        err /= result_image_points.size();        
        calibrateErrs.push_back(err);    
    }
}

#include <iostream>
#include "CameraCalibrate.h"
#include <fstream>  
#include <io.h>     
#include <string>
#include <direct.h> 
#include <vector> 
void getFiles(std::string path, std::vector<std::string>& files)
{

    intptr_t hFile = 0;//win10使用
    struct _finddata_t fileinfo;
    string p;
    if ((hFile = _findfirst(p.assign(path).append("\\*").c_str(), &fileinfo)) != -1)
    {
        do 
        { 
            if ((fileinfo.attrib & _A_SUBDIR)) 
            { 
                if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)        
                    getFiles(p.assign(path).append("\\").append(fileinfo.name), files); 
            }
            else 
            { 
                files.push_back(p.assign(path).append("\\").append(fileinfo.name));
            }
        } while (_findnext(hFile, &fileinfo) == 0);
        _findclose(hFile);
    }
}

std::string path = "E:\\欣奕華\\項目\\視覺\\標定\\chess";
void main()
{
    vector<string>files;
    getFiles(path, files);

    Mat image;
    CameraCalibrate cameraCali;
    Size boardSize(6, 4);// 標定圖像中每行、列中內角點的數量
    image = imread(files[7], 0);

    cameraCali.addChessboardPoints(files, boardSize);
    cameraCali.CalibCamera(image.size());
    Mat uImage = cameraCali.remap(image);// 對圖像進行矯正操作
    imshow("Original Image", image);
    imshow("Undistorted Image", uImage);

    cout << "相機內參矩陣：" << endl;
    Mat cameraMatrix = cameraCali.getCameraMatrix();
    for (int i = 0; i < cameraMatrix.rows; i++)
    {
        for (int j = 0; j < cameraMatrix.cols; j++)
        {
            cout << cameraMatrix.at<double>(i, j) << "   ";
        }
        cout << endl;
    }
    cout << "相機畸變矩陣：" << endl;
    Mat distCoeffs = cameraCali.getDistCoeffs();
    for (int i = 0; i < distCoeffs.rows; i++)
    {
        for (int j = 0; j < distCoeffs.cols; j++)
        {
            cout << distCoeffs.at<double>(i, j) << "   ";
        }
        cout << endl;
    }

    cout << "標定結果：" << endl;
    cameraCali.computeCalibrateError();
    vector<double> errs = cameraCali.getCalibrateErrs();
    for (int i = 0; i < errs.size(); i++)
    {
        std::cout << "第" << i + 1 << "幅圖像的平均矯正偏差： " << errs[i] << "像素大小" << std::endl;
    }
    waitKey(0);

相機標定程序解讀：

1. 讀取文件圖片；

2.初始化圖片角點在世界坐標系的位置：

for (int i = 0; i < boardSize.height; i++)
    {
        for (int j = 0; j < boardSize.width; j++)
        {
            objectCorners.push_back(Point3f(i, j, 0.0f));// 保存世界坐標系坐標
        }
    }

3.遍歷文件夾中所有圖片，並將其轉化為灰度圖：

cvtColor(image, grayImage, COLOR_BGR2GRAY);

4. 尋找棋盤圖的內角點位置

 bool found = findChessboardCorners(grayImage, boardSize, imageCorners);

bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners, int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE );
第一個參數Image，傳入拍攝的棋盤圖Mat圖像，必須是8位的灰度或者彩色圖像；
第二個參數patternSize，每個棋盤圖上內角點的行列數，一般情況下，行列數不要相同，便於后續標定程序識別標定板的方向；
第三個參數corners，用於存儲檢測到的內角點圖像坐標位置，一般用元素是Point2f的向量來表示：vector<Point2f> image_points_buf;
第四個參數flage：用於定義棋盤圖上內角點查找的不同處理方式，有默認值。

返回值：如果找到角點返回1，沒有找到返回：0；

解釋：a regular chessboard has 8 x 8 squares and 7 x 7 internal corners, that is, points where the black squares touch each other. The detected coordinates are approximate, and to determine their positions more accurately, the function calls cornerSubPix. You also may use the function cornerSubPix with different parameters if returned coordinates are not accurate enough.

5. 精確確定棋盤格內角點位置

 

cornerSubPix(grayImage, imageCorners, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 30, 0.1));// 計算亞像素級別角點信息

 

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

image：輸入圖像
corners：輸入角點的初始坐標以及精准化后的坐標用於輸出
winSize：搜索窗口邊長的一半，例如如果winSize=Size(5,5)，則一個大小為：的搜索窗口將被使用。
zeroZone：搜索區域中間的dead region邊長的一半，有時用於避免自相關矩陣的奇異性。如果值設為(-1,-1)則表示沒有這個區域。
criteria：角點精准化迭代過程的終止條件。也就是當迭代次數超過criteria.maxCount，或者角點位置變化小於criteria.epsilon時，停止迭代過程。
CvTermCriteria 類：迭代算法的終止准則

原型：
`typedef struct CvTermCriteria
{
int    type;  /* CV_TERMCRIT_ITER 和CV_TERMCRIT_EPS二值之一，或者二者的組合 */
int    max_iter; /* 最大迭代次數 */
double epsilon; /* 結果的精確性 */
宏定義：
CV_TERMCRIT_ITER：代終止條件為達到最大迭代次數終止
CV_TERMCRIT_EPS：迭代到閾值終止

6.保存角點在圖像坐標系中坐標以及在世界坐標系的坐標

 角點的圖像坐標系是二維空間，定義vector<Point2f>imageCorner

 角點的世界坐標系是三維空間，定義vector<Point3f>projectCorner

 判斷條件判：  如果角點數目滿足要求，那么將它加入數據

 最后調用 addPoints(const vector<Point2f>& imageCorners, const vector<Point3f>& objectCorners)；

7. 求出角點在兩坐標系中坐標后，調用標定函數

calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flag);

計算相機內參和外參系數

double calibrateCamera( InputArrayOfArrays objectPoints, 
                                     InputArrayOfArrays imagePoints, 
                                     Size imageSize, 
                                     CV_OUT InputOutputArray cameraMatrix, 
                                     CV_OUT InputOutputArray distCoeffs, 
                                     OutputArrayOfArrays rvecs,   OutputArrayOfArrays tvecs, 
                                     int flags=0, TermCriteria criteria = TermCriteria( 
                                     TermCriteria::COUNT+TermCriteria::EPS, 30,DBL_EPSILON) ); 
 
第一個參數objectPoints，為世界坐標系中的三維點。在使用時，應該輸入一個三維坐標點的向量的向量，即vector<vector<Point3f>> object_points。需要依據棋盤上單個黑白矩陣的大小，計算出（初始化）每一個內角點的世界坐標。
第二個參數imagePoints，為每一個內角點對應的圖像坐標點。和objectPoints一樣，應該輸入vector<vector<Point2f>>image_points_seq形式的變量；
第三個參數imageSize，   為圖像的像素尺寸大小，在計算相機的內參和畸變矩陣時需要使用到該參數；
第四個參數cameraMatrix為相機的內參矩陣。輸入一個Mat cameraMatrix即可，如Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0));
第五個參數distCoeffs為畸變矩陣。輸入一個Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0))即可；
第六個參數rvecs為旋轉向量；應該輸入一個Mat類型的vector，即vector<Mat>rvecs;
第七個參數tvecs為位移向量，和rvecs一樣，應該為vector<Mat> tvecs；
第八個參數flags為標定時所采用的算法。有如下幾個參數：
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是最優迭代終止條件設定。
在使用該函數進行標定運算之前，需要對棋盤上每一個內角點的空間坐標系的位置坐標進行初始化，標定的結果是生成相機的內參矩陣cameraMatrix、相機的5個畸變系數distCoeffs，另外每張圖像都會生成屬於自己的平移向量和旋轉向量。

8. 查看標定效果——利用標定結果對棋盤圖進行矯正

initUndistortRectifyMap(cameraMatrix,distCoeffs, cv::Mat(), cv::Mat(), image.size(), CV_32FC1, map1, map2);// 首先計算得到用於矯正計算的兩個矩陣map1和ma

利用求得的相機的內參和外參數據，可以對圖像進行畸變的矯正，這里有兩種方法可以達到矯正的目的，分別說明一下。

方法一：使用initUndistortRectifyMap和remap兩個函數配合實現。
initUndistortRectifyMap用來計算畸變映射，remap把求得的映射應用到圖像上。
方法二：使用undistort函數實現
正向矯正的流程為：畸變像素坐標→畸變物理坐標→標准物理坐標→標准像素坐標
逆向矯正的流程為：標准像素坐標→標准物理坐標→畸變物理坐標→畸變像素坐標
UndistortPoints就是執行的正向矯正過程，而initUndistortRectifyMap執行的是逆向矯正過程。
initUndistortRectifyMap
cv::initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,
                              InputArray matR, InputArray newCameraMatrix,
                              Size size, int m1type, OutputArray map1, OutputArray map2 )

1.cameraMatrix：輸入相機矩陣
2.distCoeffs：輸入參數，相機的畸變系數：，有4，5,8,12或14個元素。如果這個向量是空的，就認為是零畸變系數。
3.matR：可選的修正變換矩陣，是個3*3的矩陣。通過stereoRectify計算得來的R1或R2可以放在這里。如果這個矩陣是空的，就假設為單位矩陣。在cvInitUndistortMap中，R被認為是單位矩陣。
4.newCameraMatrix：新的相機矩陣
5.size：未畸變的圖像尺寸。
6.m1type：第一個輸出的映射的類型，可以為 CV_32FC1, CV_32FC2或CV_16SC2，參見cv::convertMaps。
7.map1：第一個輸出映射。
8.map2：第二個輸出映射。

函數說明：

這個函數用於計算無畸變和修正轉換關系，為了重映射，將結果以映射的形式表達。無畸變的圖像看起來就想原始的圖像，就像這個圖像是用內參為newCameraMatrix的且無畸變的相機采集得到的。
在單目相機例子中，newCameraMatrix一般和cameraMatrix相等，或者可以用cv::getOptimalNewCameraMatrix來計算，獲得一個更好的有尺度的控制結果。
在雙目相機例子中，newCameraMatrix一般是用cv::stereoRectify計算而來的，設置為P1或P2。
此外，根據R，新的相機在坐標空間中的取向是不同的。例如，它幫助配准雙目相機的兩個相機方向，從而使得兩個圖像的極線是水平的，且y坐標相同（在雙目相機的兩個相機誰水平放置的情況下）。
該函數實際上為反向映射算法構建映射，供反向映射使用。也就是，對於在已經修正畸變的圖像中的每個像素，該函數計算原來圖像（從相機中獲得的原始圖像）中對應的坐標系。這個過程是這樣的： 

其中，是畸變系數。
在雙目相機的例子中，這個函數調用兩次：一次是為了每個相機的朝向，經過stereoRectify之后，依次調用cv::stereoCalibrate。但是如果這個雙目相機沒有標定，依然可以使用cv::stereoRectifyUncalibrated

直接從單應性矩陣H中計算修正變換。對每個相機，函數計算像素域中的單應性矩陣H作為修正變換，而不是3D空間中的旋轉矩陣R。R可以通過H矩陣計算得來：

 9.重映射

cv::remap(image, undistorted, map1, map2, INTER_LINEAR)

第一個參數，InputArray類型的src，輸入圖像，即源圖像，填Mat類的對象即可，且需為單通道8位或者浮點型圖像。

第二個參數，OutputArray類型的dst，函數調用后的運算結果存在這里，即這個參數用於存放函數調用后的輸出結果，需和源圖片有一樣的尺寸和類型。

第三個參數，InputArray類型的map1，它有兩種可能的表示對象。表示點（x，y）的第一個映射。表示CV_16SC2 , CV_32FC1 或CV_32FC2類型的X值。

第四個參數，InputArray類型的map2，同樣，它也有兩種可能的表示對象，而且他是根據map1來確定表示那種對象。若map1表示點（x，y）時。這個參數不代表任何值。表示CV_16UC1 , CV_32FC1類型的Y值（第二個值）。

第五個參數，int類型的interpolation,插值方式，之前的resize( )函數中有講到，需要注意，resize( )函數中提到的INTER_AREA插值方式在這里是不支持的，所以可選的插值方式如下：INTER_NEAREST - 最近鄰插值INTER_LINEAR – 雙線性插                      值（默認值）INTER_CUBIC – 雙三次樣條插值（逾4×4像素鄰域內的雙三次插值）INTER_LANCZOS4 -Lanczos插值（逾8×8像素鄰域的Lanczos插值）

第六個參數，int類型的borderMode，邊界模式，有默認值BORDER_CONSTANT，表示目標圖像中“離群點（outliers）”的像素值不會被此函數修改.

第七個參數，const Scalar&類型的borderValue，當有常數邊界時使用的值，其有默認值Scalar( )，即默認值為0。

這個函數可參考：https://blog.csdn.net/qq_20823641/article/details/52232046，講述映射以及原理。

10.對標定結果進行評價。

對標定結果進行評價的方法是通過得到的攝像機內外參數，對空間的三維點進行重新投影計算，得到空間三維點在圖像上新的投影點的坐標，計算投影坐標和亞像素角點坐標之間的偏差，偏差越小，標定結果越好。

對空間三維坐標點進行反向投影的函數是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，為相機坐標系中的三維點坐標；

第二個參數rvec為旋轉向量，每一張圖像都有自己的選擇向量；

第三個參數tvec為位移向量，每一張圖像都有自己的平移向量；

第四個參數cameraMatrix為求得的相機的內參數矩陣；

第五個參數distCoeffs為相機的畸變矩陣；

第六個參數iamgePoints為每一個內角點對應的圖像上的坐標點；

第七個參數jacobian是雅可比行列式；

第八個參數aspectRatio是跟相機傳感器的感光單元有關的可選參數，如果設置為非0，則函數默認感光單元的dx/dy是固定的，會依此對雅可比矩陣進行調整；

 

備注：關於畸變矯正仍需要看看書中詳細介紹，還有網址講述了矯正函數的重定義。