第六節、雙目視覺之相機標定

時間 2019-11-10

原文原文鏈接

去年三四月份實驗室作了一個機器人與視覺識別系統的項目，主要就是利用雙目攝像頭進行物體空間座標定位，而後利用機器人進行抓取物體。當時我才研一，仍是個菜雞，項目主要是幾個學長負責作的，我也就是參與打打醬油，混混經驗。如今過了一年多了，機器人一直在實驗室放着，空着也是浪費，因此就想搞點事情。這裏咱們就先從利用雙目攝像頭進行空間定位提及，所以這是整個項目的核心部分。html

雙目視覺是創建在幾何數學的基礎上，數學推導是枯燥乏味的。所以這裏不去過多的介紹數學原理，只是簡要的敘述一下雙目視覺的流程。ios

雙目視覺主要包括相機標定、圖片畸變矯正、攝像機校訂、圖片匹配、3D恢復五個部分。算法

下面咱們從相機標定開始提及。相機標定的目的有兩個。數組

第一，要還原攝像頭成像的物體在真實世界的位置就須要知道世界中的物體到計算機圖像平面是如何變換的，相機標定的目的之一就是爲了搞清楚這種變換關係，求解內外參數矩陣。
第二，攝像機的透視投影有個很大的問題——畸變。攝像頭標定的另外一個目的就是求解畸變係數，而後用於圖像矯正。

1、三大座標系

談到相機標定，咱們不得不提及攝相機座標系、世界座標系、圖像座標系。緩存

上圖是三個座標的示意簡圖，經過它你們能夠對三個座標有一個直觀的認識。架構

世界座標系（X_W,Y_W,Z_W）：目標物體位置的參考系。除了無窮遠，世界座標能夠根據運算方便與否自由放置，單位爲長度單位如mm。。在雙目視覺中世界座標系主要有三個用途：框架

一、標定時肯定標定物的位置；ide

二、做爲雙目視覺的系統參考系，給出兩個攝像機相對世界座標系的關係，從而求出相機之間的相對關係；函數

三、做爲重建獲得三維座標的容器，存放重建後的物體的三維座標。世界座標系是將看見中物體歸入運算的第一站。工具

攝像機座標系(X_C,Y_C,Z_C)：攝像機站在本身角度上衡量的物體的座標系。攝像機座標系的原點在攝像機的光心上，z軸與攝像機光軸平行。它是與拍攝物體發生聯繫的橋頭堡，世界座標系下的物體需先經歷剛體變化轉到攝像機座標系，而後在和圖像座標系發生關係。它是圖像座標與世界座標之間發生關係的紐帶，溝通了世界上最遠的距離。單位爲長度單位如mm。

圖像座標系(x,y):以CCD 圖像平面的中心爲座標原點，爲了描述成像過程當中物體從相機座標系到圖像座標系的投影透射關係而引入，方便進一步獲得像素座標系下的座標。圖像座標系是用物理單位（例如毫米）表示像素在圖像中的位置。

像素座標系(u,v) ：以 CCD 圖像平面的左上角頂點爲原點，爲了描述物體成像後的像點在數字圖像上（相片）的座標而引入，是咱們真正從相機內讀取到的信息所在的座標系。像素座標系就是以像素爲單位的圖像座標系。

(備註：)有不少人把圖像座標系和像素座標系合在一塊兒，稱做三大座標系，也有人分開，稱爲四大座標系。

一、圖像座標系到像素座標系

講到這裏，你可能會問有了圖像座標系爲何還要建一個像素座標系？

咱們以圖像左上角爲原點創建以像素爲單位的直接座標系u-v。像素的橫座標u與縱座標v分別是在其圖像數組中所在的列數與所在行數。

因爲(u,v)只表明像素的列數與行數，而像素在圖像中的位置並無用物理單位表示出來，因此，咱們還要創建以物理單位（如毫米）表示的圖像座標系x-y。將相機光軸與圖像平面的交點（通常位於圖像平面的中心處，也稱爲圖像的主點（principal point）定義爲該座標系的原點O1，且x軸與u軸平行，y軸與v軸平行，假設(u0,v0)表明O1在u-v座標系下的座標，dx與dy分別表示每一個像素在橫軸x和縱軸y上的物理尺寸，則圖像中的每一個像素在u-v座標系中的座標和在x-y座標系中的座標之間都存在以下的關係：

其中，咱們假設物理座標系中的單位爲毫米，那麼dx的的單位爲:毫米/像素。那麼x/dx的單位就是像素了，即和u的單位同樣都是像素。爲了使用方便，可將上式用齊次座標與矩陣形式表示爲：

爲了讓你更直接的理解這一塊內容，咱們舉個例子。因爲被攝像機攝物體的圖像通過鏡頭投影到CCD芯片上(像平面)，咱們設CCD的大小爲8x6mm,而拍攝到的圖像大小爲640x480，則dx=1/80mm/像素，dy=1/80mm/像素,u0=320,v0=240。

上面的矩陣公式運用了齊次座標，初學者可能會感到有些迷惑。你們會問：怎樣將普通座標轉換爲齊次座標呢？齊次座標能帶來什麼好處呢？

這裏對齊次座標作一個通俗的解釋。此處只講怎麼將普通座標改寫爲齊次座標及爲何引入齊次座標。這裏只作一個通俗但不太嚴謹的表述。力求簡單明瞭。針對齊次座標的嚴謹的純數學推導，可參見「周興和版的《高等幾何》---1.3拓廣平面上的齊次座標」。玉米曾詳細讀過《高等幾何》這本書，但以爲離計算機視覺有點遠，是講純數學的投影關係的，較爲生澀難懂。

