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

位姿的定义

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

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

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

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

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

$p_w=T_1*p_c$
$p_c=T_2*p_w$

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

$p_w=Twc*p_c$
$p_c=Tcw*p_w=Twc^{-1}*p_w$

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

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

由定义可知，位姿本质上是变换矩阵，特指世界坐标系与相机坐标系之间的变化关系。
而变换矩阵是任意两个空间（坐标点）之间的欧式变换关系。
视觉SLAM理论——位姿的理解与间接求解
图中的Ti为位姿，Tij为变换矩阵，分别表达了不同坐标系之间的变换。

位姿的求解方法

这里讨论的是位姿的一种间接求解方法，不同于重投影误差优化，对极约束等直接求解方法，这类求解方法常常是作为位姿直接求解方法的初始值。关于直接求解位姿的方法可参考前面写的一系列文章。
视觉SLAM理论——位姿的理解与间接求解
如图所示，结合上面的基本推倒，有以下式子成立：

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

利用前面两式进行代换：

$T_2*p_w=T_{12}*T_1*p_w$

约去 $p_w$ 得：

$T_{12}=T_2T_1^{-1}$
$T_2=T_{12}*T_1$

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

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

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

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

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

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

目录：

位姿的定义

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

位姿的求解方法

相关推荐