視覺(jué)識(shí)別項(xiàng)目：雙目攝像頭進(jìn)行空間定位技術(shù)詳解

去年三四月份實(shí)驗(yàn)室做了一個(gè)機(jī)器人與視覺(jué)識(shí)別系統(tǒng)的項(xiàng)目，主要就是利用雙目攝像頭進(jìn)行物體空間坐標(biāo)定位，然后利用機(jī)器人進(jìn)行抓取物體。當(dāng)時(shí)我才研一，還是個(gè)菜雞，項(xiàng)目主要是幾個(gè)學(xué)長(zhǎng)負(fù)責(zé)做的，我也就是參與打打醬油，混混經(jīng)驗(yàn)?，F(xiàn)在過(guò)了一年多了，機(jī)器人一直在實(shí)驗(yàn)室放著，空著也是浪費(fèi)，所以就想搞點(diǎn)事情。這里我們就先從利用雙目攝像頭進(jìn)行空間定位說(shuō)起，因此這是整個(gè)項(xiàng)目的核心部分。

雙目視覺(jué)是建立在幾何數(shù)學(xué)的基礎(chǔ)上，數(shù)學(xué)推導(dǎo)是枯燥乏味的。因此這里不去過(guò)多的介紹數(shù)學(xué)原理，只是簡(jiǎn)要的敘述一下雙目視覺(jué)的流程。

雙目視覺(jué)主要包括相機(jī)標(biāo)定、圖片畸變矯正、攝像機(jī)校正、圖片匹配、3D恢復(fù)五個(gè)部分。

下面我們從相機(jī)標(biāo)定開始說(shuō)起。相機(jī)標(biāo)定的目的有兩個(gè)。

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

第二，攝像機(jī)的透視投影有個(gè)很大的問(wèn)題——畸變。攝像頭標(biāo)定的另一個(gè)目的就是求解畸變系數(shù)，然后用于圖像矯正。

一、三大坐標(biāo)系

談到相機(jī)標(biāo)定，我們不得不說(shuō)起攝相機(jī)坐標(biāo)系、世界坐標(biāo)系、圖像坐標(biāo)系。

上圖是三個(gè)坐標(biāo)的示意簡(jiǎn)圖，通過(guò)它大家可以對(duì)三個(gè)坐標(biāo)有一個(gè)直觀的認(rèn)識(shí)。

世界坐標(biāo)系(Xw,Yw,Zw)：目標(biāo)物體位置的參考系。除了無(wú)窮遠(yuǎn)，世界坐標(biāo)可以根據(jù)運(yùn)算方便與否自由放置，單位為長(zhǎng)度單位如mmmm。。在雙目視覺(jué)中世界坐標(biāo)系主要有三個(gè)用途：

標(biāo)定時(shí)確定標(biāo)定物的位置；

作為雙目視覺(jué)的系統(tǒng)參考系，給出兩個(gè)攝像機(jī)相對(duì)世界坐標(biāo)系的關(guān)系，從而求出相機(jī)之間的相對(duì)關(guān)系；

作為重建得到三維坐標(biāo)的容器，存放重建后的物體的三維坐標(biāo)。世界坐標(biāo)系是將看見(jiàn)中物體納入運(yùn)算的第一站。

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

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

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

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

1、圖像坐標(biāo)系到像素坐標(biāo)系

講到這里，你可能會(huì)問(wèn)有了圖像坐標(biāo)系為什么還要建一個(gè)像素坐標(biāo)系？

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

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

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

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

上面的矩陣公式運(yùn)用了齊次坐標(biāo)，初學(xué)者可能會(huì)感到有些迷惑。大家會(huì)問(wèn)：怎樣將普通坐標(biāo)轉(zhuǎn)換為齊次坐標(biāo)呢？齊次坐標(biāo)能帶來(lái)什么好處呢？