齊次座標能夠理解爲在原有座標後面加一個「小尾巴」。將普通座標轉換爲齊次座標，一般就是在增長一個維度，這個維度上的數值爲1。如圖像座標系(u,v)轉換爲(u,v,1)同樣。對於無窮遠點，小尾巴爲0。注意，給零向量增長小尾巴，數學上無心義。

那麼，爲何計算機視覺在座標運算時要加上這個「小尾巴」呢？

一、將投影平面擴展到無窮遠點。如對消隱點(vanishing point)的描述。

二、使得計算更加規整

若是用普通座標來表達的話，會是下面的樣子：

這樣的運算形式會給後與運算帶來必定的麻煩，因此齊次座標是一個更好的選擇。

齊次座標還有一個重要的性質，伸縮不變性。即：設齊次座標M，則αM=M。

咱們介紹過了像素座標系以後，咱們再次三大座標系的問題上。咱們想知道這三個座標系有什麼樣的關係，咱們先從下圖提及：

圖中顯示，世界座標系經過剛體變換到達攝像機座標系，而後攝像機座標系經過透視投影變換到達圖像座標系。能夠看出，世界座標與圖像座標的關係創建在剛體變換和透視投影變換的基礎上。

二、世界座標系到攝像機座標系

首先，讓咱們來看一下剛體變換是如何將世界座標系與圖像座標系聯繫起來的吧。這裏，先對剛體變換作一個介紹：

剛體變換(regidbody motion):三維空間中，當物體不發生形變時，對一個幾何物體做旋轉，平移的運動，稱之爲剛體變換。

由於世界座標系和攝像機座標都是右手座標系，因此其不會發生形變。咱們想把世界座標系下的座標轉換到攝像機座標下的座標，以下圖所示，能夠經過剛體變換的方式。空間中一個座標系，總能夠經過剛體變換轉換到另一個個座標系的。

下面看一下，兩者之間剛體變換的數學表達：

對應的齊次表達式爲：

其中，R是3×3的正交單位矩陣（即旋轉矩陣），t爲平移向量，R、T與攝像機無關，因此稱這兩個參數爲攝像機的外參數(extrinsic parameter)，能夠理解爲兩個座標原點之間的距離，因其受x,y,z三個方向上的份量共同控制，因此其具備三個自由度。

咱們假定在世界座標系中物點所在平面過世界座標系原點且與Zw軸垂（也即棋盤平面與Xw-Yw平面重合，目的在於方便後續計算），則Zw=0。 $X_{W} - Y_{W}$

三、攝像機座標系到圖像座標系

首先，讓咱們來看一下透視投影是如何將圖像座標系與圖像座標系聯繫起來的吧。這裏，先對透視投影作一個介紹：

透視投影(perspective projection): 用中心投影法將形體投射到投影面上，從而得到的一種較爲接近視覺效果的單面投影圖。有一點像皮影戲。它符合人們心理習慣，即離視點近的物體大，離視點遠的物體小，不平行於成像平面的平行線會相交於消隱點(vanish point)

這裏咱們仍是拿針孔成像來講明（除了成像亮度低外，成像效果和透視投影是同樣的，可是光路更簡單）

下圖是針孔-攝像機的基本模型。平面π稱爲攝像機的像平面，點Oc稱 $O_{c}$

如圖所示，圖像座標系爲o-xy，攝像機座標系爲Ox-x_cy_cz_c $O_{c} - x_{c} y_{c} z_{c}$

$O_{c} - x_{c} y_{c} z_{c}$

它的像點m在圖像座標系中的齊次座標記爲：

根據三角形類似原理，可得：

咱們使用矩陣表示爲：

注意因爲齊次座標的伸縮不變性，z_c[x,y,1]^T和(x,y,1)^T表示的是同一點。

四、總結

咱們已經介紹了各個座標系之間的轉換過程，可是咱們想知道的是如何從世界座標系轉換到像素座標系，所以咱們須要把上面介紹到的聯繫起來：

將三者相乘，能夠把這三個過程和在一塊兒，寫成一個矩陣：

咱們取世界座標到圖像座標變換矩陣P以下：

P就表示了一個投影相機，有下面公式：

其中：

咱們設：

最後用一幅圖來總結從世界座標系到像素座標系（不考慮畸變）的轉換關係：

2、圖片矯正

咱們在攝像機座標系到圖像座標系變換時談到透視投影。攝像機拍照時經過透鏡把實物投影到像平面上，可是透鏡因爲製造精度以及組裝工藝的誤差會引入畸變，致使原始圖像的失真。所以咱們須要考慮成像畸變的問題。

透鏡的畸變主要分爲徑向畸變和切向畸變，還有薄透鏡畸變等等，但都沒有徑向和切向畸變影響顯著，因此咱們在這裏只考慮徑向和切向畸變。

徑向畸變

顧名思義，徑向畸變就是沿着透鏡半徑方向分佈的畸變，產生緣由是光線在原理透鏡中心的地方比靠近中心的地方更加彎曲，這種畸變在普通廉價的鏡頭中表現更加明顯，徑向畸變主要包括桶形畸變和枕形畸變兩種。如下分別是枕形和桶形畸變示意圖：

它們在真實照片中是這樣的：

像平面中心的畸變爲0，沿着鏡頭半徑方向向邊緣移動，畸變愈來愈嚴重。畸變的數學模型能夠用主點（principle point）周圍的泰勒級數展開式的前幾項進行描述，一般使用前兩項，即k1和k2，對於畸變很大的鏡頭，如魚眼鏡頭，能夠增長使用第三項k3來進行描述，成像儀上某點根據其在徑向方向上的分佈位置，調節公式爲：

