相機投影變換(位姿)

相機投影變換(位姿).md
針對SLAM新手中常見的對各種變換的迷惑，我這裏答疑一下。

針孔相機模型

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-cPyc6VkK-1587194378847)(:/8acde09162b7499bb062e947abb2db78)]

其中:

• $M_{ext}$是等距變換，也就是歐式剛體變換
• $M_{proj}$是投影變換（攝影變換/透視變換），表達了空間中的一個正方形線框投影到平面上是什麼形狀。
• $M_{aff}$是一個仿射變換，但是實際上其本質我認爲是一個相似變換+一個翻轉形成的

參考
圖像變換——等距變換，相似變換，仿射變換，投影變換
仿射變換與投影變換
攝像機模型

什麼是相機位姿，投影矩陣，歐式變換矩陣

相機位姿T_{wc}：4*4矩陣，即能夠將相機座標系下3D點$P_c$變換爲世界座標系下點$P_w$，注意它不是投影矩陣，變換公式爲:
$P_w=T_{wc}\times{P_c} \\ p_w=R_{wc}\times{p_c} \\ t^{pw}=t^{pc}+t^{cw}$

位姿的三種等價解釋：

• 一種是位姿指的是Twc，可以將相機座標系下的點變換到世界座標系下的點
• 一種是位姿指的是相機座標系的原點在世界座標系下的3D座標
• 一種是把世界座標系變換爲相機座標系這個過程對應的那個變換

注：ORBSLAM中的Tcw代表將一個點從世界座標變換到相機座標，這不是相機位姿，而是相機位姿的逆
注意分清楚對一個3D點的變換，以及對座標系的變換，兩者敘述剛好相反。

下面是論文中常見的使用表述

兩個座標系F1，F2之間的歐式變換：指的是將新的座標系F2中的點變換爲原來的舊的座標系F1中點的座標，標記爲$T_{12}$

座標系F1到F2的歐式變換： 指的是將座標系F1中的點變換到F2中的點

Sensor extrinsics wrt. the body frame: 指的是該外參將sensor座標系下的3D點變換到Body座標系下的3D點

$T^a_b$: 表示將b座標系中的3D點變換到a座標系中的3D點

投影矩陣

投影矩陣 ： 一般的投影矩陣被廣泛認爲是相機內參乘以從世界座標系變爲相機座標系的3*4矩陣，得到的一個3*4的矩陣，其可以把一個世界座標系下的點直接變爲像素座標，（但是我下面論述的時候有可能會把世界座標系變換爲相機座標系的3*4矩陣叫做投影矩陣，最終得到相機座標系下的3D點座標（非歸一化相機座標系），注意區分，這兩種叫法都有，一般以第一種包含內參K的叫法爲準）

投影矩陣公式：

$p_{uv}=KT\times{p_w}=P_{proj}\times{p_w} \\$
展開爲:

$\begin{bmatrix} p_{pixel-x} \\ p_{pixel-y} \\ 1 \end{bmatrix} =\left[ \begin{array}{ccc} fx & 0 & cx \\ 0 & fy & cy \\ 0 & 0 & 1 \end{array} \right] \times{} \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_1 \\ r_{21} & r_{22} & r_{23} & t_2 \\ r_{31} & r_{32} & r_{33} & t_3 \\ \end{bmatrix} \times{} \begin{bmatrix} P_{wx} \\ P_{wy} \\ P_{wz} \\ 1 \end{bmatrix} =P_{proj}\times{} \begin{bmatrix} P_{wx} \\ P_{wy} \\ P_{wz} \\ 1 \end{bmatrix}$

順着雙目相機看的方向，左眼爲左目，右眼爲右目。
假設現在雙目相機左右目已經校正過了，光心平行，現在以左目座標系爲世界座標系，現有一世界座標系下點
$P=[p_{wx},p_{wy},p_{wz}]^T$，
（注意：令將點從左目座標系變換到右目座標系的平移矩陣t={-30,0,0},所以在左目座標系下，右目座標系相對於左目座標系的基線baseline=30m，即右目在左目的X軸正方向30m處。）
則將該點P變換到右目像素座標系的像素座標，即右目座標系的投影矩陣爲：

$\begin{bmatrix} p_{pixel-x} \\ p_{pixel-y} \\ 1 \end{bmatrix} =\left[ \begin{array}{ccc} fx & 0 & cx & -baseline*fx\\ 0 & fy & cy & 0\\ 0 & 0 & 1 & 0 \end{array} \right] \times{} \begin{bmatrix} P_{wx} \\ P_{wy} \\ P_{wz} \\ 1 \end{bmatrix}$

投影矩陣與相機位姿： 投影矩陣是3*4矩陣，指的是將世界座標系中的3D點變爲像素座標系中的2D點，相機位子是將相機座標系下的3D點變爲世界座標系下的3D點，有一點互逆的關係，但是投影矩陣比相機位姿多了一個內參矩陣K的值