這里對(duì)齊次坐標(biāo)做一個(gè)通俗的解釋。此處只講怎么將普通坐標(biāo)改寫為齊次坐標(biāo)及為什么引入齊次坐標(biāo)。這里只做一個(gè)通俗但不太嚴(yán)謹(jǐn)?shù)谋硎?。力求?jiǎn)單明了。針對(duì)齊次坐標(biāo)的嚴(yán)謹(jǐn)?shù)募償?shù)學(xué)推導(dǎo)，可參見(jiàn)“周興和版的《高等幾何》---1.3拓廣平面上的齊次坐標(biāo)”。玉米曾詳細(xì)讀過(guò)《高等幾何》這本書，但覺(jué)得離計(jì)算機(jī)視覺(jué)有點(diǎn)遠(yuǎn)，是講純數(shù)學(xué)的投影關(guān)系的，較為生澀難懂。

齊次坐標(biāo)可以理解為在原有坐標(biāo)后面加一個(gè)“小尾巴”。將普通坐標(biāo)轉(zhuǎn)換為齊次坐標(biāo)，通常就是在增加一個(gè)維度，這個(gè)維度上的數(shù)值為1。如圖像坐標(biāo)系(u,v)(u,v)轉(zhuǎn)換為(u,v,1)(u,v,1)一樣。對(duì)于無(wú)窮遠(yuǎn)點(diǎn)，小尾巴為0。注意，給零向量增加小尾巴，數(shù)學(xué)上無(wú)意義。

那么，為什么計(jì)算機(jī)視覺(jué)在坐標(biāo)運(yùn)算時(shí)要加上這個(gè)“小尾巴”呢？

將投影平面擴(kuò)展到無(wú)窮遠(yuǎn)點(diǎn)。如對(duì)消隱點(diǎn)(vanishing point)的描述；

使得計(jì)算更加規(guī)整；

如果用普通坐標(biāo)來(lái)表達(dá)的話，會(huì)是下面的樣子：

這樣的運(yùn)算形式會(huì)給后與運(yùn)算帶來(lái)一定的麻煩，所以齊次坐標(biāo)是一個(gè)更好的選擇。

齊次坐標(biāo)還有一個(gè)重要的性質(zhì)，伸縮不變性。即：設(shè)齊次坐標(biāo)MM，則αM=MαM=M。

我們介紹過(guò)了像素坐標(biāo)系之后，我們?cè)诖巳笞鴺?biāo)系的問(wèn)題上。我們想知道這三個(gè)坐標(biāo)系有什么樣的關(guān)系，我們先從下圖說(shuō)起：

圖中顯示，世界坐標(biāo)系通過(guò)剛體變換到達(dá)攝像機(jī)坐標(biāo)系，然后攝像機(jī)坐標(biāo)系通過(guò)透視投影變換到達(dá)圖像坐標(biāo)系?？梢钥闯觯澜缱鴺?biāo)與圖像坐標(biāo)的關(guān)系建立在剛體變換和透視投影變換的基礎(chǔ)上。

2、世界坐標(biāo)系到攝像機(jī)坐標(biāo)系

首先，讓我們來(lái)看一下剛體變換是如何將世界坐標(biāo)系與圖像坐標(biāo)系聯(lián)系起來(lái)的吧。這里，先對(duì)剛體變換做一個(gè)介紹：

剛體變換(regidbody motion):三維空間中， 當(dāng)物體不發(fā)生形變時(shí)，對(duì)一個(gè)幾何物體作旋轉(zhuǎn)，?平移的運(yùn)動(dòng)，稱之為剛體變換。

因?yàn)槭澜缱鴺?biāo)系和攝像機(jī)坐標(biāo)都是右手坐標(biāo)系，所以其不會(huì)發(fā)生形變。我們想把世界坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到攝像機(jī)坐標(biāo)下的坐標(biāo)，如下圖所示，可以通過(guò)剛體變換的方式?？臻g中一個(gè)坐標(biāo)系，總可以通過(guò)剛體變換轉(zhuǎn)換到另外一個(gè)個(gè)坐標(biāo)系的。

下面看一下，二者之間剛體變換的數(shù)學(xué)表達(dá)：

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

我們假定在世界坐標(biāo)系中物點(diǎn)所在平面過(guò)世界坐標(biāo)系原點(diǎn)且與Zw軸垂（也即棋盤平面與Xw-Yw平面重合，目的在于方便后續(xù)計(jì)算），則Zw=0。

3、攝像機(jī)坐標(biāo)系到圖像坐標(biāo)系

