位姿估計和座標系變換

SLAM是一個“雞生蛋和蛋生雞”的問題，要定位需要重建，一般通過當前sensor看到到場景跟建好的地圖進行匹配確定自身的位置。簡單的例子：比如你在平面上，別人問你的座標，那麼很顯然你得先有座標系。要重建又需要精確的定位信息，如果沒有相機位姿，那麼當前幀數據無法統一註冊到世界座標系下。

在SLAM中，所謂的位姿其實指的是相機在世界座標系中的位姿。位姿包括兩方面：位置和姿勢，即三維座標和朝向。如下所示，建圖的過程就需要知道每一刻相機的位姿，從而將當前相機捕獲的點雲註冊到全局的點雲模型中。

常用的變換有：世界座標系 -> 相機座標系 和 相機的位姿 -> 世界座標系
如下所示：世界座標系爲$wxy$, 相機座標系爲$cx^’y^’$， $P$在世界座標下的座標爲$(a,b)$, $P$在相機座標系下的座標爲$(a^’b^’)$。

(1) 已知相機座標系在世界座標系的位姿爲：$T_{cw}$, 世界座標中的點 $P_w$, 那麼相機座標系的座標爲$P_c = T^{-1}_{cw}P_w$
(2) 已知相機座標系在世界座標系的位姿爲：$T_{cw}$, 相機座標中的點 $P_c$, 那麼世界座標系的座標爲$P_w = T_{cw}P_c$

$T_{cw}$和 $T^{-1}_{cw}$均可作爲相機位姿， 主流的如ORBSLAM採用後者作爲相機的位姿。

可以檢驗一下：
(1）只包含平移，相機座標系在世界座標下只有平移，平移向量爲$(2,2)$， 那麼$T_{cw} = \begin{bmatrix} 1 & 0 & 2\\ 0 & 1 & 2\\ 0 & 0 & 1 \end{bmatrix}$， $T^{-1}_{cw} = \begin{bmatrix} 1 & 0 & -2\\ 0 & 1 & -2\\ 0 & 0 & 1 \end{bmatrix}$
已知世界座標系中的座標爲 $P_w(3,3)$, 轉換到相機座標系下爲： $P_c = T^{-1}_{cw} P_w = \begin{bmatrix} 1 & 0 & -2\\ 0 & 1 & -2\\ 0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} 3 \\ 3 \\ 1 \end{bmatrix} = \begin{bmatrix} 1 \\ 1 \\ 1 \end{bmatrix}$。因此，相機座標系下的座標$P_c = (1,1)$
反之，已知相機座標系下的座標 $P_c(1,1)$, 轉換到世界座標系下爲： $P_w = T_{cw} P_c = \begin{bmatrix} 1 & 0 & 2\\ 0 & 1 & 2\\ 0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} 1 \\ 1 \\ 1 \end{bmatrix} = \begin{bmatrix} 3 \\ 3 \\ 1 \end{bmatrix}$, 因此，世界座標系下的座標$P_w = (3,3)$

(2)只包含旋轉， 相機座標系在世界座標系中逆時針旋轉了$180\degree$, 那麼位姿矩陣$T_{cw} = \begin{bmatrix} -1 & 0 & 0\\ 0 & -1 & 0\\ 0 & 0 & 1 \end{bmatrix}$, $T^{-1}_{cw} = \begin{bmatrix} -1 & 0 & 0\\ 0 & -1 & 0\\ 0 & 0 & 1 \end{bmatrix}$,

已知世界座標系中的座標爲 $P_w(3,3)$, 轉換到相機座標系下爲$P_c = T^{-1}_{cw} P_w = (-3,-3)$
反之，相機座標下的座標爲$P_c(-3,-3)$, 轉換到世界座標系下爲$P_w = T_{cw}P_c = (3,3)$

(3)既包含旋轉又包含平移，先逆時針旋轉$180\degree$， 然後平移$(2,2)$, 因此$T_{cw} = \begin{bmatrix} 0 & -1 & 2\\ 1 & 0 & 2\\ 0 & 0 & 1 \end{bmatrix}$, $T^{-1}_{cw} = \begin{bmatrix} 0 & 1 & -2\\ -1 & 0 & 2\\ 0 & 0 & 1 \end{bmatrix}$,
已知世界座標系中的座標爲 $P_w(2,2)$, 轉換到相機座標系下爲$P_c =T^{-1}_{cw} P_w = (0,0)$
已知世界座標系中的座標爲 $P_w(3,3)$, 轉換到相機座標系下爲$P_c =T^{-1}_{cw} P_w = (1,-1)$
反之，已知相機座標系中的座標爲 $P_c(0,0)$, 轉換到相機座標系下爲$P_w =T_{cw} P_c = (2,2)$