相機參數和相機模型各個座標系(世界座標系、相機座標系、圖像座標系、像素座標系之間變換）

座標系變換

1. 幾個座標系：

• 世界座標系(world coordinate system)：
  用戶定義的三維世界的座標系，爲了描述目標物在真實世界裏的位置而被引入。單位爲m。
• 相機座標系(camera coordinate system)：在相機上建立的座標系，爲了從相機的角度描述物體位置而定義，作爲溝通世界座標系和圖像/像素座標系的中間一環。單位爲m。
• 圖像座標系(image coordinate system)：爲了描述成像過程中物體從相機座標系到圖像座標系的投影透射關係而引入，方便進一步得到像素座標系下的座標。 單位爲m。
• 像素座標系(pixel coordinate system)：爲了描述物體成像後的像點在數字圖像上（相片）的座標而引入，是我們真正從相機內讀取到的信息所在的座標系。單位爲個（像素數目）。

2.相機參數

16個單目相機的參數：

• 4個內部參數（只與相機有關）：
  $fx,fy,u0,v0,(1/dx、1/dy、\gamma 、u0、v0、f )$
  實際其$fx = F*S_x$，其中的$F$就是焦距上面的$f,Sx是像素/每毫米即上面的$1/dx$，其是最後面圖裏的後兩個矩陣進行先相乘，得出的，則把它看成整體，就相當於4個內參。把 $\gamma$ 等於零，實際上也是六個.

• 5個畸變參數D：
  $k1,k2,k3$ 徑向畸變係數;
  $p1,p2$ 是切向畸變係數;
  徑向畸變：產生原因是光線在遠離透鏡中心的地方比靠近中心的地方更加彎曲,徑向畸變主要包含桶形畸變和枕形畸變兩種。
  切向畸變：產生的原因透鏡不完全平行於圖像平面，這種現象發生於成像儀被粘貼在攝像機的時候。

• 6個外部參數(取決於相機在世界中的位置):
  3個旋轉參數$R$;
  3個平移參數$T$

3. 座標系之間的轉換

3.1 整體公式

$Z_c\left[\begin{array}{c}{u } \\ {v} \\ {1}\end{array}\right] =\left[\begin{array}{ccc}{\frac{1}{dx}} & {\gamma} & {c_x} \\ {0} & {\frac{1}{dy}} & {c_y} \\ {0} & {0} & {1}\end{array}\right] \left[\begin{array}{ccc}{f} & {0} & {0} \\ {0} & {f} & {0} \\ {0} & {0} & {1}\end{array}\right] \left[\begin{array}{cc}{\boldsymbol{R}} & {\boldsymbol{t}} \\ {\boldsymbol{0}^{T}} & {1}\end{array}\right] \left[\begin{array}{c}{X_w } \\ {Y_w} \\ {Z_w} \\{1}\end{array}\right]=\left[\begin{array}{ccc}{f_x} & {0} & {c_x} \\ {0} & {f_y} & {c_y} \\ {0} & {0} & {1}\end{array}\right] \left[\begin{array}{cc}{\boldsymbol{R}} & {\boldsymbol{t}} \\ {\boldsymbol{0}^{T}} & {1}\end{array}\right] \left[\begin{array}{c}{X_w } \\ {Y_w} \\ {Z_w} \\{1}\end{array}\right]$

3.2 世界座標系->相機座標系：

剛體變換(regidbody motion):
三維空間中，當物體不發生形變時，對一個幾何物體作旋轉 $R$， 平移 $T$ 的運動，稱之爲剛體變換。世界座標系到相機座標系的變化就是剛體變換，又因爲$R$和$T$與相機無關，所有又稱其爲相機外參。
首先考慮旋轉。我們設某個單位正交基 $(e_1; e_2; e_3)$經過一次旋轉，變成了 $(e_1′ ; e_2′ ; e_3′ )$。那麼，對於同一個向量 a（注意該向量並沒有隨着座標系的旋轉而發生運動），它在兩個座標系下的座標爲 $[a_1; a_2; a_3]^T$ 和 $[a_1′ ; a_2′ ; a_3′ ]^T$。

根據座標的定義，有

$\left[e_{1}, e_{2}, e_{3}\right]\left[\begin{array}{l}{a_{1}} \\ {a_{2}} \\ {a_{3}}\end{array}\right]=\left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right]\left[\begin{array}{c}{a_{1}^{\prime}} \\ {a_{2}^{\prime}} \\ {a_{3}^{\prime}}\end{array}\right]$
左右同事乘

$\left[\begin{array}{c}{e_{1}^{T}} \\ {e_{2}^{T}} \\ {e_{3}^{T}}\end{array}\right]$
，得到

