【機器視覺】1. 張正友平面標定法

05-14

張正友的平面標定方法是介於傳統標定方法和自標定方法之間的一種方法。它既避免了傳統方法設備要求高，操作繁瑣等缺點，又較自標定方法精度高，因此張氏標定法被廣泛應用於計算機視覺方面，本文嘗試對這一標定方法做一介紹。包括：

模型即如何由光學成像公式和坐標變換方法建立攝像機的參數矩陣
演算法 即如何對參數矩陣進行計算
優化即如何計算畸變，以及如何對參數進行優化

坐標變換

定義[位置（position）描述]

在三維坐標系A下確定空間中一點的位置，用一個 $3 imes 1$ 的矢量表示為 ${}^A {}P=(x_A,y_A,z_A)^T$

定義[姿態（orientation）描述]

在物體上固定一個坐標系B，給出此坐標系相對於參考坐標系A的表達，即在坐標系A中表達坐標系B的三個單位矢量 ${}^A oldsymbol{x_B}=egin{pmatrix}x_B cdot x_A\ x_B cdot y_A\ x_B cdot z_Aend{pmatrix}quad {}^A oldsymbol{y_B}=egin{pmatrix}y_B cdot x_A\ y_B cdot y_A\ y_B cdot z_Aend{pmatrix}quad {}^A oldsymbol{z_B}=egin{pmatrix}z_B cdot x_A\ z_B cdot y_A\ z_B cdot z_Aend{pmatrix}$

用旋轉矩陣 ${}{_B^A}{}R=({}^A oldsymbol{x_B}, {}^A oldsymbol{y_B}, {}^A oldsymbol{z_B})^T$

描述姿態。

定義[位姿（pose）描述]

位置描述和姿態描述統稱為位姿（pose）描述。將坐標系B固定在物體上，並考察坐標系B相對於參考坐標系A的位姿，用 ${}^A {P_{OB}}$ 表示坐標系B的原點在坐標系A中的位置矢量，用旋轉矩陣 ${}{_B^A}{}R$ 表示姿態，那麼B相對於A的旋轉 $oldsymbol{B}=({}{_B^A}{}R,{}^A {P_{OB}})$

旋轉矩陣的性質

- ${}{_B^A}{}R=({}^A oldsymbol{x_B}, {}^A oldsymbol{y_B}, {}^A oldsymbol{z_B})^T=egin{pmatrix}{}{^B}{oldsymbol{x}_A^T} \ {}{^B}{oldsymbol{y}_A^T} \ {}{^B}{oldsymbol{z}_A^T} end{pmatrix}$
- 旋轉矩陣中的9個元素只有3個是獨立的
旋轉矩陣是單位正交矩陣， ${}^A oldsymbol{x_B}, {}^A oldsymbol{y_B}, {}^A oldsymbol{z_B}$ 是單位矢量，且相互垂直
- $ext{det} ({}{_B^A}{}R)=1, {}{_B^A}{R^{-1}}={}{_B^A}{R^{T}}={}{^B_A}{}R$

定義[平移坐標變換]

只變換位置不變換姿態 ${}^A {}P={}{^B}{}P+{}^A {P_{OB}}$

定義[旋轉坐標變換]

只變換姿態不變換位置，兩個坐標系原點相同 ${}^A {}P={}{_B^A}{}R {}{^B}{}P$

一般坐標變換（位姿變換）方程 ${}^A {}P={}{_B^A}{}R {}{^B}{}P+{}^A {P_{OB}}$

定義[齊次坐標變換]

用 $4 imes 1$ 的列矢量表示三維空間中的點，稱為點的齊次坐標 (Homogeneous coordinate)，即 $P=(x,y,z,1)^T$ ，那麼齊次變換矩陣 ${}{_B^A}{}T=egin{pmatrix}{}{_B^A}{}R & {}^A {P_{OB}}\ oldsymbol{0} & 1end{pmatrix}$

且 ${}{_C^A}{}T={}{_B^A}{}T {}{_C^B}{}T$

使用齊次坐標的目的，是為了利用矩陣變換，不僅能表示伸縮與旋轉，還能夠表示平移。三維點的齊次坐標有形如[x,y,z,w]的形式，設w=1，此時相當於我們把3維的坐標平移搬去了w=1的平面上，也就是4維空間的點投影到w=1平面上，齊次坐標映射的3D坐標是（x/w,y/w,z/w），也就是（x,y,z）；（x,y,z）在齊次空間中有無數多個點與之對應。所有點的形式是（kx,ky,kz,k），其軌跡是通過齊次空間原點的「直線」，而每個點相當於3維的世界坐標。