式裏（x0，y0）是畸變點在像平面的原始位置，（x，y）是畸變較正後新的位置，下圖是距離光心不一樣距離上的點通過透鏡徑向畸變後點位的偏移示意圖，能夠看到，距離光心越遠，徑向位移越大，表示畸變也越大，在光心附近，幾乎沒有偏移。

切向畸變

切向畸變是因爲透鏡自己與相機傳感器平面（像平面）或圖像平面不平行而產生的，這種狀況可能是因爲透鏡被粘貼到鏡頭模組上的安裝誤差致使。畸變模型能夠用兩個額外的參數p1和p2來描述：

下圖顯示某個透鏡的切向畸變示意圖，大致上畸變位移相對於左下——右上角的連線是對稱的，說明該鏡頭在垂直於該方向上有一個旋轉角度。

徑向畸變和切向畸變模型中一共有5個畸變參數，在Opencv中他們被排列成一個5*1的矩陣，依次包含k一、k二、p一、p二、k3，常常被定義爲Mat矩陣的形式，如Mat distCoeffs=Mat（1,5，CV_32FC1，Scalar::all（0））；這5個參數就是相機標定中須要肯定的相機的5個畸變係數。求得這5個參數後，就能夠校訂因爲鏡頭畸變引發的圖像的變形失真，下圖顯示根據鏡頭畸變係數校訂後的效果：

3、張氏標定法

相機標定的目的就是創建攝像機圖像像素位置與物體空間位置之間的關係，即世界座標系與圖像座標系之間的關係。方法就是根據攝像機模型，由已知特徵點的座標求解攝像機的模型參數，從而能夠從圖像出發恢復出空間點三維座標，即三維重建。因此要求解的參數包括4個內參數和5個畸變參數，還有外部參數旋轉矩陣和平移矩陣。

「張氏標定」是指張正友教授於1998年提出的單平面棋盤格的攝像機標定方法。張氏標定法已經做爲工具箱或封裝好的函數被普遍應用。張氏標定的原文爲「A Flexible New Technique forCamera Calibration」。此文中所提到的方法，爲相機標定提供了很大便利，而且具備很高的精度。今後標定能夠不須要特殊的標定物，只須要一張打印出來的棋盤格。

上文中咱們已經獲得了像素座標系和世界座標系下的座標映射關係，咱們 $u 、 v$

其中，u,v表示 $u 、 v$

$u 、 v$

咱們若是求取H的值？

你們能夠分析一下，H是一個三3*3的矩陣，而且有一個元素是做爲齊次座標。所以，H有8個未知量待解。

(x_w,y_w)做爲標定物的空間座標，能夠由設計者人爲控制，是已知量。(u,v)是像素座標，咱們能夠直接經過攝像機得到。對於一組對應(x_w,y_w)-(u,v)咱們能夠得到兩組方程。

如今有8個未知量須要求解，因此咱們至少須要八個方程。因此須要四個對應點。四點便可算出，圖像平面到世界平面的單應性矩陣H。

這也是張氏標定採用四個角點的棋盤格做爲標定物的一個緣由。張氏標定就是利用一張打印的棋盤格，而後對每一個角點進行標記其在像素座標系的像素點座標，以及在世界座標系的座標，張氏標定證實經過4組以上的點就能夠求解出H矩陣的值，可是爲了減小偏差，具備更強的魯棒性，咱們通常會拍攝許多張照片，選取大量的角點進行標定。具體過程以下：

打印一張棋盤格標定圖紙，將其貼在平面物體的表面.
拍攝一組不一樣方向棋盤格的圖片，能夠經過移動相機來實現，也能夠移動標定圖片來實現。
對於每張拍攝的棋盤圖片，檢測圖片中全部棋盤格的特徵點（角點，也就是下圖中黑白棋盤交叉點，中間品紅色的圓圈內就是一個角點）。咱們定義打印的棋盤圖紙位於世界座標系z_w=0的平面上，世界座標系的原點位於棋盤圖紙的固定一角（好比下圖中黃色點）。像素座標系原點位於圖片左上角。

由於棋盤標定圖紙中全部角點的空間座標是已知的，這些角點對應在拍攝的標定圖片中的角點的像素座標也是已知的，若是咱們獲得這樣的N>=4個匹配點對（越多計算結果越魯棒），就能夠根據LM等優化方法獲得單應性矩陣H。

張正友教授從數學推導上證實了張氏標定算法的可行性。但在實際標定過程當中，通常使用最大似然估計進行優化。假設咱們拍攝了n張標定圖片，每張圖片裏有m個棋盤格角點。三維空間點X_j(x_w,y_w,z_w)通過相機內參M，外參R，t變換後獲得的二維像素爲x’(u,v)，假設噪聲是獨立同分布的，咱們經過最小化x_ij(實際值：棋盤格角點在像素座標系下的實際值), x’(估計值)的位置來求解上述最大似然估計問題：

如今咱們來考慮透鏡畸變的影響，因爲徑向畸變的影響相對較明顯，因此主要考慮徑向畸變參數，根據經驗，一般只考慮徑向畸變的前兩個參數k1,k2就能夠（增長更多的參數會使得模型變的複雜且不穩定）。實際求解中，一般把k1,k2也做爲參數加入上述函數一塊兒進行優化，待優化函數以下所示：

