vins中的坐标系变换及g2r函数
slam中经常会需要表示一个刚体的位姿。例如imu的位姿，相机的位姿。首先我们需要在一个刚体上架上一个坐标系。这个坐标系为本体坐标系。
怎么架一个坐标系？imu本身就有规定其本身的x,y,z轴的方向。相机一般认为，z轴平行于镜头朝外，x轴与像平面的水平方向一致，从左到右，y轴与像平面垂直方向一致，从上到下。
现在我们以这个cam坐标系作为参考，解算出了imu坐标系在cam坐标系下的位姿，为 R ，t
t 代表了平移向量，为imu坐标系原点在cam坐标系的位置
R代表的含义可以有三种解释：
- 字面含义：R的三个列向量为imu系 x,y,z轴在原坐标系的表示
- 表示Cam坐标系如何旋转才能转到IMU坐标系 --------- 坐标系的变换
- 表示imu坐标系中的点通过这个旋转矩阵变换到w坐标系中去 --------- 点的变换
关于第一种解释比较容易理解。
第二种解释的一个例子为已知此时Cam系在世界坐标系中，此时的姿态为R_cam，则imu在世界坐标系中，姿态为 R_cam * ????
第三种解释的一个例子为已知此时Imu测得的角速度为w=[????1,????2,????3]，则在cam系中，角速度为???? ????
网上很多可能告诉的是点的变换是左乘，坐标系的变换是右乘。其实也不是固定的。坐标系的变换也可以使左乘。绕着旋转轴的是右乘，绕着固定轴的左乘。
在vins 的配置文件中，需要填写相机和imu之间的外参。注解中特别说明，这个外参是相机帧变换到imu帧，注意，这个变换是点的变换。也就是这个旋转外参是以imu为参考帧，相机的姿态r。平移外参是以imu为参考帧，cam的光心在imu坐标系中的位置。vins程序中是以ric,tic来表示这组外参。
vins坐标系变换中，困扰比较久的是一个g2r（const Eigen::Vector &g）函数。这个函数主要是为了做一个重力对齐。
在初始化进行到把重力估计出来后，我们假设，现在已知解算出了在第一帧Imu帧下，重力向量为 g =[-9.4 −2.3 0.4]，这时候第一帧imu大概是这个样子：
我们假设解算都是以第一帧imu帧为参考系（实际上初始化是以第一帧图像帧为参考系，不过不影响）。
为了使得参考坐标系和重力对齐。我们期望的参考坐标系应该是：
因此vins中有两步操作：
这里可能有两个问题：
1.为什么是[0 0 9.8]而不是[0 0 -9.8] ？因为vins的公式推算中，g都是用-号，因此这个相当于是一个朝上的重力.
2.为啥是R1R0 而不是R0R1？因为这个旋转是绕着固定轴旋转的，所以使用左乘。
注意，vins中使用了g2r后又使用了一次yaw的调整，主要是因为他是以C0为参考系。又进行一次调整保证最初是的imu帧变换后，yaw角为0.