當w=0時，可解釋為無窮遠的「點」，其意義是描述方向。這也是平移變換的開關，當w=0時，此時不能平移變換了。這個現象是非常有用的，因為有些向量代表「位置」，應當平移，而有些向量代表「方向」，如表面的法向量，不應該平移。從幾何意義上說，能將第一類數據當作」點」，第二類數據當作」向量」。可以通過設置w的值來控制向量的意義。

下面對旋轉運動的表示與轉換進行討論。

方向餘弦矩陣可以用來表示兩個坐標系之間的旋轉，同樣也可以用來表示一個向量繞相同坐標系中某個軸的旋轉。討論一下當它表達兩個坐標系之間的選擇時的定義方式，如下，假設兩組坐標系的基底，分別為： $vec v = (vec {{i}_{0}},vec {{j}_{0}},vec {{k}_{0}})quad vec u = (vec {{i}_{1}},vec {{j}_{1}},vec {{k}_{1}})$

另外，假設有一個向量a ，那麼a 在這兩組基底下的投影為： $vec a=vec vcdot a_{v}=vec ucdot a_{u}$

則 $C=vec v^{T}cdotvec u = egin{pmatrix} vec {{i}_{0}}cdotvec {{i}_{1}} & vec {{i}_{0}}cdotvec {{j}_{1}} & vec {{i}_{0}}cdotvec {{k}_{1}} \ vec {{j}_{0}}cdotvec {{i}_{1}} & vec {{j}_{0}}cdotvec {{j}_{1}} & vec {{j}_{0}}cdotvec {{k}_{1}} \ vec {{k}_{0}}cdotvec {{i}_{1}} & vec {{k}_{0}}cdotvec {{j}_{1}} & vec {{k}_{0}}cdotvec {{k}_{1}} end{pmatrix}$ $a_{v}=vec v^{T}cdotvec u cdot a_{u}=a_{v}=Ccdot a_{u}$

歐拉角適合用於表示兩個坐標系之間的旋轉。歐拉角方法根據一切旋轉都能分解為三次繞空間中不同軸的旋轉的原理，表明了一切坐標系的取向，都可以用三個歐拉角來表示。

歐拉角

事實上，歐拉角法可以分為兩類，一類是依次旋轉三個不同的軸，稱為Tait-Bryan

angles，因此可選順序有：X-Y-Z，X-Z-Y，Y-X-Z，Y-Z-X，Z-X-Y，Z-Y-X；另一類是相鄰兩次旋轉不同的軸，也就是上文介紹的那一類，稱為Euler

angles，可選順序有：X-Y-X，X-Z-X，Y-X-Y，Y-Z-Y，Z-X-Z，Z-Y-Z。由於繞不同的軸旋轉最後得到的歐拉角是不同的，因此在用到歐拉角的場合必須指明旋轉的順序。歐拉角表示方法中其實還存在外在旋轉和內在旋轉的區別，前者是指每次圍繞的旋轉軸是原始坐標系的軸，後者則是圍繞旋轉後得到的坐標系的軸。

設歐拉角的旋轉順序與方式為Z-Y-X，並且是內在旋轉。下面，我們來推導由歐拉角到旋轉矩陣的轉換關係。

繞Z軸旋轉 $psi$ 角度（從n繫到1系），即偏航角(yaw)

${}{_{n}^{1}}{}oldsymbol{R} = egin{pmatrix} cos{psi} & sin{psi} & 0 \ -sin{psi} & cos{psi} & 0 \ 0 & 0 & 1 end{pmatrix}$

繞Y軸旋轉 $heta$ 角度（從1繫到2系），即俯仰角(pitch)

${}{_{1}^{2}}{}oldsymbol{R} = egin{pmatrix} cos{ heta} & 0 & -sin{ heta} \ 0 & 1 & 0 \ sin{ heta} & 0 & cos{ heta} end{pmatrix}$

繞X軸旋轉 $gamma$ 角度（從2繫到b系），即滾轉角(roll)

${}{_{2}^{b}}{}oldsymbol{R} = egin{pmatrix} 1 & 0 & 0 \ 0 & cos{gamma} & sin{gamma} \ 0 & -sin{gamma} & cos{gamma} end{pmatrix}$

則

${}{_{n}^{b}}{}oldsymbol{R} = {}{_{2}^{b}}{}oldsymbol{R} cdot {}{_{1}^{2}}{}oldsymbol{R} cdot {}{_{n}^{1}}{}oldsymbol{R} =egin{pmatrix} cos{ heta} cos{psi} & cos{ heta} sin{psi} & -sin{ heta} \ sin{ heta} sin{gamma} cos{psi} - cos{gamma} sin{psi} & sin{ heta} sin{gamma} sin{psi} + cos{gamma} cos{psi} & cos{ heta} sin{gamma} \ sin{ heta} cos{gamma} cos{psi} + sin{gamma} sin{psi} & sin{ heta} cos{gamma} sin{psi} - sin{gamma} cos{psi} & cos{ heta} cos{gamma} end{pmatrix}$