首先，讓我們來(lái)看一下透視投影是如何將攝像機(jī)坐標(biāo)系與圖像坐標(biāo)系聯(lián)系起來(lái)的吧。這里，先對(duì)透視投影做一個(gè)介紹：

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

這里我們還是拿針孔成像來(lái)說(shuō)明（除了成像亮度低外，成像效果和透視投影是一樣的，但是光路更簡(jiǎn)單）

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

如圖所示，圖像坐標(biāo)系為oo-xyxy，攝像機(jī)坐標(biāo)系為Oc-xcyczc。記空間點(diǎn)Xc攝像機(jī)坐標(biāo)系中的齊次坐標(biāo)為:

它的像點(diǎn)mm在圖像坐標(biāo)系中的齊次坐標(biāo)記為：

根據(jù)三角形相似原理，可得：

我們使用矩陣表示為：

4、總結(jié)

我們已經(jīng)介紹了各個(gè)坐標(biāo)系之間的轉(zhuǎn)換過(guò)程，但是我們想知道的是如何從世界坐標(biāo)系轉(zhuǎn)換到像素坐標(biāo)系，因此我們需要把上面介紹到的聯(lián)系起來(lái)：

將三者相乘，可以把這三個(gè)過(guò)程和在一起，寫成一個(gè)矩陣：

我們?nèi)∈澜缱鴺?biāo)到圖像坐標(biāo)變換矩陣P如下：

P就表示了一個(gè)投影相機(jī)，有下面公式：

其中：

我們?cè)O(shè)：

最后用一幅圖來(lái)總結(jié)從世界坐標(biāo)系到像素坐標(biāo)系（不考慮畸變）的轉(zhuǎn)換關(guān)系：

回到頂部

二、圖片矯正

我們?cè)跀z像機(jī)坐標(biāo)系到圖像坐標(biāo)系變換時(shí)談到透視投影。攝像機(jī)拍照時(shí)通過(guò)透鏡把實(shí)物投影到像平面上，但是透鏡由于制造精度以及組裝工藝的偏差會(huì)引入畸變，導(dǎo)致原始圖像的失真。因此我們需要考慮成像畸變的問(wèn)題。

透鏡的畸變主要分為徑向畸變和切向畸變，還有薄透鏡畸變等等，但都沒(méi)有徑向和切向畸變影響顯著，所以我們?cè)谶@里只考慮徑向和切向畸變。

徑向畸變

顧名思義，徑向畸變就是沿著透鏡半徑方向分布的畸變，產(chǎn)生原因是光線在原理透鏡中心的地方比靠近中心的地方更加彎曲，這種畸變?cè)谄胀畠r(jià)的鏡頭中表現(xiàn)更加明顯，徑向畸變主要包括桶形畸變和枕形畸變兩種。以下分別是枕形和桶形畸變示意圖：

像平面中心的畸變?yōu)?，沿著鏡頭半徑方向向邊緣移動(dòng)，畸變?cè)絹?lái)越嚴(yán)重?；兊臄?shù)學(xué)模型可以用主點(diǎn)（principle point）周圍的泰勒級(jí)數(shù)展開式的前幾項(xiàng)進(jìn)行描述，通常使用前兩項(xiàng)，即k1k1和k2k2，對(duì)于畸變很大的鏡頭，如魚眼鏡頭，可以增加使用第三項(xiàng)k3k3來(lái)進(jìn)行描述，成像儀上某點(diǎn)根據(jù)其在徑向方向上的分布位置，調(diào)節(jié)公式為：

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

切向畸變

切向畸變是由于透鏡本身與相機(jī)傳感器平面（像平面）或圖像平面不平行而產(chǎn)生的，這種情況多是由于透鏡被粘貼到鏡頭模組上的安裝偏差導(dǎo)致?；兡Ｐ涂梢杂脙蓚€(gè)額外的參數(shù)p1和p2來(lái)描述：

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

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

三、張氏標(biāo)定法

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

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

上文中我們已經(jīng)得到了像素坐標(biāo)系和世界坐標(biāo)系下的坐標(biāo)映射關(guān)系，我們假設(shè)標(biāo)定棋盤位于世界坐標(biāo)中zw=0平面，則化簡(jiǎn)前文中的公式：

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

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

單應(yīng)性(在計(jì)算機(jī)視覺(jué)中被定義為一個(gè)平面到另一個(gè)平面的投影映射)矩陣定義為：

