世界坐标系，图像坐标系，车体坐标系，雷达坐标系转换

许多场合下都需要用到坐标系的转换，那么坐标系转换中最重要最核心的就是RT矩阵，

                                                                                

R矩阵是将坐标系b旋转为坐标系a下的旋转矩阵，T为B坐标系在A坐标系下的坐标，如果要改变方向将A坐标系下的点转换为B坐标系下的点的话，要重新计算新的RT矩阵

                                                                             

                                                                     

从世界坐标系到相机图像的转换：
                                                                         

K矩阵是相机的内参数矩阵，基本计算理论是小孔成像，主要涉及到的参数是相机焦距，x,y轴的像素分辨率(pix/sel)，相机的主点

                                                                        

Px,Py就是相机主点坐标，f为焦距，NcNr为像素分辨率

需要注意的是R矩阵是一个正交矩阵，如果打乱旋转的顺序，并不会产生影响，只需要把R矩阵右乘的坐标顺序相应转换就行了：

R=Rx*Ry*Rz:对应的就是[x,y,z]

R=Ry*Rz*Rx:对应的就是[y,z,x]

x,y,z轴分别代表pitch,roll,azimuth

%x,y,z代表对应的角度
Rx=[1 0 0;0 cos(x) sin(x);0 -sin(x) cos(x)];
Ry=[cos(y) 0 -sin(y);0 1 0;sin(y) 0 cos(y)];
Rz=[cos(z) sin(z) 0;-sin(z) cos(z) 0; 0 0 1];

当图像被resize时，外参数不变，K矩阵的前两行才需要乘以一个系数：