以上便定義了由歐拉角到旋轉矩陣的轉換關係。

攝像機模型

攝像機模型中的幾個坐標系

-[世界坐標系(w)] 參考坐標系/基準坐標系，用於描述攝像機和物體的位置
-[攝像機坐標系(c)] 固定在攝像機上，原點在光心，Zc軸沿光軸方向， Xc/Yc軸分別平行於成像平面
-[以物理單位表示的圖像坐標系 (x, y)] 原點在攝像機光軸與圖像平面的交點，x/y軸與攝像機Xc/Yc軸平行，沿圖像平面方向
-[以像素為單位表示的圖像坐標系 (u, v)] 原點在數字圖像的左上角，u/v軸沿圖像平面向右向下為正方向

首先考慮針孔攝像機模型，記空間點在攝像機坐標系中的齊次坐標為 $X_c=(x_c, y_c, z_c, 1)^T$ ，它的像點在圖像坐標系中的齊次坐標記為 $m=(x, y, 1)^T$ ，相機焦距為f，根據相似三角形有

$egin{cases} x=frac{fx_c}{z_c} \ y=frac{fy_c}{z_c} end{cases}$

即

$z_cm = egin{pmatrix} f_x \ f_y \ 1 end{pmatrix}= egin{pmatrix} f & 0 & 0 & 0 \ 0 & f & 0 & 0 \ 0 & 0 & 1 & 0 end{pmatrix}X_c$

針孔攝像機模型將物體從攝像機坐標系轉換到xy坐標系表示，下面我們需要將點向uv坐標系轉換，也就是圖像數字化。通常我們獲取得到的圖像是CCD攝像機採集的數字圖像，CCD相機是將圖像平面的點進行數字離散化。設CCD攝像機數字離散化後的像素是一個矩形，矩形的長與寬分別為dx，dy；主點不是圖象坐標系原點，在圖像坐標系中坐標為

$(x_0, y_0, 1)^T$ ，則 $(u_0,v_0)^T=(x_0/d_x,y_0/d_y)^T$ 為CCD攝像機的主點

$1^{circ}$ 當uv軸互相垂直時，則

$egin{cases} u=frac{x}{d_x} +u_0\ v=frac{y}{d_y} +v_0 end{cases}Rightarrow egin{pmatrix} u \ v \ 1 end{pmatrix}= egin{pmatrix}frac{1}{d_x} & 0 & u_0 \ 0 & frac{1}{d_y} & v_0 \ 0 & 0 & 1 end{pmatrix} egin{pmatrix} x \ y \ 1 end{pmatrix}$

則攝像機內參數矩陣

$K=egin{pmatrix}frac{1}{d_x} & 0 & u_0 \ 0 & frac{1}{d_y} & v_0 \ 0 & 0 & 1 end{pmatrix} egin{pmatrix} f & 0 & 0 \ 0 & f & 0 \ 0 & 0 & 1 end{pmatrix}=egin{pmatrix}k_x & 0 & u_0 \ 0 & k_y & v_0 \ 0 & 0 & 1 end{pmatrix}$

其中

$egin{cases}k_x=f/d_x\ k_y=f/d_yend{cases}$

稱為CCD攝像機在u軸和v軸方向上的尺度因子。

$2^{circ}$ 當uv軸有夾角 $heta$ 時，則

$egin{cases} u=frac{x-y ext{cot} heta}{d_x} +u_0\ v=frac{y}{d_y sin heta} +v_0 end{cases}Rightarrow egin{pmatrix} u \ v \ 1 end{pmatrix}= egin{pmatrix}frac{1}{d_x} & -frac{1}{d_x an heta} & u_0 \ 0 & frac{1}{d_ysin heta} & v_0 \ 0 & 0 & 1 end{pmatrix} egin{pmatrix} x \ y \ 1 end{pmatrix}$

則攝像機內參數矩陣

$K=egin{pmatrix}frac{1}{d_x} & -frac{1}{d_x an heta} & u_0 \ 0 & frac{1}{d_ysin heta} & v_0 \ 0 & 0 & 1 end{pmatrix} egin{pmatrix} f & 0 & 0 \ 0 & f & 0 \ 0 & 0 & 1 end{pmatrix}=egin{pmatrix}k_x & k_s & u_0 \ 0 & k_y & v_0 \ 0 & 0 & 1 end{pmatrix}$

其中

$egin{cases}k_x=f/d_x\ k_y=f/(d_ysin heta)\ k_s=-k_x / an hetaend{cases}$

