//坐标系旋转,IMU坐标系下点变换到相机系,变换顺序先平移,后旋转
//Pcam为1*3行矩阵,T为1*3行矩阵,R为3*3矩阵
Pcam = (Pimu - T)*R;
//同理可推:已知相机坐标系点Pcam,求IMU坐标系下点Pimu
Pimu = Pcam*R.inv() + T;
//stereoRectify函数的R T定义
//@param R Rotation matrix from the coordinate system of the first camera to the second camera,
//@param R 从第一个相机到第二个相机坐标系系统的旋转矩阵
//@param T Translation vector from the coordinate system of the first camera to the second camera,
//@param T 从第一个相机到第二个相机的坐标系的平移矩阵
//代码中以左相机为基,RT为左相机变换到右相机
//推理过程
//左相机点P1在IMU坐标系下坐标Pimu
Pimu = P1*R1.inv() + T1
//IMU坐标系下坐标Pimu在右相机坐标系下点P2
P2 = (Pimu - T2) * R2
//故P1变换到P2(相机1的点变换到相机2)过程为
P2 = (P1*R1.inv() + T1 - T2)*R2
//推导可得
P2 = P1*R1.inv()*R2 + (T1 - T2)*R2
R = R1.inv()*R2
T = (T1 - T2)*R2
激光外标定参数:
相机外标定参数
R
激光绕自身轴旋转到IMU坐标系
zyx
T
激光中心在IMU坐标系下坐标
//激光坐标系点到IMU系,先旋转,后平移
Pimu = Pldr * R + T
//IMU坐标系点到激光系
Pldr = (Pimu - T)* R.inv()