相机、IMU、世界坐标系的转换关系

假设相机、IMU、世界坐标系的结构如下：

其中：

C1：摄像头从初始状态（即单位阵I，这个单位阵是指摄像头开机的时候当做起始状态）到T1时刻的转换为C1；
C2： 摄像头从初始状态（即单位阵I） 到T2时刻的转换为C2 ；
Rc：从T1时刻的状态转换到T2时刻的状态所需要的转换（旋转）矩阵；
M1：imu初始状态（即单位阵，这个单位阵是指imu与世界坐标系重合时为单位阵。）到T1时刻的转换为M1；
M2： imu初始状态（即单位阵，这个单位阵是指imu与世界坐标系重合时为单位阵。）到T2时刻的转换为M2；
Rm：从T1时刻的状态转换到T2时刻的状态所需要的转换（旋转）矩阵；
R：imu自身的坐标系到与摄像头的坐标系之间存在的转换。
 

注意：我们在级进行相机和IMU的联合标定时，会给一个初始的旋转矩阵R，和平移向量t； 但是这两个变量不准确，需要进行优化，具体的优化原理参考：

https://blog.csdn.net/liweibin1994/article/details/53704140/

 

假设：

相机坐标系下，IMU的位置（平移）： camera_.p_I_C

世界坐标系的一个点P的位置： p_f_G

世界坐标系下IMU的位置：imu.p_I_G

相机坐标系到世界坐标系的转换矩阵： R_CG             //  注意：坐标系到坐标系的矩阵和 坐标系上的点到另一个坐标系的矩阵，是相反的

求：相机坐标系下的P的位置 p_f_C 的 计算 ？

 

p_f_C = R_CG *  (p_f_G - imu.p_I_G) + camera_.p_I_C;

 

理解：

1、 (p_f_G - imu.p_I_G)； 先求出 世界坐标系下，   IMU到P的相对位置；

2、R_CG *  (p_f_G - imu.p_I_G) ； 在求，相机坐标系下，  IMU到P的相对位置；

3、 R_CG *  (p_f_G - imu.p_I_G) + camera_.p_I_C;  相机坐标系下，  IMU到P的相对位置，加上 相机坐标系下，IMU的位置， 便可以得到 ：相机坐标系下，P的位置