位姿估计和座标系变换

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)$