以上推導出了攝像機內參數模型，然而，我們一般描述一個三維點，由於相機可能一直在運動，所以我們並不是基於攝像機坐標系下對其描述。我們通常是在世界坐標系下進行描述。攝像機外參數模型就是將物體在世界坐標系中的位置，變換到攝像機坐標系下。攝像機外參數矩陣是一個四階矩陣

${}{_w^c} M=egin{pmatrix}{}{_w^c}{}R & {}{^c}P_{Ow} \ 0^T & 1end{pmatrix}$

則攝像機參數矩陣（單應矩陣）

$M_{3 imes 4}=(K,oldsymbol{0})cdot {}{_w^c} M$

直接線性變換（DLT）標定

定義[單應性變換]

單應性變換（homography transform）就是一個平面到另一個平面的映射關係。在標定問題里，單應矩陣包括攝像機內外參數矩陣。

我們先舉一個簡單的例子。在圖像拼接中，得到了兩張圖像的特徵匹配，兩個點集分別記作X和X；用單應性變換來擬合二者的關係，可表達為 $cegin{pmatrix}u\v\1end{pmatrix}=Hegin{pmatrix}x\y\1end{pmatrix}$ 其中 $egin{pmatrix}u&v&1end{pmatrix}^T$ 是X中特徵點的坐標， $egin{pmatrix}x&y&1end{pmatrix}^T$ 是X中特徵點的坐標，H即是單應性矩陣，代表它們之間的變換關係。H是個3×3的矩陣，有8個自由度，所以待求未知參數有8個 $H=egin{pmatrix}h_1&h_2&h_3\h_4&h_5&h_6\h_7&h_8&h_9end{pmatrix}$ 則

$egin{cases} -h_1 x-h_2 y-h_3 +(h_7 x+h_8 y +h_9)u=0\ -h_4 x-h_5 y-h_6 +(h_7 x+h_8 y +h_9)v=0 end{cases}$

整理為Ah=0的形式，其中

$A=egin{pmatrix}-x&-y&-1&0&0&0&ux&uy&u\0&0&0&-x&-y&-1&vx&vy&vend{pmatrix}$

$h=egin{pmatrix}h_1&h_2&h_3&h_4&h_5&h_6&h_7&h_8&h_9end{pmatrix}^T$

由未知變數的個數可知，求解出H至少需要4對匹配點。通常情況下為了得到更穩定的結果，會用到多於4對的特徵匹配。所以，這個方程會變成超定的，可以將最小二乘解作為最後的解。方程的最小二乘解有一個既定的結論，即對A進行SVD分解，A的最小的奇異值對應的右奇異向量即是h的解。

證明：解方程Ah=0等價於優化問題 $min |Ah| \ ext{s.t. } |h|=1$

因為U是單位正交矩陣，所以 $|Ah|=|USigma V^T h|=|Sigma V^T h|$

令 $y=V^T h$ ，則方程等價於

$min |Sigma y| \ ext{s.t. } |y|=1$

由於 $Sigma$ 是一個對角矩陣，對角元的元素按遞減的順序排列，因此最優解在 $y = (0, 0,..., 1)^T$ 取得，就是V的最小奇異值對應的列向量，即V的最後一列。Q.E.D.

回到標定問題，當uv坐標系中u垂直於v時，若不考慮畸變，那麼 $z_c egin{pmatrix} u \ v \ 1 end{pmatrix}=egin{pmatrix}k_x & 0 & u_0 &0\ 0 & k_y & v_0 &0\ 0 & 0 & 1 &0end{pmatrix}egin{pmatrix} {}{_w^c}{}R & {}{^c}P_{Ow} \ 0^T & 1 end{pmatrix}egin{pmatrix} x_w \ y_w \z_w \1 end{pmatrix}$

攝像機矩陣

$M=egin{pmatrix} m_{11} & m_{12} & m_{13} & m_{14}\m_{21} & m_{22} & m_{23} & m_{24}\m_{31} & m_{32} & m_{33} & m_{34} end{pmatrix}$

將M的元素作為未知數，矩陣展開消去 $z_c$ ，對於n個已知的空間點，得到2n個關於M的方程

$A_{2n imes 11}(m_{11},m_{12},...,m_{33})^T=(u_1 m_{34},v_1 m_{34},...,u_n m_{34},v_n m_{34})^T$

設 $m=frac{1}{m_{34}} (m_{11},m_{12},...,m_{33})^T,b=(u_1 ,v_1 ,...,u_n ,v_n)^T$