世界座標系下“3D座標” 轉 像素座標系下“像素座標”

投影公式爲：
$P_{uv}=KTP_{w}$
其中：

• $P_{uv}$是一個3維列向量，前兩維爲像素座標u和v，是二維齊次座標。
• $K$矩陣爲3*3矩陣，就是相機內參數矩陣
• $T$矩陣爲34矩陣，爲從世界座標投影到相機座標系的投影矩陣，也是該相機此時刻的位姿，左邊33維爲相對於最初始相機位姿旋轉矩陣，右邊的3*1維爲平移向量
• $P_{w}$爲場景點在世界座標系下的3D位置，是4*1向量，前三個元素爲X，Y，Z軸座標，最後一個爲1，是三維齊次座標。
• KT合起來叫做相機的投影矩陣，可以直接將世界座標系變爲像素座標系

注，如果有畸變的話，那上述uv值爲去畸變後的uv值，之後計算過程一致

展開爲:
$\left[ \begin{array}{c} u \\ v \\ 1 \end{array} \right]= \left[ \begin{array}{ccc} fx & 0 & cx \\ 0 & fy & cy \\ 0 & 0 & 1 \end{array} \right] \times{} \left[ \begin{array}{cccc} 1 & 0 & 0 & 7\\ 0 & 1 & 0 & 8\\ 0 & 0 & 1 & 9 \end{array} \right] \times{} \left[ \begin{array}{c} X \\ Y \\ Z \\ 1 \end{array} \right]$

純粹兩個座標系之間的單一變換

這裏假設我們要把一個在相機座標系中的3D點變換到世界座標系中的3D點
$P_w=T_{wc}\times{}P_c$
旋轉矩陣變換爲：
$p_w=R_{wc}\times{}p_c$
平移向量變換爲：從相機座標系原點指向世界座標系原點。

其中：

• $P_c$爲相機座標系下3D空間點座標
• $T_{wc}$爲位姿變換矩陣，指的是從相機座標系到世界座標系之間的變換，也就是相機位姿。注意其3*4部分仍然不是投影矩陣！！！
• $P_w$爲世界座標系下3D空間點座標

展開爲：

$\begin{pmatrix} P_{wx} \\ P_{wy} \\ P_{wz} \\ 1 \end{pmatrix} =\begin{pmatrix} r_{11} & r_{12} & r_{13} & t_1 \\ r_{21} & r_{22} & r_{23} & t_2 \\ r_{31} & r_{32} & r_{33} & t_3 \\ 0& 0 & 0 & 1 \end{pmatrix} \times{} \begin{pmatrix} P_{cx} \\ P_{cy} \\ P_{cz} \\ 1 \end{pmatrix}$

約定：$T_{wc}代表一個能將3D點從相機座標系轉換爲世界座標系下的3D點，這樣的一個的變換矩陣$

多重位姿變換

現在有:

• 世界座標系下點$P_{w}=(P_{wx},P_{wy},P_{wz},1)$，
• 相機座標系F1對應的相機位姿$T_1$,以及F1下$P_{w}$對應的相機座標系座標$P_{c1}$
• 相機座標系F2對應的相機位姿$T_2$,以及F1下$P_{w}$對應的相機座標系座標$P_{c2}$
• 座標系F1到F2之間的歐式變換$T_{12}$，即將心座標系F2中的點變換到F1中的點，
  滿足：
  $P_{w}=T_1*P_{c1} \\ P_{c1}=T_{12}*P_{c2} \\ \quad得到: \\ P_{w}=T_{12}*T_1*P_{c2}$

相機位姿就是當前相機座標系和世界座標系之間的歐式變換矩陣，這個矩陣將點從當前相機座標系變換爲世界座標系中的點

初始相機位姿爲4*4單位矩陣，代表世界座標系經過單位矩陣變換仍然是同一個變換矩陣，其中世界座標系與初始的相機朝向有一個固定關係，比如一般定義爲Z軸正方向爲相機看的方向。從而建立相機看的方向與歐式變換矩陣真正的關聯

相機座標系下“相機座標” 轉 像素座標系下“像素座標”

座標轉換公式爲：
$Z\times{}P_{uv}=K\times{}P_{c} \\ 即 ZP_{uv}=KP_{c}$
其中：

• $P_{uv}$爲一個三維列向量，前兩維爲像素座標u和v，是二維齊次座標
• Z爲相機座標系下3D點的Z座標
• K矩陣爲3*3相機內參矩陣
• $P_{c}$爲相機座標系下的3D座標

展開爲：

$Z\begin{pmatrix} u \\ v \\ 1 \end{pmatrix} =\begin{pmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{pmatrix} \times{} \begin{pmatrix} X \\ Y \\ Z \end{pmatrix}$