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

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

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

下面我們從相機標定開始說起。相機標定的目的有兩個。

第一，要還原攝像頭成像的物體在真實世界的位置就需要知道世界中的物體到計算機圖像平面是如何變換的，相機標定的目的之一就是為了搞清楚這種變換關系，求解內外參數矩陣。

第二，攝像機的透視投影有個很大的問題——畸變。攝像頭標定的另一個目的就是求解畸變系數，然后用于圖像矯正。

一、三大坐標系

談到相機標定，我們不得不說起攝相機坐標系、世界坐標系、圖像坐標系。

上圖是三個坐標的示意簡圖，通過它大家可以對三個坐標有一個直觀的認識。

世界坐標系(Xw,Yw,Zw)：目標物體位置的參考系。除了無窮遠，世界坐標可以根據運算方便與否自由放置，單位為長度單位如mmmm。。在雙目視覺中世界坐標系主要有三個用途：

標定時確定標定物的位置；

作為雙目視覺的系統參考系，給出兩個攝像機相對世界坐標系的關系，從而求出相機之間的相對關系；

作為重建得到三維坐標的容器，存放重建后的物體的三維坐標。世界坐標系是將看見中物體納入運算的第一站。

攝像機坐標系(Xc,Yc,Zc)：攝像機站在自己角度上衡量的物體的坐標系。攝像機坐標系的原點在攝像機的光心上，zz軸與攝像機光軸平行。它是與拍攝物體發生聯系的橋頭堡，世界坐標系下的物體需先經歷剛體變化轉到攝像機坐標系，然后在和圖像坐標系發生關系。它是圖像坐標與世界坐標之間發生關系的紐帶，溝通了世界上最遠的距離。單位為長度單位如mmmm。

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

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

備注：有很多人把圖像坐標系和像素坐標系合在一起，稱作三大坐標系，也有人分開，稱為四大坐標系。

1、圖像坐標系到像素坐標系

講到這里，你可能會問有了圖像坐標系為什么還要建一個像素坐標系？

我們以圖像左上角為原點建立以像素為單位的直接坐標系uu-vv。像素的橫坐標uu與縱坐標vv分別是在其圖像數組中所在的列數與所在行數。

由于(u,v)(u,v)只代表像素的列數與行數，而像素在圖像中的位置并沒有用物理單位表示出來，所以，我們還要建立以物理單位（如毫米）表示的圖像坐標系xx-yy。將相機光軸與圖像平面的交點（一般位于圖像平面的中心處，也稱為圖像的主點（principal point）定義為該坐標系的原點O1O1，且xx軸與uu軸平行，yy軸與vv軸平行，假設(u0,v0)(u0,v0)代表O1O1在uu-vv坐標系下的坐標，dxdx與dydy分別表示每個像素在橫軸xx和縱軸yy上的物理尺寸，則圖像中的每個像素在uu-vv坐標系中的坐標和在xx-yy坐標系中的坐標之間都存在如下的關系：

其中，我們假設物理坐標系中的單位為毫米，那么dxdx的的單位為:毫米/像素。那么x/dxx/dx的單位就是像素了，即和uu的單位一樣都是像素。為了使用方便，可將上式用齊次坐標與矩陣形式表示為：

為了讓你更直接的理解這一塊內容，我們舉個例子。由于被攝像機攝物體的圖像經過鏡頭投影到CCD芯片上(像平面)，我們設CCD的大小為8×6mm8×6mm,而拍攝到的圖像大小為640×480，

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

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

齊次坐標可以理解為在原有坐標后面加一個“小尾巴”。將普通坐標轉換為齊次坐標，通常就是在增加一個維度，這個維度上的數值為1。如圖像坐標系(u,v)(u,v)轉換為(u,v,1)(u,v,1)一樣。對于無窮遠點，小尾巴為0。注意，給零向量增加小尾巴，數學上無意義。

