假设相机、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的位置