則 $m=(A^T A)^{-1} A^T b$ 在相差一個常數因子 $m_{34}$ 的前提下，確定M，設 $m_i=(m_{i1},m_{i2},m_{i3})^T$ ，平移向量 $t={}{^c}P_{Ow}=(t_x,t_y,t_z)^T$ 旋轉矩陣 $R=egin{pmatrix} r_1^T \ r_2^T \ r_3^T end{pmatrix}$ 則

$egin{gather} m_{34}=frac{1}{||m_3||}\u_0=m_{34}^2 m_1{}{^T}m_3quad v_0=m_{34}^2 m_2{}{^T}m_3\k_x=m_{34}^2 ||m_1 imes m_3||quad k_y=m_{34}^2 ||m_2 imes m_3||\t_x=frac{m_{34}(m_{14}-u_0)}{k_x}quad t_y=frac{m_{34}(m_{24}-v_0)}{k_y}quad t_z=m_{34}\r_1=frac{m_{34}(m_1-u_0m_3)}{k_x}quad r_2=frac{m_{34}(m_2-v_0m_3)}{k_y}quad r_3=m_{34} m_3 end{gather}$

張氏標定法：攝像機參數的估計

張正友平面標定法的前提

- 認為內參數矩陣 $K=egin{pmatrix}k_x & k_s & u_0 \ 0 & k_y & v_0 \ 0 & 0 & 1 end{pmatrix}$

- 標定物：平面靶標

- 將世界坐標系置於靶標平面，原點設在靶標一角，Xw/Yw方向沿靶標平面，Zw方向垂直於靶標平面

- 先不考慮畸變，標定攝像機參數，得到參數的線性初值；然後利用線性初值，進行非線性標定，得到畸變參數

因此，在

$z_c egin{pmatrix} u \ v \ 1 end{pmatrix}=egin{pmatrix}k_x & k_s & u_0 &0\ 0 & k_y & v_0 &0\ 0 & 0 & 1 &0end{pmatrix}egin{pmatrix} {}{_w^c}{}R & {}{^c}P_{Ow} \ 0^T & 1 end{pmatrix}egin{pmatrix} x_w \ y_w \z_w \1 end{pmatrix}$

中令 $z_w=0$ ， ${}{^c}P_{Ow}=t,{}{_w^c}{}R=(r_1,r_2,r_3)$ 則

$z_c egin{pmatrix} u \ v \ 1 end{pmatrix}=egin{pmatrix}k_x & k_s & u_0 \ 0 & k_y & v_0 \ 0 & 0 & 1 end{pmatrix}egin{pmatrix} r_1 & r_2 & t end{pmatrix}egin{pmatrix} x_w \ y_w \1 end{pmatrix}=Hegin{pmatrix} x_w \ y_w \1 end{pmatrix}$

令

$H=egin{pmatrix}h_1^T \ h_2^T \ h_3^Tend{pmatrix}quad P_i=egin{pmatrix}x_i\y_i\1end{pmatrix}$

則

$egin{pmatrix} P_i^T & 0 & -u_i P_i^T\0 & P_i^T & v_i P_i^T end{pmatrix}egin{pmatrix} h_1 \ h_2 \h_3 end{pmatrix}=0$

對於n個特徵點

$egin{pmatrix} P_1^T & 0 & -u_1 P_1^T\0 & P_1^T & v_1 P_1^T\ vdots & vdots & vdots \P_n^T & 0 & -u_n P_n^T\0 & P_n^T & v_n P_n^T end{pmatrix}egin{pmatrix} h_1 \ h_2 \h_3 end{pmatrix}=Aegin{pmatrix} h_1 \ h_2 \h_3 end{pmatrix}=0$

對A進行SVD分解，即$A=USigma V^T$，則以上方程的解是V的最後一列。

假如考慮雜訊影響，假設雜訊為零均值高斯雜訊，方差矩陣為 $Sigma_i$ ，由最大似然估計求解單應矩陣H，或定義目標函數F，求解H 使F取到最小

$min F=sum_{i=1}^n (Q_i-hat Q_i)^T Sigma_i (Q_i-hat Q_i) quad Q_i=egin{pmatrix} u_i \ v_i end{pmatrix}hat Q_i=(h_3^T P_i)^{-1}egin{pmatrix} h_1^T P_i\h_2^T P_i end{pmatrix}$

實際應用中假設 $Sigma_i=sigma_i^2 I$ ，則 $F=sum_{i=1}^n ||Q_i-hat Q_i||^2$ 使用不考慮雜訊情況下得到的單應矩陣H作為初值計算 $hat Q_i$ 通過Levenburg-Marquardt演算法求出H的最終解。

H是一個齊次矩陣，所以有8個未知數，至少需要8個方程，每對對應點能提供兩個方程，所以至少需要四個對應點，就可以算出世界平面到圖像平面的單應性矩陣H。這樣得到的H，計算結果與真實解相差一個常數因子，即

