传感器标定 坐标系旋转

坐标系定义：

	坐标系定义
IMU系	右手系(右前上)	右x正，前y正，上z正
相机系	右手系(右前上)	右x正，下y正，前z正

相机外标定参数：

相机外标定参数
R	IMU绕自身轴旋转到相机坐标系	zyx
T	相机光心在IMU坐标系下坐标

相机坐标系变换：

//坐标系旋转，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()