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

本文链接：https://blog.csdn.net/weixin_38907330/article/details/82664975

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

$\begin{bmatrix} ax\\ ay\\ az \end{bmatrix}=R*\begin{bmatrix} bx\\ by\\ bz \end{bmatrix}+T$

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

$\begin{bmatrix} bx\\ by\\ bz \end{bmatrix}=R'*\begin{bmatrix} ax\\ ay\\ az \end{bmatrix}+T'$

$R'=inv(R); T'=-R'*T;$

从世界坐标系到相机图像的转换：
$a*\begin{bmatrix} c\\ r\\ 1 \end{bmatrix}=K*[R ,T']*\bigl(\begin{smallmatrix} X\\ Y\\ Z\\ 1 \end{smallmatrix}\bigr)$

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

$K=\begin{bmatrix} fNc &0 &NcPx \\ 0&fNr & NrPy\\ 0&0 &1 \end{bmatrix}\\ \\$

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矩阵的前两行才需要乘以一个系数：