極大似然估計是一種估計整體未知參數的方法。它主要用於點估計問題。所謂點估計是指用一個估計量的觀測值來估計未知參數的真值。說穿了就一句話：就是在參數空間中選取使得樣本取得觀測值的機率最大的參數。

這裏我沒有過多的介紹張氏標定法的數學推導過程，感興趣的童鞋能夠看一下博客最後給出來的連接。

假設咱們已經經過LM求得了相機的內參M和外參R,t。那若是獲取到圖像中二維座標，如何獲取到現實中三維座標呢？固然單目攝像機沒法獲取三維座標，但能反映必定關係。綜上來說，矩陣變換公式爲：

根據座標系之間轉化和相機模型，研究了極幾何及基本矩陣的知識，證實了基本矩陣是求得攝像機投影矩陣的關鍵，根據基礎矩陣F和攝像機標定階段獲取的內參數矩陣M，計算獲得本徵矩陣E，經過對E進行奇異值分解求得外參數矩陣R，T，接着求出投影矩陣P1和P二、重投影矩陣Q。OpenCV提供了兩個相關的函數stereoCalibrate()和stereoRectify()。

4、使用opencv實現單目標定

相機標定的目的：獲取攝像機的內參和外參矩陣（同時也會獲得每一幅標定圖像的選擇和平移矩陣），內參和外參係數能夠對以後相機拍攝的圖像就進行矯正，獲得畸變相對很小的圖像。

相機標定的輸入：標定圖像上全部內角點的圖像座標，標定板圖像上全部內角點的空間三維座標（通常狀況下假定圖像位於Z=0平面上）。

相機標定的輸出：攝像機的內參、外參係數。

這三個基礎的問題就決定了使用Opencv實現張正友法標定相機的標定流程、標定結果評價以及使用標定結果矯正原始圖像的完整流程：

1. 準備標定圖片
2. 對每一張標定圖片，提取角點信息
3.對每一張標定圖片，進一步提取亞像素角點信息
4. 在棋盤標定圖上繪製找到的內角點（非必須，僅爲了顯示）
5. 相機標定
6. 對標定結果進行評價
7. 查看標定效果——利用標定結果對棋盤圖進行矯正

準備標定圖片

標定圖片須要使用標定板在不一樣位置、不一樣角度、不一樣姿態下拍攝，最少須要3張，以10~20張爲宜。標定板須要是黑白相間的矩形構成的棋盤圖，製做精度要求較高。

這裏咱們使用OpenCV提供的sample程序中的標定圖片，圖片位於opencv(C++版本)的安裝路徑：opencv\sources\samples\data下：

咱們先建立一個C++控制檯項目，並把標定圖片按以下格式存放：

sample文件夾下有兩個文件夾left和right，分別對應左攝像頭和右攝像頭拍攝到的標定板圖片：

filename.txt存放標定圖片的路徑，內容以下：

關於OpenCV提供的用於相機標定的API函數能夠查看博客雙目視覺標定程序講解，單目標定的代碼以下：

/*************************************************************************************
*
*   Description：相機標定，張氏標定法  單目標定
*   Author     ：JNU
*   Data       ：2018.7.22
*
************************************************************************************/
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <fstream>
#include <vector>

using namespace cv;
using namespace std;

