如何将相机坐标系得到的位姿转换到世界系下机体的位姿

        在做定位过程中遇到一个比较简单也很基础的问题，那就是如何将相机（或者其他传感器）坐标系下得到的位置和姿态转换到在世界坐标系下机体的位姿。

        上述表述多少有些绕，这是一个很有用的实际问题，比如说在做vins-mono的时候，将imu和camera作为一个独立的模块安装在机器人上，这个模块里程计部分输出的是Rwi和twi，然而这个w实际上是这个模块第一帧位姿所在的坐标系，（为了下面更好讲解，将上面的位姿替换成Rvm-i和tvm-i）而实际安装过程并不一定把这个模块安装在机体的中心位置，这个模块和车体中心存在一个旋转和平移，那么如何根据该模块计算出的旋转和平移转化成机体的位姿呢？

        首先，由于定位模块和机器人是刚性连接，也就是说定位模块发生旋转机体也会发生同样的旋转因此，Rvm-i = Rwi这里的w代表的是真正的世界坐标系。关于世界坐标系的定义通常是机体中心最初始的位置为坐标原点，然后根据车体实际需求确定坐标轴。

        位置的坐标可以由以下公式得到：

也可以通过几何方法得到关于位置的计算，如下图：

        左边为机体最初始时刻所在坐标系，当做世界系，右上为机体运动一段时间后到达新的位置，b为机体坐标系，vm为定位模块所在坐标系。根据上图可以得到以下几何关系：

        在上式中，计算出的OO'向量就是机体在世界坐标系下的位置，世界坐标系下的OA向量和集体坐标系下的O'A'向量各分量值相等，考虑一种常见的特殊情况：二维平面下运动，并且定位模块坐标系和机体坐标系重合，那么从vm系到w系坐标变换是个单位阵，而从b系到w系的坐标变换是只和偏航角相关的旋转矩阵。