视觉SLAM理论——位姿的理解与间接求解

目录：

• 位姿的定义
• 位姿与变换矩阵的区别与联系
• 位姿的求解方法

位姿的定义

  在SLAM中，位姿是世界坐标系到相机坐标系的变换，包括旋转与平移。根据以上定义可以衍生以下几个问题：

1.世界坐标系在哪？
通常世界坐标系是自己定义的，一经定义，便不可更改，通常构图所用点的坐标便是世界坐标系下的坐标。

2.相机坐标系在哪？
相机坐标系是指以相机的光心为原点所构成的坐标系，由于相机是运动的，所以相机坐标系也是运动的。

3.如何表达位姿？
位姿通常以三维空间中的欧式变换来表示，变换矩阵T最常用，也可以分别用旋转R和平移向量t来表示。因为相机坐标系是运动的，所以位姿也是变换的。

  设一点$P$在世界坐标系下的坐标为$p_w$，相机坐标系下的坐标为$p_C$，根据位姿的定义有：

$p_w=T_1\ast p_c$
$p_c=T_2\ast p_w$

  通常把$T_1,T_2$记作$Tcw$与$Twc$,其中$T_1,T_2$是互为逆矩阵，因此有：

$p_w=Twc\ast p_c$
$p_c=Tcw\ast p_w=Tw{c}^{-1}\ast p_w$

  $Twc,Tcw$是位姿的两种表达方式，实际过程中根据情况自己选择。

位姿与变换矩阵的区别与联系

  由定义可知，位姿本质上是变换矩阵，特指世界坐标系与相机坐标系之间的变化关系。
  而变换矩阵是任意两个空间（坐标点）之间的欧式变换关系。

  图中的Ti为位姿，Tij为变换矩阵，分别表达了不同坐标系之间的变换。

位姿的求解方法

  这里讨论的是位姿的一种间接求解方法，不同于重投影误差优化，对极约束等直接求解方法，这类求解方法常常是作为位姿直接求解方法的初始值。关于直接求解位姿的方法可参考前面写的一系列文章。

  如图所示，结合上面的基本推倒，有以下式子成立：

$p_{c1}=T_1\ast p_w$
$p_{c2}=T_2\ast p_w$
$p_{c2}=T_{12}\ast p_{c1}$

  利用前面两式进行代换：

$T_2\ast p_w=T_{12}\ast T_1\ast p_w$

  约去$p_w$得：

$T_{12}=T_2{T}_1^{-1}$
$T_2=T_{12}\ast T_1$

  $T_{12}$是一类特殊的变换矩阵，表达的是相机之间的运动，并且跟位姿有着非常密切的联系。在实际的使用过程中，通常可以按照以下步骤进行使用：

1.利用初始化算法得到$T_1,p_{c1}$
2.设定一个初始值$T_{12}$
3.令$T_2=T_{12}\ast T_1,p_{c2}=p_{c1}$，得到一个初步的$T_2$与$p_{c2}$
4.利用重投影误差重新估计$T_2$与$p_{c2}$
5.重新更新$T_{12}$
6.令$T_3=T_{12}\ast T_1,p_{c3}=p_{c2}$，得到一个初步的$T_3$与$p_{c3}$…依次类推求得每一个$T_{ij}，p_{ci}$

  经过1234,一帧图像的优化过程便结束了,后面是进行连续的图像追踪与优化。

  值得注意的是，我们可以把世界坐标系的原点与相机的原点重合，这样得到的$T_{ij}$会更特殊，会直接包含位姿中的平移信息。

参考文献：
[1]视觉SLAM十四讲
[2]https://blog.csdn.net/cnn_block/article/details/88790909