void main(char *args)
{
    //保存文件名稱
    std::vector<std::string>  filenames;

    //須要更改的參數
    //左相機標定，指定左相機圖片路徑，以及標定結果保存文件
    string infilename = "sample/left/filename.txt";        //若是是右相機把left改成right
    string outfilename = "sample/left/caliberation_result.txt";

    //標定所用圖片文件的路徑,每一行保存一個標定圖片的路徑  ifstream 是從硬盤讀到內存
    ifstream fin(infilename);
    //保存標定的結果  ofstream 是從內存寫到硬盤
    ofstream fout(outfilename);

    /*
    1.讀取毎一幅圖像，從中提取出角點，而後對角點進行亞像素精確化、獲取每一個角點在像素座標系中的座標
    像素座標系的原點位於圖像的左上角
    */
    std::cout << "開始提取角點......" << std::endl;;
    //圖像數量
    int imageCount = 0;
    //圖像尺寸
    cv::Size imageSize;
    //標定板上每行每列的角點數
    cv::Size boardSize = cv::Size(9, 6);
    //緩存每幅圖像上檢測到的角點
    std::vector<Point2f>  imagePointsBuf;
    //保存檢測到的全部角點
    std::vector<std::vector<Point2f>> imagePointsSeq;
    char filename[100];
    if (fin.is_open())
    {
        //讀取完畢？
        while (!fin.eof())
        {
            //一次讀取一行
            fin.getline(filename, sizeof(filename) / sizeof(char));
            //保存文件名
            filenames.push_back(filename);
            //讀取圖片
            Mat imageInput = cv::imread(filename);
            //讀入第一張圖片時獲取圖寬高信息
            if (imageCount == 0)
            {
                imageSize.width = imageInput.cols;
                imageSize.height = imageInput.rows;
                std::cout << "imageSize.width = " << imageSize.width << std::endl;
                std::cout << "imageSize.height = " << imageSize.height << std::endl;
            }

            std::cout << "imageCount = " << imageCount << std::endl;
            imageCount++;

            //提取每一張圖片的角點
            if (cv::findChessboardCorners(imageInput, boardSize, imagePointsBuf) == 0)
            {
                //找不到角點
                std::cout << "Can not find chessboard corners!" << std::endl;
                exit(1);
            }
            else
            {
                Mat viewGray;
                //轉換爲灰度圖片
                cv::cvtColor(imageInput, viewGray, cv::COLOR_BGR2GRAY);
                //亞像素精確化   對粗提取的角點進行精確化
                cv::find4QuadCornerSubpix(viewGray, imagePointsBuf, cv::Size(5, 5));
                //保存亞像素點
                imagePointsSeq.push_back(imagePointsBuf);
                //在圖像上顯示角點位置
                cv::drawChessboardCorners(viewGray, boardSize, imagePointsBuf, true);
                //顯示圖片
                //cv::imshow("Camera Calibration", viewGray);
                cv::imwrite("test.jpg", viewGray);
                //等待0.5s
                //waitKey(500);
            }
        }        
        
        //計算每張圖片上的角點數 54
        int cornerNum = boardSize.width * boardSize.height;

        //角點總數
        int total = imagePointsSeq.size()*cornerNum;
        std::cout << "total = " << total << std::endl;

        for (int i = 0; i < total; i++)
        {
            int num = i / cornerNum;
            int p = i%cornerNum;
            //cornerNum是每幅圖片的角點個數，此判斷語句是爲了輸出，便於調試
            if (p == 0)
            {                                        
                std::cout << "\n第 " << num+1 << "張圖片的數據 -->: " << std::endl;
            }
            //輸出全部的角點
            std::cout<<p+1<<":("<< imagePointsSeq[num][p].x;
            std::cout << imagePointsSeq[num][p].y<<")\t";
            if ((p+1) % 3 == 0)
            {
                std::cout << std::endl;
            }
        }

        std::cout << "角點提取完成!" << std::endl;

        /*
        2.攝像機標定 世界座標系原點位於標定板左上角(第一個方格的左上角)
        */
        std::cout << "開始標定" << std::endl;
        //棋盤三維信息，設置棋盤在世界座標系的座標
        //實際測量獲得標定板上每一個棋盤格的大小
        cv::Size squareSize = cv::Size(26, 26);
        //毎幅圖片角點數量
        std::vector<int> pointCounts;
        //保存標定板上角點的三維座標
        std::vector<std::vector<cv::Point3f>> objectPoints;
        //攝像機內參數矩陣 M=[fx γ u0,0 fy v0,0 0 1]
        cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F, Scalar::all(0));
        //攝像機的5個畸變係數k1,k2,p1,p2,k3
        cv::Mat distCoeffs = cv::Mat(1, 5, CV_64F, Scalar::all(0));
        //每幅圖片的旋轉向量
        std::vector<cv::Mat> tvecsMat;
        //每幅圖片的平移向量
        std::vector<cv::Mat> rvecsMat;

        //初始化標定板上角點的三維座標
        int i, j, t;
        for (t = 0; t < imageCount; t++)
        {
            std::vector<cv::Point3f> tempPointSet;
            //行數
            for (i = 0; i < boardSize.height; i++)
            {
                //列數
                for (j = 0; j < boardSize.width; j++)
                {
                    cv::Point3f realPoint;
                    //假設標定板放在世界座標系中z=0的平面上。
                    realPoint.x = i*squareSize.width;
                    realPoint.y = j*squareSize.height;
                    realPoint.z = 0;
                    tempPointSet.push_back(realPoint);
                }
            }
            objectPoints.push_back(tempPointSet);
        }

        //初始化每幅圖像中的角點數量，假定每幅圖像中均可以看到完整的標定板
        for (i = 0; i < imageCount; i++)
        {
            pointCounts.push_back(boardSize.width*boardSize.height);
        }
        //開始標定
        cv::calibrateCamera(objectPoints, imagePointsSeq, imageSize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);
        std::cout << "標定完成" << std::endl;
        //對標定結果進行評價
        std::cout << "開始評價標定結果......" << std::endl;
        //全部圖像的平均偏差的總和
        double totalErr = 0.0;
        //每幅圖像的平均偏差
        double err = 0.0;
        //保存從新計算獲得的投影點
        std::vector<cv::Point2f> imagePoints2;
        std::cout << "每幅圖像的標定偏差:" << std::endl;
        fout << "每幅圖像的標定偏差:" << std::endl;
        for (i = 0; i < imageCount; i++)
        {
            std::vector<cv::Point3f> tempPointSet = objectPoints[i];
            //經過獲得的攝像機內外參數，對空間的三維點進行從新投影計算，獲得新的投影點imagePoints2（在像素座標系下的點座標）
            cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, imagePoints2);
            //計算新的投影點和舊的投影點之間的偏差
            std::vector<cv::Point2f> tempImagePoint = imagePointsSeq[i];
            cv::Mat tempImagePointMat = cv::Mat(1, tempImagePoint.size(), CV_32FC2);
            cv::Mat imagePoints2Mat = cv::Mat(1, imagePoints2.size(), CV_32FC2);
            for (int j = 0; j < tempImagePoint.size(); j++)
            {
                imagePoints2Mat.at<cv::Vec2f>(0, j) = cv::Vec2f(imagePoints2[j].x, imagePoints2[j].y);
                tempImagePointMat.at<cv::Vec2f>(0, j) = cv::Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
            }
            //Calculates an absolute difference norm or a relative difference norm.
            err = cv::norm(imagePoints2Mat, tempImagePointMat, NORM_L2);
            totalErr += err /= pointCounts[i];
            std::cout << "  第" << i + 1 << "幅圖像的平均偏差：" << err << "像素" << endl;
            fout<<  "第" << i + 1 << "幅圖像的平均偏差：" << err << "像素" << endl;

        }
        //每張圖像的平均總偏差
        std::cout << "  整體平均偏差:" << totalErr / imageCount << "像素" << std::endl;
        fout << "整體平均偏差:" << totalErr / imageCount << "像素" << std::endl;
        std::cout << "評價完成!" << std::endl;
        //保存標定結果
        std::cout << "開始保存標定結果....." << std::endl;
        //保存每張圖像的旋轉矩陣
        cv::Mat rotationMatrix = cv::Mat(3, 3, CV_32FC1, Scalar::all(0));
        fout << "相機內參數矩陣:" << std::endl;
        fout << cameraMatrix << std::endl << std::endl;
        fout << "畸變係數:" << std::endl;
        fout << distCoeffs << std::endl << std::endl;

        for (int i = 0; i < imageCount; i++)
        {
            fout << "第" << i + 1 << "幅圖像的旋轉向量:" << std::endl;
            fout << tvecsMat[i] << std::endl;
            //將旋轉向量轉換爲相對應的旋轉矩陣
            cv::Rodrigues(tvecsMat[i], rotationMatrix);
            fout << "第" << i + 1 << "幅圖像的旋轉矩陣:" << std::endl;
            fout << rotationMatrix << std::endl;
            fout << "第" << i + 1 << "幅圖像的平移向量:" << std::endl;
            fout << rvecsMat[i] << std::endl;
        }
        std::cout << "保存完成" << std::endl;

        /************************************************************************
        顯示定標結果
        *************************************************************************/
        cv::Mat mapx = cv::Mat(imageSize, CV_32FC1);
        cv::Mat mapy = cv::Mat(imageSize, CV_32FC1);
        cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
        std::cout << "顯示矯正圖像" << endl;
        for (int i = 0; i != imageCount; i++)
        {
            std::cout << "Frame #" << i + 1 << "..." << endl;
            //計算圖片畸變矯正的映射矩陣mapx、mapy(不進行立體校訂、立體校訂須要使用雙攝)
            initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
            //讀取一張圖片
            Mat imageSource = imread(filenames[i]);
            Mat newimage = imageSource.clone();
            //另外一種不須要轉換矩陣的方式
            //undistort(imageSource,newimage,cameraMatrix,distCoeffs);
            //進行校訂
            remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
            imshow("原始圖像", imageSource);
            imshow("矯正後圖像", newimage);
            waitKey();
        }

        //釋放資源
        fin.close();
        fout.close();
        system("pause");        
    }
}