$H=(h_1 h_2 h_3) = lambda K(r_1 r_2 t)$

那麼

$egin{cases} r_1=frac{1}{lambda}K^{-1}h_1 \ r_2=frac{1}{lambda}K^{-1}h_2 end{cases}$

由於旋轉矩陣是個酉矩陣， $r_1$ 和 $r_2$ 正交，即

$egin{cases} r_1^Tr_2=0 \ ||r_1||=||r_2||=1 end{cases}$

可得約束條件

$egin{cases} h_1^TK^{-T}K^{-1}h_2=0 \ h_1^TK^{-T}K^{-1}h_1 = h_2^TK^{-T}K^{-1}h_2 end{cases}$

即每個單應性矩陣能提供兩個方程，而內參數矩陣包含5個參數，要求解，至少需要3個單應性矩陣。為了得到三個不同的單應性矩陣，我們使用至少三幅棋盤格平面的圖片進行標定。通過改變相機與標定板之間的相對位置來得到三個不同的圖片。假如只有兩幅圖片，那麼 $k_s$ 將不能估計，也就是認為數字圖像坐標系uv相互垂直（ $k_s =0$ ）。記

$K=egin{pmatrix} alpha & gamma & u_0 \ 0 & eta & v_0 \ 0 & 0 & 1 end{pmatrix}$

則

$B=K^{-T}K^{-1}= egin{pmatrix} B_{11} & B_{12} & B_{13} \ B_{21} & B_{22} & B_{23} \ B_{31} & B_{32} & B_{33} end{pmatrix}= egin{pmatrix} frac{1}{alpha^2} & -frac{gamma}{alpha^2eta} & frac{v_0gamma-u_0eta}{alpha^2eta} \ -frac{gamma}{alpha^2eta} & frac{gamma^2}{alpha^2eta^2}+frac{1}{eta^2} & -frac{gamma(v_0gamma-u_0eta)}{alpha^2eta^2}-frac{v_0}{eta^2} \ frac{v_0gamma-u_0eta}{alpha^2eta} & -frac{gamma(v_0gamma-u_0eta)}{alpha^2eta^2}-frac{v_0}{eta^2} & frac{(v_0gamma-u_0eta)^2}{alpha^2eta^2}+frac{v_0}{eta^2}+1 end{pmatrix}$

可以看到，B是一個對稱陣，所以B的有效元素為六個，讓這六個元素寫成向量b，即

$b=egin{pmatrix} B_{11} & B_{12} & B_{22} & B_{13} & B_{23} & B_{33} end{pmatrix}^T$

那麼

$h_i^TBh_j = v^T_{ij}b \ ext{where } v_{ij}=egin{pmatrix} h_{i1}h_{j1} & h_{i1}h_{j2}+h_{i2}h_{j1} & h_{i2}h_{j2} & h_{i3}h_{j1}+h_{i1}h_{j3} & h_{i3}h_{j2}+h_{i2}h_{j3} & h_{i3}h_{j3} end{pmatrix}^T$

利用約束條件可得

$egin{pmatrix} v^T_{12} \ (v_{11}-v_{22})^T end{pmatrix} b=0$

我們至少需要三幅包含棋盤格的圖像，可以計算得到B，然後通過Cholesky分解得到相機的內參數矩陣K，首先計算出

$v_0=frac{B_{12} B_{13}- B_{11} B_{23}}{B_{11}B_{22}-B_{12}^2}$

然後定義

$c=B_{33}-frac{B_{13}^2 + v_0 (B_{12} B_{13}- B_{11} B_{23})}{B_{11}}$

於是內參數

$egin{gather} k_x=alpha=sqrt{frac{c}{B_{11}}}\k_y=eta=sqrt{frac{cB_{11}}{B_{11}B_{22}-B_{12}^2}}\k_s=gamma=frac{-B_{12} alpha^2 eta}{c}\u_0=frac{gamma v_0}{eta}-frac{B_{13} alpha^2}{c} end{gather}$

而外參數

$egin{gather} lambda =frac{1}{|K^{-1}h_1|}=frac{1}{|K^{-1}h_2|} \ r_1=frac{1}{lambda}K^{-1}h_1 \ r_2=frac{1}{lambda}K^{-1}h_2 \ r_3 = r_1 imes r_2 \ t=lambda K^{-1}h_3 end{gather}$

考慮到R是單位正交陣，因此對R進行奇異值分解就有 $Rapprox UIV^T =UV^T$ ，其中U和V通過對 $R^T R$ 的特徵向量作正交化單位化得到。

張氏標定法：畸變的估計