那么現(xiàn)在就有：

我們?nèi)绻笕的值？

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

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

現(xiàn)在有8個(gè)未知量需要求解，所以我們至少需要八個(gè)方程。所以需要四個(gè)對(duì)應(yīng)點(diǎn)。四點(diǎn)即可算出，圖像平面到世界平面的單應(yīng)性矩陣HH。

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

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

拍攝一組不同方向棋盤格的圖片，可以通過(guò)移動(dòng)相機(jī)來(lái)實(shí)現(xiàn)，也可以移動(dòng)標(biāo)定圖片來(lái)實(shí)現(xiàn)。

對(duì)于每張拍攝的棋盤圖片，檢測(cè)圖片中所有棋盤格的特征點(diǎn)（角點(diǎn)，也就是下圖中黑白棋盤交叉點(diǎn)，中間品紅色的圓圈內(nèi)就是一個(gè)角點(diǎn)）。我們定義打印的棋盤圖紙位于世界坐標(biāo)系z(mì)w=0的平面上，世界坐標(biāo)系的原點(diǎn)位于棋盤圖紙的固定一角（比如下圖中黃色點(diǎn)）。像素坐標(biāo)系原點(diǎn)位于圖片左上角。

因?yàn)槠灞P標(biāo)定圖紙中所有角點(diǎn)的空間坐標(biāo)是已知的，這些角點(diǎn)對(duì)應(yīng)在拍攝的標(biāo)定圖片中的角點(diǎn)的像素坐標(biāo)也是已知的，如果我們得到這樣的N>=4N>=4個(gè)匹配點(diǎn)對(duì)（越多計(jì)算結(jié)果越魯棒），就可以根據(jù)LM等優(yōu)化方法得到單應(yīng)性矩陣H。

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

現(xiàn)在我們來(lái)考慮透鏡畸變的影響，由于徑向畸變的影響相對(duì)較明顯，所以主要考慮徑向畸變參數(shù)，根據(jù)經(jīng)驗(yàn)，通常只考慮徑向畸變的前兩個(gè)參數(shù)k1,k2就可以（增加更多的參數(shù)會(huì)使得模型變的復(fù)雜且不穩(wěn)定）。實(shí)際求解中，通常把k1,k2也作為參數(shù)加入上述函數(shù)一起進(jìn)行優(yōu)化，待優(yōu)化函數(shù)如下所示：

極大似然估計(jì)是一種估計(jì)總體未知參數(shù)的方法。它主要用于點(diǎn)估計(jì)問(wèn)題。所謂點(diǎn)估計(jì)是指用一個(gè)估計(jì)量的觀測(cè)值來(lái)估計(jì)未知參數(shù)的真值。說(shuō)穿了就一句話：就是在參數(shù)空間中選取使得樣本取得觀測(cè)值的概率最大的參數(shù)。

這里我沒(méi)有過(guò)多的介紹張氏標(biāo)定法的數(shù)學(xué)推導(dǎo)過(guò)程，感興趣的童鞋可以看一下博客最后給出來(lái)的鏈接。

假設(shè)我們已經(jīng)通過(guò)LM求得了相機(jī)的內(nèi)參MM和外參RR,tt。那如果獲取到圖像中二維坐標(biāo)，如何獲取到現(xiàn)實(shí)中三維坐標(biāo)呢？當(dāng)然單目攝像機(jī)無(wú)法獲取三維坐標(biāo)，但能反映一定關(guān)系。綜上來(lái)講，矩陣變換公式為：

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

回到頂部

四、使用opencv實(shí)現(xiàn)單目標(biāo)定

相機(jī)標(biāo)定的目的：獲取攝像機(jī)的內(nèi)參和外參矩陣（同時(shí)也會(huì)得到每一幅標(biāo)定圖像的選擇和平移矩陣），內(nèi)參和外參系數(shù)可以對(duì)之后相機(jī)拍攝的圖像就進(jìn)行矯正，得到畸變相對(duì)很小的圖像。

相機(jī)標(biāo)定的輸入：標(biāo)定圖像上所有內(nèi)角點(diǎn)的圖像坐標(biāo)，標(biāo)定板圖像上所有內(nèi)角點(diǎn)的空間三維坐標(biāo)（一般情況下假定圖像位于Z=0平面上）。

