位姿估计和坐标系变换

SLAM是一个“鸡生蛋和蛋生鸡”的问题，要定位需要重建，一般通过当前sensor看到到场景跟建好的地图进行匹配确定自身的位置。简单的例子：比如你在平面上，别人问你的坐标，那么很显然你得先有坐标系。要重建又需要精确的定位信息，如果没有相机位姿，那么当前帧数据无法统一注册到世界坐标系下。

在SLAM中，所谓的位姿其实指的是相机在世界坐标系中的位姿。位姿包括两方面：位置和姿势，即三维坐标和朝向。如下所示，建图的过程就需要知道每一刻相机的位姿，从而将当前相机捕获的点云注册到全局的点云模型中。

常用的变换有：世界坐标系 -> 相机坐标系 和 相机的位姿 -> 世界坐标系
如下所示：世界坐标系为$wxy$, 相机坐标系为$c{x}^{’}{y}^{’}$， $P$在世界坐标下的坐标为$(a,b)$, $P$在相机坐标系下的坐标为$(a^{’}{b}^{’})$。

(1) 已知相机坐标系在世界坐标系的位姿为：$T_{cw}$, 世界坐标中的点 $P_w$, 那么相机坐标系的坐标为$P_c=T_{cw}^{-1}{P}_w$
(2) 已知相机坐标系在世界坐标系的位姿为：$T_{cw}$, 相机坐标中的点 $P_c$, 那么世界坐标系的坐标为$P_w=T_{cw}{P}_c$

$T_{cw}$和 $T_{cw}^{-1}$均可作为相机位姿， 主流的如ORBSLAM采用后者作为相机的位姿。

可以检验一下：
(1）只包含平移，相机坐标系在世界坐标下只有平移，平移向量为$(2,2)$， 那么$T_{cw}=\left[\begin{array}{ccc}1& 0& 2\\ 0& 1& 2\\ 0& 0& 1\end{array}\right]$， $T_{cw}^{-1}=\left[\begin{array}{ccc}1& 0& -2\\ 0& 1& -2\\ 0& 0& 1\end{array}\right]$
已知世界坐标系中的坐标为 $P_w(3,3)$, 转换到相机坐标系下为： $P_c=T_{cw}^{-1}{P}_w=\left[\begin{array}{ccc}1& 0& -2\\ 0& 1& -2\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}3\\ 3\\ 1\end{array}\right]=\left[\begin{array}{c}1\\ 1\\ 1\end{array}\right]$。因此，相机坐标系下的坐标$P_c=(1,1)$
反之，已知相机坐标系下的坐标 $P_c(1,1)$, 转换到世界坐标系下为： $P_w=T_{cw}{P}_c=\left[\begin{array}{ccc}1& 0& 2\\ 0& 1& 2\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}1\\ 1\\ 1\end{array}\right]=\left[\begin{array}{c}3\\ 3\\ 1\end{array}\right]$, 因此，世界坐标系下的坐标$P_w=(3,3)$

(2)只包含旋转， 相机坐标系在世界坐标系中逆时针旋转了$180°$, 那么位姿矩阵$T_{cw}=\left[\begin{array}{ccc}-1& 0& 0\\ 0& -1& 0\\ 0& 0& 1\end{array}\right]$, $T_{cw}^{-1}=\left[\begin{array}{ccc}-1& 0& 0\\ 0& -1& 0\\ 0& 0& 1\end{array}\right]$,

已知世界坐标系中的坐标为 $P_w(3,3)$, 转换到相机坐标系下为$P_c=T_{cw}^{-1}{P}_w=(-3,-3)$
反之，相机坐标下的坐标为$P_c(-3,-3)$, 转换到世界坐标系下为$P_w=T_{cw}{P}_c=(3,3)$

(3)既包含旋转又包含平移，先逆时针旋转$180°$， 然后平移$(2,2)$, 因此$T_{cw}=\left[\begin{array}{ccc}0& -1& 2\\ 1& 0& 2\\ 0& 0& 1\end{array}\right]$, $T_{cw}^{-1}=\left[\begin{array}{ccc}0& 1& -2\\ -1& 0& 2\\ 0& 0& 1\end{array}\right]$,
已知世界坐标系中的坐标为 $P_w(2,2)$, 转换到相机坐标系下为$P_c=T_{cw}^{-1}{P}_w=(0,0)$
已知世界坐标系中的坐标为 $P_w(3,3)$, 转换到相机坐标系下为$P_c=T_{cw}^{-1}{P}_w=(1,-1)$
反之，已知相机坐标系中的坐标为 $P_c(0,0)$, 转换到相机坐标系下为$P_w=T_{cw}{P}_c=(2,2)$