坐标系的旋转

1、简单3维坐标系旋转平移

原始坐标系下坐标点p1(0,2,1)。原始坐标系绕自身z轴旋转-90度变换为新坐标系，p1在新坐标系下的坐标点为p2（-2,0,1）

得出结论：

求原始点在目标坐标系下的坐标，需要将原始坐标系绕自身3轴旋转与目标坐标系轴向重合，得到旋转矩阵R，求得原始坐标系原点在目标坐标系下的3维坐标T(△x,△y,△z)，那么p1在目标坐标系下的坐标点p2 = p1 * R + T;

实战演练（控制点投影影像）：

前提1、相机外标定参数定义Tx,Ty,Tz相机在IMU坐标系下的偏移量(坐标点)，Rx，Ry，Rz分别是相机绕自己轴旋转到与IMU坐标轴重合的角度（后期计算IMU点在相机系下的坐标时先平移，平移过程需要减去坐标原点的值，后旋转）

前提2、标定场有控制点p1在北东高下的坐标（e,n,u）和当前的投影度带号

前提3、相机的内外参已知，相机中心在IMU坐标系下的坐标为Tcam2imu,相机旋转到IMU坐标系下的旋转矩阵为Rcam2imu

前提4、惯导当前的经纬度lng，lat，h，roll，pitch，heading已知

思路：

1、求控制点在ecef下的坐标p1

2、求出ecef坐标系到当地水平坐标系的偏移量，通过当前IMU的经纬度计算得到IMU中心点的ecef坐标即Tsp2ecef

3、求出ecef坐标系到当地水平系的旋转矩阵，通过当前IMU的经纬度计算ecef旋转到当地水平坐标系的旋转矩阵Recef2sp

4、求p1在当地水平坐标系下的点p2 = (p1 - Tsp2ecef) * Recef2sp

5、求当地水平坐标系到载体坐标系的旋转矩阵Rsp2body（这里重点提到旋转矩阵内heading取负值是因为，IMU输出的heading定义东偏为正，但是右手系旋转法则是逆时针为正（西偏为正，东偏为负）所以在构建旋转矩阵时要对heading取反（-heading），同时旋转矩阵旋转角度范围（-180,180）和航向范围（0-360）在旋转上矩阵值是一样的，不用再将范围统一，直接使用即可），Rsp2body=getMatrixByAngle（pitch，roll，-heading，zxy）

6、求p2在载体系的点p3 = p2 * Rsp2body

7、构建相机在载体系下的平移Tcam2body（Tx,Ty,Tz）带符号,构建相机旋转到载体系下的旋转矩阵Rcam2body=getMatrixByAngle（Rx，Ry，Rz，zxy）

8、求p3在相机系下的点p4 = (p3 - Tcam2body) * Rcam2body.inverse()（求逆矩阵）;(旋转矩阵的转置transpose和逆inverse是一样的)

9、求p4在图像坐标系下的点p5  cv::projectPoints