$\left[\begin{array}{l}{a_{1}} \\ {a_{2}} \\ {a_{3}}\end{array}\right]=\left[\begin{array}{lll}{e_{1}^{T} e_{1}^{\prime}} & {e_{1}^{T} e_{2}^{\prime}} & {e_{1}^{T} e_{3}^{\prime}} \\ {e_{2}^{T} e_{1}^{\prime}} & {e_{2}^{T} e_{2}^{\prime}} & {e_{2}^{T} e_{3}^{\prime}} \\ {e_{3}^{T} e_{1}^{\prime}} & {e_{3}^{T} e_{2}^{\prime}} & {e_{3}^{T} e_{3}^{\prime}}\end{array}\right]\left[\begin{array}{c}{a_{1}^{\prime}} \\ {a_{2}^{\prime}} \\ {a_{3}^{\prime}}\end{array}\right] \triangleq R a^{\prime}$
中間的矩陣 $R$ 由兩組基之間的內積組成，刻畫了旋轉前後同一個向量的座標變換關係。只要旋轉是一樣的，那麼這個矩陣也是一樣的。可以說，矩陣 $R$ 描述了旋轉本身。因此它又稱爲旋轉矩陣。
旋轉矩陣有一些特別的性質：是一個行列式爲 1 的正交矩陣。
可以把旋轉矩陣的集合定義如下：

$SO(n)=\left\{\boldsymbol{R} \in \mathbb{R}^{n \times n} | \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}$
$n$ 代表座標軸個數，比如 $SO(3)$ 就代表包含 $x，y，z$ 三個座標軸的座標系。
在歐氏變換中，除了旋轉之外還有一個平移。考慮世界座標系中的向量 $ a $，經過一次
旋轉 $R$和一次平移 $t$ 後，得到了$a′$，那麼把旋轉和平移合到一起，有：

$\boldsymbol{a}^{\prime}=\boldsymbol{R} \boldsymbol{a}+\boldsymbol{t}$
其實表示爲座標就是

$\left[\begin{array}{c}{X_{\mathrm{c}}} \\ {Y_{\mathrm{c}}} \\ {Z_{\mathrm{c}}}\end{array}\right]=\left[\begin{array}{ccc}{r_{00}} & {r_{01}} & {r_{02}} \\ {r_{10}} & {r_{11}} & {r_{12}} \\ {r_{20}} & {r_{21}} & {r_{22}}\end{array}\right]\left[\begin{array}{c}{X} \\ {Y} \\ {Z}\end{array}\right]+\left[\begin{array}{c}{T_{x}} \\ {T_{y}} \\ {T_{z}}\end{array}\right]$
上面變換關係不是一個線性關係。假設我們進行了兩次變換：$R1; t1$ 和 $R2; t2$，滿足：
$\boldsymbol{b}=\boldsymbol{R}_{1} \boldsymbol{a}+\boldsymbol{t}_{1}, \quad \boldsymbol{c}=\boldsymbol{R}_{2} \boldsymbol{b}+\boldsymbol{t}_{2}$ 合併爲 $c=R_{2}\left(R_{1} a+t_{1}\right)+t_{2}$，這樣當變化次數特別多的時候很複雜。
所以一般在三維向量的末尾添加一個1，表示爲齊次座標的形式

$\left[\begin{array}{c}{a^{\prime}} \\ {1}\end{array}\right]=\left[\begin{array}{cc}{\boldsymbol{R}} & {\boldsymbol{t}} \\ {\boldsymbol{0}^{T}} & {1}\end{array}\right]\left[\begin{array}{l}{\boldsymbol{a}} \\ {1}\end{array}\right] \triangleq \boldsymbol{T}\left[\begin{array}{l}{\boldsymbol{a}} \\ {1}\end{array}\right]$
對於這個四維向量，我們可以把旋轉和平移寫在一個矩陣裏面，使得整個關係變成了線性關係。該式中，矩陣 $T$ 稱爲變換矩陣（Transform Matrix） 。
相機座標系和世界座標系的座標原點之間的距離，受 $x,y,z$ 三個方向上的分量共同控制，所以具有三個自由度。$R$ 其實是分別繞 $X, Y, Z$三軸旋轉的效果之和.