相機(jī)標(biāo)定的輸出：攝像機(jī)的內(nèi)參、外參系數(shù)。

這三個(gè)基礎(chǔ)的問(wèn)題就決定了使用Opencv實(shí)現(xiàn)張正友法標(biāo)定相機(jī)的標(biāo)定流程、標(biāo)定結(jié)果評(píng)價(jià)以及使用標(biāo)定結(jié)果矯正原始圖像的完整流程：

1. 準(zhǔn)備標(biāo)定圖片

2. 對(duì)每一張標(biāo)定圖片，提取角點(diǎn)信息

3.對(duì)每一張標(biāo)定圖片，進(jìn)一步提取亞像素角點(diǎn)信息

4. 在棋盤標(biāo)定圖上繪制找到的內(nèi)角點(diǎn)（非必須，僅為了顯示）

5. 相機(jī)標(biāo)定

6. 對(duì)標(biāo)定結(jié)果進(jìn)行評(píng)價(jià)

7. 查看標(biāo)定效果——利用標(biāo)定結(jié)果對(duì)棋盤圖進(jìn)行矯正

準(zhǔn)備標(biāo)定圖片

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

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

我們先創(chuàng)建一個(gè)C++控制臺(tái)項(xiàng)目，并把標(biāo)定圖片按如下格式存放：

sample文件夾下有兩個(gè)文件夾left和right，分別對(duì)應(yīng)左攝像頭和右攝像頭拍攝到的標(biāo)定板圖片。

filename.txt存放標(biāo)定圖片的路徑，內(nèi)容如下：

關(guān)于OpenCV提供的用于相機(jī)標(biāo)定的API函數(shù)可以查看博客雙目視覺(jué)標(biāo)定程序講解，單目標(biāo)定的代碼如下：