上面有兩個函數須要單獨介紹一下：

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

函數功能：該函數功能是計算畸變矯正和攝像機立體校訂的映射變換矩陣。 $爲了重映射，將結果以映射的形式表達。無畸變的圖像看起來和原始的圖像同樣，就像這個圖像是用內參爲 newCameraMatrix 的且無畸變的相機採集獲得的。$ $該函數實際上爲反向映射算法構建映射，供反向映射使用。也就是，對於已經修正畸變的圖像中的每一個像素(u,v)，該函數計算原來圖像（從相機中得到的原始圖像）中對應的座標系。$

參數說明：

cameraMatrix：輸入相機內參矩陣

distCoeffs：輸入參數，相機的畸變係數

有4，5,8,12或14個元素。若是這個向量是空的，就認爲是零畸變係數。

R：可選的立體修正變換矩陣，是個3*3的矩陣。

$在單目相機例子中，R就設置爲單位矩陣cv::Mat R = cv::Mat::eye(3, 3, CV_32F)，表示不進行立體校訂。$

在 $雙目相機例子中， newCameraMatrix 通常是用 cv::stereoRectify() 計算而來的，設置爲R1或R2(左右相機平面行對準的校訂旋轉矩陣)。此外，根據R，新的相機在座標空間中的取向是不一樣的。例如，它幫助配準雙目相機的兩個相機方向，從而使得兩個圖像的極線是水平的，且y座標相同（在雙目相機的兩個相機誰水平放置的狀況下）。$

newCameraMatrix：新的相機內參矩陣

$A = (\begin{matrix} f_{x}^{'} & 0 & c_{x}^{'} \\ 0 & f_{y}^{'} & c_{y}^{'} \\ 0 & 0 & 1 \end{matrix})$

$在單目相機例子中， newCameraMatrix 通常和 cameraMatrix 相等，或者能夠用 cv::getOptimalNewCameraMatrix() 來計算，得到一個更好的有尺度的控制結果。$

在 $雙目相機例子中， newCameraMatrix 通常是用 cv::stereoRectify() 計算而來的，設置爲P1或P2(左右相機把空間3D點的座標轉換到圖像的2D點的座標的投影矩陣)。$

size：未畸變的圖像尺寸。
.m1type：第一個輸出的映射的類型，能夠爲 CV_32FC1, CV_32FC2或CV_16SC2，參見cv::convertMaps。
.map1：第一個輸出映射。
.map2：第二個輸出映射。

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

函數功能：重映射：就是把一幅圖像中某位置的像素放置到另外一個圖片指定位置的過程。

參數說明：

src：輸入圖像，即原圖像，須要單通道8位或者浮點類型的圖像
dst：輸出圖像，即目標圖像，需和原圖形同樣的尺寸和類型
map1：它有兩種可能表示的對象：（1）表示點（x,y）的第一個映射；（2）表示CV_16SC2，CV_32FC1等
map2：它有兩種可能表示的對象：（1）若map1表示點（x,y）時，這個參數不表明任何值；（2）表示CV_16UC1，CV_32FC1類型的Y值

interpolation：插值方式，有四中插值方式：

（1）INTER_NEAREST——最近鄰插值

