一、坐标系之间的转换
一个有4个坐标系：图像坐标系、像素坐标系、相机坐标系、世界坐标系。
1.图像坐标系和像素坐标系之间的转换
图像坐标系和像素坐标系在同一个平面，利用平面坐标系之间的转换关系可以之知道两个坐标系变换的公式，并且该坐标系变换只存在平移和缩放。
平面坐标系之间的转换关系参考：https://blog.csdn.net/a549742320/article/details/116127701
故两个坐标系之间的变换满足 u=x/dx+u0 ,v=y/dy+v0。其中，dx，dy表示像素坐标系中每个像素点的宽和高，而图像坐标系原点在像素坐标系中的横纵坐标分别为 u0,v0。
2.图像坐标系和相机坐标系之间的转换
利用小孔成像和相似三角形的原理得到转换关系。
M为向极坐标系上的点，P为图像坐标系上的点。根据相似三角形原理：Xc/x=Yc/y=Zc/f，可以写成如下的矩阵形式。
3.相机坐标系到世界坐标系之间的转换
通过刚体的旋转和平移的原理得到公式。
R旋转矩阵利用分别绕3个坐标轴对roll、yaw、pitch、进行变换。
T为平移矩阵。