/*************************************************************************************
*
*   Description：相機(jī)標(biāo)定，張氏標(biāo)定法  單目標(biāo)定
*   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;


    //需要更改的參數(shù)
    //左相機(jī)標(biāo)定，指定左相機(jī)圖片路徑，以及標(biāo)定結(jié)果保存文件
    string infilename = "sample/left/filename.txt";        //如果是右相機(jī)把left改為right
    string outfilename = "sample/left/caliberation_result.txt";


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


    /*
    1.讀取毎一幅圖像，從中提取出角點(diǎn)，然后對(duì)角點(diǎn)進(jìn)行亞像素精確化、獲取每個(gè)角點(diǎn)在像素坐標(biāo)系中的坐標(biāo)
    像素坐標(biāo)系的原點(diǎn)位于圖像的左上角
    */
    std::cout << "開始提取角點(diǎn)......" << std::endl;;
    //圖像數(shù)量
    int imageCount = 0;
    //圖像尺寸
    cv::Size imageSize;
    //標(biāo)定板上每行每列的角點(diǎn)數(shù)
    cv::Size boardSize = cv::Size(9, 6);
    //緩存每幅圖像上檢測(cè)到的角點(diǎn)
    std::vector  imagePointsBuf;
    //保存檢測(cè)到的所有角點(diǎn)
    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);
            //讀入第一張圖片時(shí)獲取圖寬高信息
            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++;


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


        //角點(diǎn)總數(shù)
        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是每幅圖片的角點(diǎn)個(gè)數(shù)，此判斷語(yǔ)句是為了輸出，便于調(diào)試
            if (p == 0)
            {                                        
                std::cout << "
第 " << num+1 << "張圖片的數(shù)據(jù) -->: " << std::endl;
            }
            //輸出所有的角點(diǎn)
            std::cout< pointCounts;
        //保存標(biāo)定板上角點(diǎn)的三維坐標(biāo)
        std::vector> objectPoints;
        //攝像機(jī)內(nèi)參數(shù)矩陣 M=[fx γ u0,0 fy v0,0 0 1]
        cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F, Scalar::all(0));
        //攝像機(jī)的5個(gè)畸變系數(shù)k1,k2,p1,p2,k3
        cv::Mat distCoeffs = cv::Mat(1, 5, CV_64F, Scalar::all(0));
        //每幅圖片的旋轉(zhuǎn)向量
        std::vector tvecsMat;
        //每幅圖片的平移向量
        std::vector rvecsMat;


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


        //初始化每幅圖像中的角點(diǎn)數(shù)量，假定每幅圖像中都可以看到完整的標(biāo)定板
        for (i = 0; i < imageCount; i++)
        {
            pointCounts.push_back(boardSize.width*boardSize.height);
        }
        //開始標(biāo)定
        cv::calibrateCamera(objectPoints, imagePointsSeq, imageSize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);
        std::cout << "標(biāo)定完成" << std::endl;
        //對(duì)標(biāo)定結(jié)果進(jìn)行評(píng)價(jià)
        std::cout << "開始評(píng)價(jià)標(biāo)定結(jié)果......" << std::endl;
        //所有圖像的平均誤差的總和
        double totalErr = 0.0;
        //每幅圖像的平均誤差
        double err = 0.0;
        //保存重新計(jì)算得到的投影點(diǎn)
        std::vector imagePoints2;
        std::cout << "每幅圖像的標(biāo)定誤差:" << std::endl;
        fout << "每幅圖像的標(biāo)定誤差:" << std::endl;
        for (i = 0; i < imageCount; i++)
        {
            std::vector tempPointSet = objectPoints[i];
            //通過(guò)得到的攝像機(jī)內(nèi)外參數(shù)，對(duì)空間的三維點(diǎn)進(jìn)行重新投影計(jì)算，得到新的投影點(diǎn)imagePoints2（在像素坐標(biāo)系下的點(diǎn)坐標(biāo)）
            cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, imagePoints2);
            //計(jì)算新的投影點(diǎn)和舊的投影點(diǎn)之間的誤差
            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 << "評(píng)價(jià)完成!" << std::endl;
        //保存標(biāo)定結(jié)果
        std::cout << "開始保存標(biāo)定結(jié)果....." << std::endl;
        //保存每張圖像的旋轉(zhuǎn)矩陣
        cv::Mat rotationMatrix = cv::Mat(3, 3, CV_32FC1, Scalar::all(0));
        fout << "相機(jī)內(nèi)參數(shù)矩陣:" << std::endl;
        fout << cameraMatrix << std::endl << std::endl;
        fout << "畸變系數(shù):" << std::endl;
        fout << distCoeffs << std::endl << std::endl;


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


        /************************************************************************
        顯示定標(biāo)結(jié)果
        *************************************************************************/
        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;
            //計(jì)算圖片畸變矯正的映射矩陣mapx、mapy(不進(jìn)行立體校正、立體校正需要使用雙攝)
            initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
            //讀取一張圖片
            Mat imageSource = imread(filenames[i]);
            Mat newimage = imageSource.clone();
            //另一種不需要轉(zhuǎn)換矩陣的方式
            //undistort(imageSource,newimage,cameraMatrix,distCoeffs);
            //進(jìn)行校正
            remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
            imshow("原始圖像", imageSource);
            imshow("矯正后圖像", newimage);
            waitKey();
        }


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

上面有兩個(gè)函數(shù)需要單獨(dú)介紹一下：

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

函數(shù)功能：該函數(shù)功能是計(jì)算畸變矯正和攝像機(jī)立體校正的映射變換矩陣。為了重映射，將結(jié)果以映射的形式表達(dá)。無(wú)畸變的圖像看起來(lái)和原始的圖像一樣，就像這個(gè)圖像是用內(nèi)參為newCameraMatrix的且無(wú)畸變的相機(jī)采集得到的。該函數(shù)實(shí)際上為反向映射算法構(gòu)建映射，供反向映射使用。也就是，對(duì)于已經(jīng)修正畸變的圖像中的每個(gè)像素$(u,v)$，該函數(shù)計(jì)算原來(lái)圖像（從相機(jī)中獲得的原始圖像）中對(duì)應(yīng)的坐標(biāo)系。

參數(shù)說(shuō)明：

cameraMatrix：輸入相機(jī)內(nèi)參矩陣

distCoeffs：輸入?yún)?shù)，相機(jī)的畸變系數(shù)

有4，5,8,12或14個(gè)元素。如果這個(gè)向量是空的，就認(rèn)為是零畸變系數(shù)。

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

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