繞z軸旋轉如右圖 [外鏈圖片轉存失敗(img-kC5cKcHh-1568880519328)

$繞z旋轉矩陣： \left[\begin{array}{l}{X_{c}} \\ {Y_{c}} \\ {Z_{c}}\end{array}\right]=\left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {\sin \theta} \\ {0} & {-\sin \theta} & {\cos \theta}\end{array}\right]\left[\begin{array}{l}{X} \\ {Y} \\ {Z}\end{array}\right]$
$繞x旋轉矩陣： \left[\begin{array}{c}{X_{c}} \\ {Y_{c}} \\ {Z_{c}}\end{array}\right]=\left[\begin{array}{ccc}{\cos \theta} & {0} & {-\sin \theta} \\ {0_{1}+1} & {1} & {0} \\ {\sin \theta} & {0} & {\cos \theta}\end{array}\right]\left[\begin{array}{c}{X} \\ {Y} \\ {Z}\end{array}\right]$
$繞y旋轉矩陣： \left[\begin{array}{l}{X_{\mathrm{c}}} \\ {Y_{\mathrm{c}}} \\ {Z_{\mathrm{c}}}\end{array}\right]=\left[\begin{array}{ccc}{\cos \theta} & {\sin \theta} & {0} \\ {-\sin \theta} & {\cos \theta} & {0} \\ {0} & {0} & {1}\end{array}\right]\left[\begin{array}{c}{X} \\ {Y} \\ {Z}\end{array}\right]$
以上分別爲繞 $x, y, z$軸旋轉的矩陣，所以旋轉矩陣爲 $R=R_x*R_y * R_z$

設 $O-x-y-z$爲相機座標系， $z$ 軸指向相機前方，$O$爲攝像機的光心，也是針孔模型中的針孔。

3.3 相機座標系->圖像座標系：

現實世界的空間點$P$，經過小孔 $O$ 投影之後，落在物理成像平面 $O′-x′-y′$ 上，成像點爲 $P′$。設 $P$的座標爲 $[X; Y; Z]^T$， $P′$ 爲 $[X′; Y ′; Z′]^T$，並且設物理成像平面到小孔的距離爲 $f$（焦距）。

我們平時處理一般會用歸一化成像平面，把成像位置轉換到與它對稱的紅色位置，這樣比較容易描述各個座標系之間關係。

根據三角形相似關係，有 $\frac{Z}{f}=- \frac{X}{X'}=- \frac{Y}{Y'}$爲了方便，一般去掉負號。
整理後：

$X′ = \frac{fX}{Z}$
$Y′ = \frac{fY}{Z}$

3.4 圖像座標系->像素座標系：

因爲相機最後得到的是像素，所以在成像平面上對像進行採樣和量化，把圖像座標系的點轉化到一個像素平面 $o-u-v$。像素座標系與圖像座標系處於同一平面，像素座標系原點爲左上角，圖像座標系原點爲中心。

像素座標系‹通常的定義方式是：原點 $o′$ 位於圖像的左上角， $u$軸向右與 $x$軸平行，$v$
軸向下與$y$軸平行。像素座標系與成像平面之間，相差了一個縮放和一個原點的平移。
設像素座標在$u$軸上縮放了$α$倍，在 v$上縮放了$β$ 倍。同時，原點平移了$[cx;cy]^T$。那麼， $P′$的座標與像素座標$[u;v]^T$的關係爲：
$\begin{cases} u = α X' + c_x = \frac{1}{dx} X' + c_x \\ v = βX' + c_y = \frac{1}{dy} Y' + c_y \end{cases}$

$\frac{1}{dx}$, $\frac{1}{dy}$分別爲在$x$, $y$方向上每個像素的物理尺寸,把 $αf$ 合併爲 $f_x$，$βf$ 合併爲 $f_y$ 得到：
$\begin{cases} u = f_x\frac{X}{Z}+ c_x \\ v = f_y\frac{Y}{Z} + c_y \end{cases}$
$f $ 的單位爲米， $α; β$ 的單位爲像素每米，所以 $f_x; f_y$的單位爲像 素。

把該式寫成矩陣形式，會更加簡潔，不過左側需要用到齊次座標, $\gamma$ 爲$x, y$不垂直時的扭曲係數，一般爲0：

$\left[\begin{array}{c}{u } \\ {v} \\ {1}\end{array}\right] = \frac{1}{Z} \left[\begin{array}{ccc}{f_x} & {\gamma} & {c_x} \\ {0} & {f_y} & {c_y} \\ {0} & {0} & {1}\end{array}\right] \left[\begin{array}{c}{X } \\ {Y} \\ {Z}\end{array}\right] = \frac{1}{Z} KP$
習慣把Z移動到左面：

$Z\left[\begin{array}{c}{u } \\ {v} \\ {1}\end{array}\right] =\left[\begin{array}{ccc}{f_x} & {0} & {c_x} \\ {0} & {f_y} & {c_y} \\ {0} & {0} & {1}\end{array}\right] \left[\begin{array}{c}{X } \\ {Y} \\ {Z}\end{array}\right] =KP$

把中間的量組成的矩陣稱爲相機的內參數矩陣（Camera Intrinsics） $K$。