（2）INTER_LINEAR——雙線性插值（默認）

（3）INTER_CUBIC——雙三樣條插值（默認）

（4）INTER_LANCZOS4——lanczos插值（默認）

borderMode：邊界模式，默認BORDER_CONSTANT
borderValue：邊界顏色，默認Scalar()黑色

程序運行後，把相機內部參數和外部參數保存在caliberation_result.txt文件中，內容以下：

每幅圖像的標定偏差:
第1幅圖像的平均偏差：0.0644823像素
第2幅圖像的平均偏差：0.0769712像素
第3幅圖像的平均偏差：0.057877像素
第4幅圖像的平均偏差：0.0596713像素
第5幅圖像的平均偏差：0.0625956像素
第6幅圖像的平均偏差：0.0658863像素
第7幅圖像的平均偏差：0.0568134像素
第8幅圖像的平均偏差：0.0643699像素
第9幅圖像的平均偏差：0.058048像素
第10幅圖像的平均偏差：0.0565483像素
第11幅圖像的平均偏差：0.0590138像素
第12幅圖像的平均偏差：0.0569968像素
第13幅圖像的平均偏差：0.0698826像素
整體平均偏差:0.0622428像素
相機內參數矩陣:
[530.5277314196954, 0, 338.8371277433631;
 0, 530.5883296858968, 231.5390118666163;
 0, 0, 1]

畸變係數:
[-0.2581406917163123, -0.11124480187392, 0.0004630258905514519, -0.0009475605555950018, 0.413646790569884]

第1幅圖像的旋轉向量:
[-75.22204622827574;
 -109.7328226714255;
 412.7511174854986]
第1幅圖像的旋轉矩陣:
[0.9927105083879407, -0.1161407096490343, -0.03220531164846807;
 0.1168004495051158, 0.9929655913965856, 0.01941621224214358;
 0.02972375365863362, -0.02303627280285992, 0.999292664139887]
第1幅圖像的平移向量:
[-1.985720132175791;
 -2.010141521348128;
 0.1175016759367312]
第2幅圖像的旋轉向量:
[-57.88571684656549;
 88.73102475029921;
 365.4767680110305]
第2幅圖像的旋轉矩陣:
[-0.880518198944593, 0.2965025784551226, -0.36982958548071;
 -0.4330747951156081, -0.8203927789645991, 0.3733656519530371;
 -0.192701642865192, 0.4889191233652108, 0.8507785655767596]
第2幅圖像的平移向量:
[-2.431974050326802;
 -0.2015324617416875;
 0.2103186188188722]
第3幅圖像的旋轉向量:
[-38.96229403649615;
 -101.619482335263;
 328.7991741655258]
第3幅圖像的旋轉矩陣:
[0.7229826652152683, -0.6501194230369263, -0.2337537199455046;
 0.6686409526220074, 0.7435854196067706, -1.49985835111166e-05;
 0.1738256088007802, -0.1562864662674188, 0.9722958388199968]
第3幅圖像的平移向量:
[1.726707502757928;
 2.49410066154742;
 -0.5169212442744683]
第4幅圖像的旋轉向量:
[-99.94408740929534;
 -67.11904896100746;
 341.7035262057663]
第4幅圖像的旋轉矩陣:
[-0.4166240767662854, 0.8762113538151707, -0.2422355095852507;
 -0.7194830230098562, -0.4806860756468779, -0.5012834290895748;
 -0.5556694685325433, -0.03456240912595265, 0.8306845861192869]
第4幅圖像的平移向量:
[-2.144507828065959;
 -2.137658756455213;
 0.3861555312888436]
第5幅圖像的旋轉向量:
[63.1817601794685;
 -117.2855578733511;
 327.5340459209377]
第5幅圖像的旋轉矩陣:
[-0.1237680939389874, -0.9830519969136794, -0.1352413778646805;
 0.8454470843144938, -0.03311262698003439, -0.5330316890754268;
 0.5195196690663707, -0.1803117447603135, 0.8352167312468426]
第5幅圖像的平移向量:
[-0.3394208745634724;
 -2.941274925899604;
 0.7239987875443074]
第6幅圖像的旋轉向量:
[176.6380486063267;
 -65.02048705679623;
 345.2669628180993]
第6幅圖像的旋轉矩陣:
[-0.4823787195065527, 0.3144101256594393, 0.8175922234525194;
 -0.5902636261183672, -0.8063068742380883, -0.03818476447485269;
 0.6472245534965549, -0.5010144682933011, 0.5745301383843724]
第6幅圖像的平移向量:
[0.144403698794371;
 -2.686413562533621;
 -0.08279238304814077]
第7幅圖像的旋轉向量:
[23.37912628758978;
 -71.28708027930361;
 401.7783087659996]
第7幅圖像的旋轉矩陣:
[0.950756682549477, -0.3056521783663705, -0.05136610212392408;
 0.3046663933949521, 0.9520979509442887, -0.02622747687825021;
 0.05692204602107398, 0.009286423831555549, 0.9983354361181394]
第7幅圖像的平移向量:
[0.4433620069430767;
 -2.778035766165631;
 0.1565310822654871]
第8幅圖像的旋轉向量:
[84.53413910746443;
 -88.75268154189268;
 326.4489757550855]
第8幅圖像的旋轉矩陣:
[-0.882333219506006, -0.1387045774185431, 0.4497211691251699;
 -0.1080922696912742, -0.870309912144045, -0.4804963247068739;
 0.4580438308602738, -0.4725692510383723, 0.7529104541603049]