那么，為什么計算機視覺在坐標運算時要加上這個“小尾巴”呢？

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

使得計算更加規整；

如果用普通坐標來表達的話，會是下面的樣子：

這樣的運算形式會給后與運算帶來一定的麻煩，所以齊次坐標是一個更好的選擇。

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

我們介紹過了像素坐標系之后，我們在此三大坐標系的問題上。我們想知道這三個坐標系有什么樣的關系，我們先從下圖說起：

圖中顯示，世界坐標系通過剛體變換到達攝像機坐標系，然后攝像機坐標系通過透視投影變換到達圖像坐標系。可以看出，世界坐標與圖像坐標的關系建立在剛體變換和透視投影變換的基礎上。

2、世界坐標系到攝像機坐標系

首先，讓我們來看一下剛體變換是如何將世界坐標系與圖像坐標系聯系起來的吧。這里，先對剛體變換做一個介紹：

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

因為世界坐標系和攝像機坐標都是右手坐標系，所以其不會發生形變。我們想把世界坐標系下的坐標轉換到攝像機坐標下的坐標，如下圖所示，可以通過剛體變換的方式。空間中一個坐標系，總可以通過剛體變換轉換到另外一個個坐標系的。

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

其中，RR是3×33×3的正交單位矩陣（即旋轉矩陣），tt為平移向量，RR、tt與攝像機無關，所以稱這兩個參數為攝像機的外參數(extrinsic parameter)，可以理解為兩個坐標原點之間的距離，因其受xx,yy,zz三個方向上的分量共同控制，所以其具有三個自由度。

我們假定在世界坐標系中物點所在平面過世界坐標系原點且與Zw軸垂（也即棋盤平面與Xw-Yw平面重合，目的在于方便后續計算），則Zw=0。

3、攝像機坐標系到圖像坐標系

首先，讓我們來看一下透視投影是如何將攝像機坐標系與圖像坐標系聯系起來的吧。這里，先對透視投影做一個介紹：

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

這里我們還是拿針孔成像來說明（除了成像亮度低外，成像效果和透視投影是一樣的，但是光路更簡單）

下圖是針孔-攝像機的基本模型。平面ππ稱為攝像機的像平面，點Oc稱為攝像機中心（或光心），ff成為攝像機的焦距，Oc為端點且垂直于像平面的射線成為光軸或主軸，主軸與像平面的交點pp是攝像機的主點。

如圖所示，圖像坐標系為oo-xyxy，攝像機坐標系為Oc-xcyczc。記空間點Xc攝像機坐標系中的齊次坐標為:

它的像點mm在圖像坐標系中的齊次坐標記為：

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

我們使用矩陣表示為：

4、總結

我們已經介紹了各個坐標系之間的轉換過程，但是我們想知道的是如何從世界坐標系轉換到像素坐標系，因此我們需要把上面介紹到的聯系起來：

將三者相乘，可以把這三個過程和在一起，寫成一個矩陣：

我們取世界坐標到圖像坐標變換矩陣P如下：

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

其中：

我們設：

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

回到頂部

二、圖片矯正

我們在攝像機坐標系到圖像坐標系變換時談到透視投影。攝像機拍照時通過透鏡把實物投影到像平面上，但是透鏡由于制造精度以及組裝工藝的偏差會引入畸變，導致原始圖像的失真。因此我們需要考慮成像畸變的問題。

透鏡的畸變主要分為徑向畸變和切向畸變，還有薄透鏡畸變等等，但都沒有徑向和切向畸變影響顯著，所以我們在這里只考慮徑向和切向畸變。

徑向畸變

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

像平面中心的畸變為0，沿著鏡頭半徑方向向邊緣移動，畸變越來越嚴重。畸變的數學模型可以用主點（principle point）周圍的泰勒級數展開式的前幾項進行描述，通常使用前兩項，即k1k1和k2k2，對于畸變很大的鏡頭，如魚眼鏡頭，可以增加使用第三項k3k3來進行描述，成像儀上某點根據其在徑向方向上的分布位置，調節公式為：