在雙目相機(jī)例子中，newCameraMatrix一般是用cv::stereoRectify()計(jì)算而來(lái)的，設(shè)置為R1R1或R2R2(左右相機(jī)平面行對(duì)準(zhǔn)的校正旋轉(zhuǎn)矩陣)。此外，根據(jù)RR，新的相機(jī)在坐標(biāo)空間中的取向是不同的。例如，它幫助配準(zhǔn)雙目相機(jī)的兩個(gè)相機(jī)方向，從而使得兩個(gè)圖像的極線是水平的，且yy坐標(biāo)相同（在雙目相機(jī)的兩個(gè)相機(jī)誰(shuí)水平放置的情況下）。

newCameraMatrix：新的相機(jī)內(nèi)參矩陣

在單目相機(jī)例子中，newCameraMatrix一般和cameraMatrix相等，或者可以用cv::getOptimalNewCameraMatrix()來(lái)計(jì)算，獲得一個(gè)更好的有尺度的控制結(jié)果。

在雙目相機(jī)例子中，newCameraMatrix一般是用cv::stereoRectify()計(jì)算而來(lái)的，設(shè)置為P1或P2(左右相機(jī)把空間3D點(diǎn)的坐標(biāo)轉(zhuǎn)換到圖像的2D點(diǎn)的坐標(biāo)的投影矩陣)。

size：未畸變的圖像尺寸。

m1type：第一個(gè)輸出的映射的類型，可以為 CV_32FC1, CV_32FC2或CV_16SC2，參見(jiàn)cv::convertMaps。

map1：第一個(gè)輸出映射。

map2：第二個(gè)輸出映射。

void remap(InputArray src, OutputArray dst, InputArray map1, InputArray?

　　　　　　map2, int interpolation,int borderMode=BORDER_CONSTANT, const Scalar& 
　　　　　　borderValue=Scalar())

函數(shù)功能：重映射：就是把一幅圖像中某位置的像素放置到另一個(gè)圖片指定位置的過(guò)程。

參數(shù)說(shuō)明：

src：輸入圖像，即原圖像，需要單通道8位或者浮點(diǎn)類型的圖像

dst：輸出圖像，即目標(biāo)圖像，需和原圖形一樣的尺寸和類型

map1：它有兩種可能表示的對(duì)象：（1）表示點(diǎn)（x,y）的第一個(gè)映射；（2）表示CV_16SC2，CV_32FC1等

map2：它有兩種可能表示的對(duì)象：（1）若map1表示點(diǎn)（x,y）時(shí)，這個(gè)參數(shù)不代表任何值；（2）表示CV_16UC1，CV_32FC1類型的Y值

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

（1）INTER_NEAREST——最近鄰插值

（2）INTER_LINEAR——雙線性插值（默認(rèn)）

（3）INTER_CUBIC——雙三樣條插值（默認(rèn)）

（4）INTER_LANCZOS4——lanczos插值（默認(rèn)）

borderMode：邊界模式，默認(rèn)BORDER_CONSTANT

borderValue：邊界顏色，默認(rèn)Scalar()黑色

?

下面在附上一份來(lái)自于其他博客的源碼：

/*************************************************************************************
*
*   Description：相機(jī)標(biāo)定，張氏標(biāo)定法  單目標(biāo)定，一次只能標(biāo)定一個(gè)相機(jī)
                 OPENCV3.0 單目攝像頭標(biāo)定(使用官方自帶的標(biāo)定圖片)
                 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;


//程序運(yùn)行之前需要更改的參數(shù)


//使用官方標(biāo)定圖片集？
//#define   SAMPLE  
#define  MY_DATA


#ifdef SAMPLE


/* 官方數(shù)據(jù)集  */
const int imageWidth = 640;                                //攝像頭的分辨率
const int imageHeight = 480;
const int boardWidth = 9;                                //橫向的角點(diǎn)數(shù)目
const int boardHeight = 6;                                //縱向的角點(diǎn)數(shù)據(jù)
const int boardCorner = boardWidth * boardHeight;        //總的角點(diǎn)數(shù)據(jù)
const int frameNumber = 13;                                //相機(jī)標(biāo)定時(shí)需要采用的圖像幀數(shù)
const int squareSize = 20;                                //標(biāo)定板黑白格子的大小 單位mm
const Size boardSize = Size(boardWidth, boardHeight);
const char imageFilePathFormat[] = "sample/right%02d.jpg";          //用于標(biāo)定的圖片路徑，格式化字符串sample/left%02d.bmp表明圖片路徑為 sample/left01.bmp - sample/leftxx.bmp


