功能说明:
功能: 求cam2相机坐标系(3D)到IMU坐标系(3D)的变换Tbc (输出, 即T_body_cam2)
输入:
- T_velo_imu: IMU到激光雷达的变换矩阵
- T_cam_velo: 激光雷达到cam0的变换矩阵
- R_i_rect: 从cam0到cami的旋转矩阵
- P_i_rect: 由cami的内参, 和 cami相对于cam0的位移组成的投影矩阵(3D->2D)
输出：
- T_imu_cam2_new: cam2相机坐标系(3D)到IMU坐标系(3D)的变换
- T_imu_cam2: cam2图像坐标系(2D)到IMU坐标系(3D)的变换
公式来源: 论文 Vision meets robotics: the KITTI dataset
- y为图像i中的像素点2D坐标，x为IMU坐标系下的3D坐标
-
T_cam2_imu = P_2_rect_real * R_0_rect * T_cam_velo * T_velo_imu
- T_cam2_imu_new = T_cam2_cam0 * T_c