式里(x0,y0)是畸變點在像平面的原始位置，(x,y)是畸變較正后新的位置，下圖是距離光心不同距離上的點經過透鏡徑向畸變后點位的偏移示意圖，可以看到，距離光心越遠，徑向位移越大，表示畸變也越大，在光心附近，幾乎沒有偏移。

切向畸變

切向畸變是由于透鏡本身與相機傳感器平面（像平面）或圖像平面不平行而產生的，這種情況多是由于透鏡被粘貼到鏡頭模組上的安裝偏差導致。畸變模型可以用兩個額外的參數p1和p2來描述：

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

徑向畸變和切向畸變模型中一共有5個畸變參數，在Opencv中他們被排列成一個5×15×1的矩陣，依次包含k1、k2、p1、p2、k3，經常被定義為Mat矩陣的形式，如Mat distCoeffs=Mat（1,5，CV_32FC1，Scalar::all（0））；這5個參數就是相機標定中需要確定的相機的5個畸變系數。求得這5個參數后，就可以校正由于鏡頭畸變引起的圖像的變形失真，下圖顯示根據鏡頭畸變系數校正后的效果：

三、張氏標定法

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

“張氏標定”是指張正友教授于1998年提出的單平面棋盤格的攝像機標定方法。張氏標定法已經作為工具箱或封裝好的函數被廣泛應用。張氏標定的原文為“A Flexible New Technique forCamera Calibration”。此文中所提到的方法，為相機標定提供了很大便利，并且具有很高的精度。從此標定可以不需要特殊的標定物，只需要一張打印出來的棋盤格。

上文中我們已經得到了像素坐標系和世界坐標系下的坐標映射關系，我們假設標定棋盤位于世界坐標中zw=0平面，則化簡前文中的公式：