張氏標定法只關注了影響最大的徑向畸變，並忽略四階以上的畸變數

$egin{cases} x_d =x_u[1+k_1(x_u^2+y_u^2)+k_2(x_u^2+y_u^2)^2]\y_d =y_u[1+k_1(x_u^2+y_u^2)+k_2(x_u^2+y_u^2)^2] end{cases}$

其中 $(x_d,y_d)$ 表示角點在成像面上的實際坐標， $(x_u,y_u)$ 表示角點在成像面上的理想坐標。將畸變模型轉換到數字圖像坐標進行求解

$egin{cases} hat u = u_0 + alpha x_d + gamma y_d \ hat v = v_0 + eta y_d end{cases}$

其中，(u,v)是理想的像素坐標， $(hat u,hat v)$ 是實際的像素坐標。 $(u_0,v_0)$ 代表主點，則

$egin{cases} hat u = u + (u-u_0)[k_1(x_u^2+y_u^2)+k_2(x_u^2+y_u^2)^2] \ hat v = v + (v-v_0)[k_1(x_u^2+y_u^2)+k_2(x_u^2+y_u^2)^2] end{cases}$

即

$egin{pmatrix} (u-u_0)(x_u^2+y_u^2) & (u-u_0)(x_u^2+y_u^2)^2 \ (v-v_0)(x_u^2+y_u^2) & (v-v_0)(x_u^2+y_u^2)^2 end{pmatrix}egin{pmatrix}k_1 \ k_2 end{pmatrix}= egin{pmatrix} hat u -u \ hat v -v end{pmatrix}$

簡記為 $Dk=d$

那麼

$k=[k_1 k_2]^T = (D^TD)^{-1}D^Td$

上述的推導結果是基於理想情況下的解，但由於可能存在高斯雜訊，所以使用最大似然估計進行優化。設我們採集了n副包含棋盤格的圖像進行定標，每個圖像里有棋盤格角點m個。令第i副圖像上的角點 $M_{ij}$ 在上述計算得到的攝像機矩陣下圖像上的投影點為：

$hat{m}(K,R_i,t_i,M_{ij}) = K(R|t)M_{ij}$

其中Ri和ti是第i副圖對應的旋轉矩陣和平移向量，K是內參數矩陣。則角點的概率密度函數為：

$f(m_{ij})=frac{1}{sqrt{2pi}}exp left(frac{-(hat{m}(K,R_i,t_i,M_{ij})-m_{ij})^2}{sigma^2} ight)$

似然函數

$L(A,R_i,t_i,M_{ij}) = prod^{n,m}_{i=1,j=1}f(m_{ij})=frac{1}{sqrt{2pi}}expleft(frac{-sum^n_{i=1}sum^m_{j=1}(hat{m}(K,R_i,t_i,M_{ij})-m_{ij})^2}{sigma^2} ight)$

讓L取得最大值，即讓

$sum^n_{i=1}sum^m_{j=1} | hat{m}(K,R_i,t_i,M_{ij})-m_{ij} |^2$

最小。這裡使用的是多參數非線性系統優化問題的Levenberg-Marquardt演算法進行迭代求最優解。

Levenburg-Marquardt演算法

通常的最小二乘問題都可以表示為

$min_x F(x) = frac{1}{2}sum_{i=1}^{n}(f_i(x)^2) = frac{1}{2} left | f(x) ight | ^2 = frac{1}{2}f(x)^Tf(x)$

對 $f_i (x)$ 在 $x_k$ 處作泰勒展開

$f_i(x_k+h) approx f_i(x_k)+ abla f_i(x_k)^Th$

$f(x_k+h) approx f(x_k)+J(x_k)h$

其中Jacobi矩陣

$J(x_k)=egin{pmatrix} abla f_1(x_k)^T \ abla f_2(x_k)^T \ vdots \ abla f_n(x_k) \ end{pmatrix}= egin{pmatrix} frac{partial f_1(x_k)}{partial x_1} &frac{partial f_1(x_k)}{partial x_2} &cdots &frac{partial f_1(x_k)}{partial x_m} \ frac{partial f_2(x_k)}{partial x_1} &frac{partial f_2(x_k)}{partial x_2} &cdots &frac{partial f_2(x_k)}{partial x_m} \ vdots &vdots &ddots &vdots \ frac{partial f_n(x_k)}{partial x_1} &frac{partial f_n(x_k)}{partial x_2} &cdots &frac{partial f_n(x_k)}{partial x_m} end{pmatrix}$

記 $f_k=f(x_k),J_k=J(x_k)$ 那麼

$frac{partial F(x)}{partial x_j} = sum_{i=1}^{n}f_i(x)frac{partial f_i(x)}{partial x_j}$