第8幅圖像的平移向量:
[0.3026042878663719;
 -2.832559861959414;
 0.5197600078874884]
第9幅圖像的旋轉向量:
[-66.87955552666558;
 -81.79728232518671;
 287.3798612501427]
第9幅圖像的旋轉矩陣:
[-0.06408698919457989, 0.997286705569611, 0.03622270986668297;
 -0.8668814706204128, -0.03765202403427882, -0.4970903750638435;
 -0.4943777641752957, -0.06325782149453277, 0.8669423708118097]
第9幅圖像的平移向量:
[1.918018245182696;
 2.198445482038513;
 0.6398190872020209]
第10幅圖像的旋轉向量:
[51.38889872566385;
 -112.4792732922813;
 348.8614284720838]
第10幅圖像的旋轉矩陣:
[0.8410751829508221, 0.5075468667660225, 0.1870527055678015;
 -0.521221221444936, 0.852916565973049, 0.0293559159998552;
 -0.1446408481020841, -0.1221863720908967, 0.9819111546039054]
第10幅圖像的平移向量:
[0.2388869800501047;
 2.534868757127185;
 0.05816455567725017]
第11幅圖像的旋轉向量:
[55.25157597573984;
 -103.974863603741;
 332.3331998859927]
第11幅圖像的旋轉矩陣:
[0.7603104175748064, -0.6302201082550355, -0.1573235013538499;
 0.6075084686586226, 0.7756458925501082, -0.1711926104661106;
 0.2299163531271294, 0.0345841657577196, 0.9725957053388442]
第11幅圖像的平移向量:
[-0.02801590475009446;
 -3.011578659457537;
 0.5796308944847007]
第12幅圖像的旋轉向量:
[37.20265745451167;
 -92.46700742075161;
 299.3885458741333]
第12幅圖像的旋轉矩陣:
[0.1968247409885918, -0.9604756585987335, -0.1968413843024444;
 0.9041946443200382, 0.2554459280495449, -0.3423148010616344;
 0.3790673640894628, -0.1106069034112951, 0.9187350251296783]
第12幅圖像的平移向量:
[-0.4442257873668548;
 -2.891665626351126;
 -0.7306268697464358]
第13幅圖像的旋轉向量:
[49.15686896201693;
 -109.7597615043953;
 322.2472823512488]
第13幅圖像的旋轉矩陣:
[-0.02527960043733595, 0.888126856668879, 0.4589026348422781;
 -0.9835935284565535, 0.05992383782219021, -0.170155530145356;
 -0.1786189031992861, -0.4556751256368033, 0.8720409779911538]
第13幅圖像的平移向量:
[0.2685697410235677;
 2.70549028727733;
 0.2575020268614151]

下面在附上一份來自於其餘博客的源碼：

/*************************************************************************************
*
*   Description：相機標定，張氏標定法  單目標定，一次只能標定一個相機
                 OPENCV3.0 單目攝像頭標定(使用官方自帶的標定圖片)
                 https://blog.csdn.net/zc850463390zc/article/details/48946855
*   Author     ：JNU
*   Data       ：2018.7.22
*
************************************************************************************/
#include <opencv2/opencv.hpp>
#include <highgui.hpp>
#include "cv.h"
#include <cv.hpp>
#include <iostream>

using namespace std;
using namespace cv;

//程序運行以前須要更改的參數

//使用官方標定圖片集？
//#define   SAMPLE  
#define  MY_DATA

#ifdef SAMPLE

/* 官方數據集  */
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);
const char imageFilePathFormat[] = "sample/right%02d.jpg";          //用於標定的圖片路徑，格式化字符串sample/left%02d.bmp代表圖片路徑爲 sample/left01.bmp - sample/leftxx.bmp

#elif defined  MY_DATA
//本身的數據
const int imageWidth = 1600;                                //攝像頭的分辨率
const int imageHeight = 1200;
const int boardWidth = 9;                                    //橫向的角點數目
const int boardHeight = 6;                                    //縱向的角點數據
const int boardCorner = boardWidth * boardHeight;           //總的角點數據
const int frameNumber = 10;                                         //相機標定時須要採用的圖像幀數
const int squareSize = 30;                                   //標定板黑白格子的大小 單位mm
const Size boardSize = Size(boardWidth, boardHeight);
Size imageSize = Size(imageWidth, imageHeight);
const char imageFilePathFormat[] = "image/right/%d.bmp";

#endif // SAMPLE



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

    /*保存數據*/
    /*輸出數據*/
    FileStorage fs("intrinsics.yml", FileStorage::WRITE);
    if (fs.isOpened())
    {
        fs << "intrinsic" << intrinsic << "distortion_coeff" << distortion_coeff ;
        fs.release();
    }
    else
    {
        cout << "Error: can not save the intrinsics!!!!!" << endl;
    }

    fs.open("extrinsics.yml", FileStorage::WRITE);
    if (fs.isOpened())
    {
        fs << "rvecs" << rvecs << "tvecs" << tvecs;
        fs.release();
    }
    else
    {    
        cout << "Error: can not save the extrinsics parameters\n";
    }

    /*輸出數據*/
    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;
}


void main(char *args)
{
    Mat img;
    int goodFrameCount = 0;
    namedWindow("chessboard");
    cout << "按Q退出 ..." << endl;
    while (goodFrameCount < frameNumber)
    {
        char filename[100];
        //sprintf_s(filename, "image/right/%d.bmp", goodFrameCount + 1);
        sprintf_s(filename, imageFilePathFormat, 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 << goodFrameCount+1 <<" 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");
}