其中，uu,v表示像素坐標系中的坐標，fx=f/dx,fy=f/dy,u0,x0,γ(由于制造誤差產生的兩個坐標軸偏斜參數，通常很小，如果按上文中矩陣運算得到的值即為0）表示5個相機內參,RR,tt示相機外參,xw,yw,zw?表示世界坐標系中的坐標。

fx,fy和物理焦距f之間的關系為：fx=fsx和fy=fsy。其中sx=1/dx表示xx方向上的1毫米長度所代表像素值，即像素/單位毫米，fx,fy是在相機標定中整體計算的，而不是通過該公式計算的。

單應性(在計算機視覺中被定義為一個平面到另一個平面的投影映射)矩陣定義為：

那么現在就有：

我們如果求取H的值？

大家可以分析一下，H是一個三3×33×3的矩陣，并且有一個元素是作為齊次坐標。因此，H有8個未知量待解。

(xw,yw)作為標定物的空間坐標，可以由設計者人為控制，是已知量。(u,v)是像素坐標，我們可以直接通過攝像機獲得。對于一組對應(xw,yw)-(u,v)我們可以獲得兩組方程。

現在有8個未知量需要求解，所以我們至少需要八個方程。所以需要四個對應點。四點即可算出，圖像平面到世界平面的單應性矩陣HH。

這也是張氏標定采用四個角點的棋盤格作為標定物的一個原因。張氏標定就是利用一張打印的棋盤格，然后對每個角點進行標記其在像素坐標系的像素點坐標，以及在世界坐標系的坐標，張氏標定證明通過4組以上的點就可以求解出H矩陣的值，但是為了減少誤差，具有更強的魯棒性，我們一般會拍攝許多張照片，選取大量的角點進行標定。具體過程如下：

打印一張棋盤格標定圖紙，將其貼在平面物體的表面.

拍攝一組不同方向棋盤格的圖片，可以通過移動相機來實現，也可以移動標定圖片來實現。

對于每張拍攝的棋盤圖片，檢測圖片中所有棋盤格的特征點（角點，也就是下圖中黑白棋盤交叉點，中間品紅色的圓圈內就是一個角點）。我們定義打印的棋盤圖紙位于世界坐標系zw=0的平面上，世界坐標系的原點位于棋盤圖紙的固定一角（比如下圖中黃色點）。像素坐標系原點位于圖片左上角。

因為棋盤標定圖紙中所有角點的空間坐標是已知的，這些角點對應在拍攝的標定圖片中的角點的像素坐標也是已知的，如果我們得到這樣的N>=4N>=4個匹配點對（越多計算結果越魯棒），就可以根據LM等優化方法得到單應性矩陣H。

張正友教授從數學推導上證明了張氏標定算法的可行性。但在實際標定過程中，一般使用最大似然估計進行優化。假設我們拍攝了nn張標定圖片，每張圖片里有mm個棋盤格角點。三維空間點Xj(xw,yw,zw)經過相機內參M，外參=R，t變換后得到的二維像素為x′(u,v)，假設噪聲是獨立同分布的，我們通過最小化xij(實際值：棋盤格角點在像素坐標系下的實際值),?x′x′(估計值)的位置來求解上述最大似然估計問題：

現在我們來考慮透鏡畸變的影響，由于徑向畸變的影響相對較明顯，所以主要考慮徑向畸變參數，根據經驗，通常只考慮徑向畸變的前兩個參數k1,k2就可以（增加更多的參數會使得模型變的復雜且不穩定）。實際求解中，通常把k1,k2也作為參數加入上述函數一起進行優化，待優化函數如下所示：

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

這里我沒有過多的介紹張氏標定法的數學推導過程，感興趣的童鞋可以看一下博客最后給出來的鏈接。

假設我們已經通過LM求得了相機的內參MM和外參RR,tt。那如果獲取到圖像中二維坐標，如何獲取到現實中三維坐標呢？當然單目攝像機無法獲取三維坐標，但能反映一定關系。綜上來講，矩陣變換公式為：

根據坐標系之間轉化和相機模型，研究了極幾何及基本矩陣的知識，證明了基本矩陣是求得攝像機投影矩陣的關鍵，根據基礎矩陣F和攝像機標定階段獲取的內參數矩陣MM，計算得到本征矩陣EE，通過對EE進行奇異值分解求得外參數矩陣RR，tt，接著求出投影矩陣P1和P2、重投影矩陣Q。OpenCV提供了兩個相關的函數stereoCalibrate()和stereoRectify()。

回到頂部

四、使用opencv實現單目標定

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

相機標定的輸入：標定圖像上所有內角點的圖像坐標，標定板圖像上所有內角點的空間三維坐標（一般情況下假定圖像位于Z=0平面上）。

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

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

1. 準備標定圖片

2. 對每一張標定圖片，提取角點信息

3.對每一張標定圖片，進一步提取亞像素角點信息

4. 在棋盤標定圖上繪制找到的內角點（非必須，僅為了顯示）

5. 相機標定

6. 對標定結果進行評價

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

準備標定圖片

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

這里我們使用OpenCV提供的sample程序中的標定圖片，圖片位于opencv(C++版本)的安裝路徑：opencvsourcessamplesdata下：

我們先創建一個C++控制臺項目，并把標定圖片按如下格式存放：

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

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

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

/*************************************************************************************
*
*   Description：相機標定，張氏標定法  單目標定
*   Author     ：JNU
*   Data       ：2018.7.22
*
************************************************************************************/
#include 
#include 
#include 
#include 
#include 
#include 
#include 


using namespace cv;
using namespace std;


void main(char *args)
{
    //保存文件名稱
    std::vector  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  imagePointsBuf;
    //保存檢測到的所有角點
    std::vector> 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 << "
第 " << num+1 << "張圖片的數據 -->: " << std::endl;
            }
            //輸出所有的角點
            std::cout< pointCounts;
        //保存標定板上角點的三維坐標
        std::vector> 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 tvecsMat;
        //每幅圖片的平移向量
        std::vector rvecsMat;


        //初始化標定板上角點的三維坐標
        int i, j, t;
        for (t = 0; t < imageCount; t++)
        {
            std::vector 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 imagePoints2;
        std::cout << "每幅圖像的標定誤差:" << std::endl;
        fout << "每幅圖像的標定誤差:" << std::endl;
        for (i = 0; i < imageCount; i++)
        {
            std::vector tempPointSet = objectPoints[i];
            //通過得到的攝像機內外參數，對空間的三維點進行重新投影計算，得到新的投影點imagePoints2（在像素坐標系下的點坐標）
            cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, imagePoints2);
            //計算新的投影點和舊的投影點之間的誤差
            std::vector 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(0, j) = cv::Vec2f(imagePoints2[j].x, imagePoints2[j].y);
                tempImagePointMat.at(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::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個元素。如果這個向量是空的，就認為是零畸變系數。

RR：可選的立體修正變換矩陣，是個3×33×3的矩陣。

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

在雙目相機例子中，newCameraMatrix一般是用cv::stereoRectify()計算而來的，設置為R1R1或R2R2(左右相機平面行對準的校正旋轉矩陣)。此外，根據RR，新的相機在坐標空間中的取向是不同的。例如，它幫助配準雙目相機的兩個相機方向，從而使得兩個圖像的極線是水平的，且yy坐標相同（在雙目相機的兩個相機誰水平放置的情況下）。

newCameraMatrix：新的相機內參矩陣

在單目相機例子中，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()黑色

?

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

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


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 rvecs;                                        //旋轉向量
vector tvecs;                                        //平移向量
vector> corners;                        //各個圖像找到的角點的集合 和objRealPoint 一一對應
vector> objRealPoint;                    //各副圖像的角點的實際物理坐標集合

vector corner;                                    //某一副圖像找到的角點

Mat rgbImage, grayImage;

/*計算標定板上模塊的實際物理坐標*/
void calRealPoint(vector>& obj, int boardwidth, int boardheight, int imgNumber, int squaresize)
{
    //    Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));
    vector imgpoint;
    for (int rowIndex = 0; rowIndex < boardheight; rowIndex++)
    {
        for (int colIndex = 0; colIndex < boardwidth; colIndex++)
        {
            //    imgpoint.at(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(0, 0) = 256.8093262;   //fx        
    intrinsic.at(0, 2) = 160.2826538;   //cx
    intrinsic.at(1, 1) = 254.7511139;   //fy
    intrinsic.at(1, 2) = 127.6264572;   //cy


    intrinsic.at(0, 1) = 0;
    intrinsic.at(1, 0) = 0;
    intrinsic.at(2, 0) = 0;
    intrinsic.at(2, 1) = 0;
    intrinsic.at(2, 2) = 1;


    /*
    k1 k2 p1 p2 p3
    */
    distortion_coeff.at(0, 0) = -0.193740;  //k1
    distortion_coeff.at(1, 0) = -0.378588;  //k2
    distortion_coeff.at(2, 0) = 0.028980;   //p1
    distortion_coeff.at(3, 0) = 0.008136;   //p2
    distortion_coeff.at(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
";
    }


    /*輸出數據*/
    cout << "fx :" << intrinsic.at(0, 0) << endl << "fy :" << intrinsic.at(1, 1) << endl;
    cout << "cx :" << intrinsic.at(0, 2) << endl << "cy :" << intrinsic.at(1, 2) << endl;


    cout << "k1 :" << distortion_coeff.at(0, 0) << endl;
    cout << "k2 :" << distortion_coeff.at(1, 0) << endl;
    cout << "p1 :" << distortion_coeff.at(2, 0) << endl;
    cout << "p2 :" << distortion_coeff.at(3, 0) << endl;
    cout << "p3 :" << distortion_coeff.at(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");
}

?

?

?