VINS fusion软件架构分析（5）--- 坐标系转换

对于VINS代码的解读，其中一个重要的知识储备就是理解坐标系间的转换，这对于后面代码阅读非常重要，因此本章重点解释一下。
VINS中有3个坐标系：

• 世界坐标系 world
• IMU坐标系body
• 相机坐标系cam0（本文以单目为例）
  

SLAM是为了定位当前的位姿（旋转R，位移t），同时也要确定周边环境的位置（特征点的3D位置）。
相机在不断运动，他会观测到特征点。以一个特征点A为例子，特征点A在相机的第100帧被第一次观测到：

• VINS系统计算得到特征点A在第100帧的相机坐标系下的坐标a（ax_cam、ay_cam、az_cam）。
• 同时也可以计算出特征点A在世界坐标系下的坐标a’（ax_world、ay_world、az_world）。

这两个坐标是怎么关联起来呢？是通过变换矩阵T来计算的，转换矩阵就是用旋转矩阵和位移向量来构建的，a’=T * a，如下图所时。

在VINS中变换矩阵T分为两部分：

1. 从相机坐标系到IMU坐标系的变换矩阵T b_c，这个通过相机和IMU的外参（旋转外参RIC、平移外参TIC）来构建的。
2. 从IMU坐标系到世界坐标系的变换矩阵T w_b，通过当前的IMU位姿（旋转Rs，位移Ps）。

因此可以得到a’=T w_b * T b_c * a。
到了这一步，我一开始以为我就已经理解完全了，但是T w_b和T b_c具体是怎么用旋转和位移进行构建的？见下面两个图

这里重点解释一下，一开始我以为当前的IMU位姿（旋转Rs，位移Ps）是指从起始位置出发指向当前位置，发生的位姿变化，但是VINS代码中当前的IMU位姿（旋转Rs，位移Ps）是表示从当前位置指向起始位置，发生的位姿变化 ，这也可以从上面解释坐标系那种图的箭头方向可以看出来。