除了內參之外，自然還有相對的外參。考慮到上式中，我們使用的是 $P$在相機座標系下的座標。由於相機在運動，所以 $P $的相機座標應該是它的世界座標（記爲 $P_w$），根據相機的當前位姿，變換到相機座標系下的結果。相機的位姿由它的旋轉矩陣 $R$ 和平移向量 $t$來描述。那麼有：
$ZP_{uv} = Z \left[\begin{array}{c} u \\v \\1\end{array}\right] = K (RP_W +t) = KTP_w$
相機的位姿 $R; t$又稱爲相機的外參數（Camera Extrinsics） 。相比於不變的內參，外參會隨着相機運動發生改變，同時也是 SLAM中待估計的目標，代表着機器人的軌跡.

3.5 畸變：

• 畸變形成的原因：
  爲了獲得好的成像效果，相機的前方一般都加了透鏡。對成像時光線的傳播會產生新的影響: 一是透鏡自身的形狀對光線傳播的影響，二是在機械組裝過程中，透鏡和成像平面不可能完全平行，光線穿過透鏡投影到成像面時的位置發生變化。

由透鏡形狀引起的畸變稱之爲徑向畸變。在針孔模型中，一條直線投影到像素平面上還是一條直線。可是，在實際拍攝的照片中，攝像機的透鏡往往使得真實環境中的一條直線在圖片中變成了曲線‹。越靠近圖像的邊緣，這種現象越明顯。由於實際加工製作的透鏡往往是中心對稱的，這使得不規則的畸變通常徑向對稱。它們主要分爲兩大類， 桶形畸變和枕形畸變。

桶形畸變是由於圖像放大率隨着離光軸的距離增加而減小，而枕形畸變卻恰好相反。
在這兩種畸變中，穿過圖像中心和光軸有交點的直線還能保持形狀不變。

除了透鏡的形狀會引入徑向畸變外，在相機的組裝過程中由於不能使得透鏡和成像面嚴格平行也會引入切向畸變。

平面上的任意一點 $p$可以用笛卡爾座標表示爲 $[x, y]^{T}$ , 也可以把它寫成極座標的形式
$[r; θ]^T$，其中 $r$ 表示點 $p$ 離座標系原點的距離， $θ$ 表示和水平軸的夾角。徑向畸變可看成座標點沿着長度方向發生了變化 $δr$, 也就是其距離原點的長度發生了變化。切向畸變可以
看成座標點沿着切線方向發生了變化，也就是水平夾角發生了變化 $δθ$。

對於徑向畸變，無論是桶形畸變還是枕形畸變，由於它們都是隨着離中心的距離增加
而增加。我們可以用一個多項式函數來描述畸變前後的座標變化：這類畸變可以用和距中
心距離有關的二次及高次多項式函數進行糾正：

$\begin{array}{l}{x_{\text {corrected}}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)} \\ {y_{\text {corrected}}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)}\end{array}$

其中 $[x, y]^{T}$ 是未糾正的點的座標，$[xcorrected; ycorrected]^T$ 是糾正後的點的座標，注意它們都是歸一化平面上的點，而不是像素平面上的點。在上式中，對於畸變較小的圖像中心區域，畸變糾正主要是 $k1$ 起作用。而對於畸變較大的邊緣區域主要是 $k2$ 起作用。普通攝像頭用這兩個係數就能很好的糾正徑向畸變。對畸變很大的攝像頭，比如魚眼鏡頭，可以加入 $k3$ 畸變項對畸變進行糾正。

另一方面，對於切向畸變，可以使用另外的兩個參數 $p1, p2$來進行糾正:

$\begin{array}{l}{x_{\text {corrected}}=x+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right)} \\ {y_{\text {corrected}}=y+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y}\end{array}$

對於相機座標系中的一點 $P(X; Y; Z)$，我們能夠通過五個畸變係數找到這個點在像素平面上的正確位置:

1. 將三維空間點投影到歸一化圖像平面。設它的歸一化座標爲 [x; y]T。
2. 對歸一化平面上的點進行徑向畸變和切向畸變糾正。
3. 將糾正後的點通過內參數矩陣投影到像素平面，得到該點在圖像上的正確位置。
1. 首先，世界座標系下有一個固定的點 P_w；
2. 由於相機在運動，它的運動由 R; t 或變換矩陣描述。 P 的相機座標爲：
P_c= R * P_w + t。
3. 這時的 P_c 仍有 X; Y; Z 三個量，把它們投影到歸一化平面 Z = 1 上，得到 P 的歸
一化相機座標： Pc = [X/Z; Y /Z; 1]T 。
4. 最後, P 的歸一化座標經過內參後，對應到它的像素座標： P_uv = K * P_c。

雙目座標

1.理想情況的雙目相機模型

$\frac{z-f}{z}=\frac{b-u_l+u_r}{b}$, 因爲$d=u_l-u_r$,
所以最後$z=\frac{f*b}{d}$

2. 真實的雙目相機模型

真實場景的雙目攝像機其實不是嚴格平行向前。所以需要雙目標定得到兩個相機之間的平移和旋轉矩陣。
參考：《視覺slam十四講》