即F(x)的梯度

$g=F(x)=J(x)^Tf(x)$

下面討論利用數值最優化方法求解非線性最小二乘問題的過程。

最速下降法

假設 $h^TF(x) < 0$ ，則h是F(x)下降方向，即對於任意足夠小的 $alpha>0$ ，都滿足

$F(x+αh)<F(x)$

則

$lim_{alpha o0}frac{F(x)-F(x+alpha h)}{alpha left | h ight |}=-frac{1}{left | h ight |}h^TF(x)=-||F(x)||cos heta$

其中為矢量h和F(x)夾角，當 $heta=pi$ 時，下降最大。即 $h_{sd}=?F(x)$ 是最快下降方向。

高斯-牛頓演算法

選擇h使得F(x)在 $x_k$ 附近二階近似，則

$egin{split} F(x_k+h) approx L(h) & = frac{1}{2}f(x_k+h)^Tf(x_k+h) \ & = frac{1}{2}f_k^Tf_k+h^TJ_k^Tf_k+frac{1}{2}h^TJ_k^TJ_kh \ & = F(x_k) + h^TJ_k^Tf_k+frac{1}{2}h^TJ_k^TJ_kh end{split}$

則

$frac{partial}{partial h} F(x_k+h)= J_k^Tf_k+J_k^TJ_kh=0 Rightarrow (J_k^TJ_k)h_{gn}=-J_k^Tf_k$

即

$egin{cases} h_{gn}=-(J_k^TJ_k)^{-1}J_k^Tf_k\ x_{k+1}=x_k+h_{gn} end{cases}$

直到 $left |F(x_{k+1})-F(x_k) ight|< varepsilon$

高斯-牛頓法可以看做使用Hessian矩陣的最速下降法

$H=egin{pmatrix} frac{partial ^2f}{partial x_1partial x_1} & frac{partial ^2f}{partial x_1partial x_2} & dots&frac{partial ^2f}{partial x_1partial x_n} \ frac{partial ^2f}{partial x_2partial x_1} & frac{partial ^2f}{partial x_2partial x_2} & dots&frac{partial ^2f}{partial x_2partial x_n}\ vdots & vdots & ddots&vdots\ frac{partial ^2f}{partial x_npartial x_1} & frac{partial ^2f}{partial x_npartial x_2} & dots&frac{partial ^2f}{partial x_npartial x_n} end{pmatrix}$

即

$egin{cases} ||x||_{ abla^2f(x)}=x^T abla^2f(x)x\Delta_{nsd}leftarrow ext{arg} min_v ( abla f(x)^Tvmid ||v||<=1)Delta_{nsd} end{cases}Rightarrow Delta_{nsd}=H^{-1} abla f(x)$

LM演算法

通常高斯牛頓法收斂較快，但是不穩定，且要求 $J^T J$ 非奇異。而梯度下降法穩定，但是收斂較慢。所以接下來我們介紹高斯牛頓演算法和最速下降法混合法，即Levenburg-Marquardt演算法，即加入正則項使得 $(J_k^TJ_k+lambda I)h=-J_k^Tf_k$

記其解為 $h_{lm}$ ，則

- $lambda=0 Rightarrow h_{lm}approx h_{gn}$ ，即為Gauss-Newton法

- 當 $lambda$ 充分大時 $lambda Ih_{lm}approx -J_k^Tf_k, h_{lm}=-frac{1}{u}J_k^Tf_k$ ，即為最速下降法

- 特別當 $lambda ightarrow infty, ||h_{lm} || ightarrow 0$

因為

$egin{split} F(x+h) approx L(h) & = frac{1}{2}f(x+h)^Tf(x+h) \ & = frac{1}{2}f^Tf+h^TJ^Tf+frac{1}{2}h^TJ^TJh \ & = F(x) + h^TJ^Tf+frac{1}{2}h^TJ^TJh end{split}$

所以定義增益比 $ho=frac{F(x)-F(x+h_{lm})}{L(0)-L(h_{lm})}$ 則

- 在實際中，我們選擇一階近似、二階近似並不是在所有定義域都滿足的，而是在 $|x-x_0|<varepsilon$ 作用域內滿足這個近似條件。
- 當 $ho$ 較大時，表明F(x+h)的二階近似L(h)比F(x+h)更加接近於F(x)，因此二階近似比較好，所以可以減小 $lambda$ ，採用更大的迭代步長，接近Gauss-Newton法來更快收斂；
- 當 $ho$ 較小時，表明採取的二階近似較差，因此通過增大 $lambda$ ，採用更小的步長，接近最速下降法來穩定的迭代。

LM演算法偽代碼

總結：張正友平面標定法偽代碼

張氏標定法偽代碼