#elif defined  MY_DATA
//自己的數(shù)據(jù)
const int imageWidth = 1600;                                //攝像頭的分辨率
const int imageHeight = 1200;
const int boardWidth = 9;                                    //橫向的角點(diǎn)數(shù)目
const int boardHeight = 6;                                    //縱向的角點(diǎn)數(shù)據(jù)
const int boardCorner = boardWidth * boardHeight;           //總的角點(diǎn)數(shù)據(jù)
const int frameNumber = 10;                                         //相機(jī)標(biāo)定時(shí)需要采用的圖像幀數(shù)
const int squareSize = 30;                                   //標(biāo)定板黑白格子的大小 單位mm
const Size boardSize = Size(boardWidth, boardHeight);
Size imageSize = Size(imageWidth, imageHeight);
const char imageFilePathFormat[] = "image/right/%d.bmp";

#endif // SAMPLE


Mat intrinsic;                                            //相機(jī)內(nèi)參數(shù)
Mat distortion_coeff;                                    //相機(jī)畸變參數(shù)
vector rvecs;                                        //旋轉(zhuǎn)向量
vector tvecs;                                        //平移向量
vector> corners;                        //各個(gè)圖像找到的角點(diǎn)的集合 和objRealPoint 一一對(duì)應(yīng)
vector> objRealPoint;                    //各副圖像的角點(diǎn)的實(shí)際物理坐標(biāo)集合

vector corner;                                    //某一副圖像找到的角點(diǎn)

Mat rgbImage, grayImage;

/*計(jì)算標(biāo)定板上模塊的實(shí)際物理坐標(biāo)*/
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);
    }
}


/*設(shè)置相機(jī)的初始參數(shù) 也可以不估計(jì)*/
void guessCameraParam(void)
{
    /*分配內(nèi)存*/
    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)
{
    /*保存數(shù)據(jù)*/
    //cvSave("cameraMatrix.xml", &intrinsic);
    //cvSave("cameraDistoration.xml", &distortion_coeff);
    //cvSave("rotatoVector.xml", &rvecs);
    //cvSave("translationVector.xml", &tvecs);


    /*保存數(shù)據(jù)*/
    /*輸出數(shù)據(jù)*/
    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
";
    }


    /*輸出數(shù)據(jù)*/
    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)    //所有角點(diǎn)都被找到 說(shuō)明這幅圖像是可行的
        {
            /*
            Size(5,5) 搜索窗口的一半大小
            Size(-1,-1) 死區(qū)的一半尺寸
            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));        //把合格的圖片保存起來(lái)
            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);
    }


    /*
    圖像采集完畢 接下來(lái)開始攝像頭的校正
    calibrateCamera()
    輸入?yún)?shù) objectPoints  角點(diǎn)的實(shí)際物理坐標(biāo)
    imagePoints   角點(diǎn)的圖像坐標(biāo)
    imageSize       圖像的大小
    輸出參數(shù)
    cameraMatrix   相機(jī)的內(nèi)參矩陣
    distCoeffs       相機(jī)的畸變參數(shù)
    rvecs           旋轉(zhuǎn)矢量(外參數(shù))
    tvecs           平移矢量(外參數(shù)）
    */


    /*設(shè)置實(shí)際初始參數(shù) 根據(jù)calibrateCamera來(lái) 如果flag = 0 也可以不進(jìn)行設(shè)置*/
    guessCameraParam();
    cout << "guess successful" << endl;
    /*計(jì)算實(shí)際的校正點(diǎn)的三維坐標(biāo)*/
    calRealPoint(objRealPoint, boardWidth, boardHeight, frameNumber, squareSize);
    cout << "cal real successful" << endl;
    /*標(biāo)定攝像頭*/
    calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distortion_coeff, rvecs, tvecs, 0);
    cout << "calibration successful" << endl;
    /*保存并輸出參數(shù)*/
    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");
}

?

?

?