1、世界坐标系一点P(Xw,Yw,Zw)转换到摄像机坐标系为(Xc,Yc,Zc)
其中R表示旋转矩阵，T表示平移矩阵
2、摄像机坐标系点(Xc,Yc,Zc)映射到图像坐标系的p(x,y)转换关系用到了透视投影
f为相机的焦距
转换为齐次坐标形式为
3、图像坐标系p(x,y)转换为像素坐标系(u,v),其中u,v表示像素点的行列数，dx和dy表示垂直和水平上每毫米有多少个像素，也就是每毫米有多少行像素和多少列像素，我们以(u,v)来表示图像上像素的列标和行标，这个是以像素为单位的图像坐标系中的坐标，坐标系的圆点Oo设在图像的左上角，同时还需要建立以实际物理单位如毫米为单位的图像坐标系，把该坐标系的圆点O1定在摄像机光轴和图像的交点处，x,y轴分别于u,v轴平行，如下图所示，并设O1的像素坐标为(u0,v0)(一般接近图像中心处)，以dx和dy表示单个像素在x和y方向上的物理尺寸，则像素坐标系于图像坐标系存在的关系有：
转换为齐次坐标形式
则最终将像素坐标系与世界坐标系进行转换，为了计算方便在上面的式子中左右都乘上Zc。
则先转换为摄像机坐标系
再转换为世界坐标系为
进一步转化为
其中，
M1是相机的内参数矩阵，由相机内部决定
M2是相机的外参数矩阵，由旋转和平移矩阵决定
畸变校正部分
在没有经过畸变校正的前点为(u,v)
则经过径向畸变校正后点为：
所以在标定的时候，需要标定出k1,k2,k3,k4畸变系数，r为畸变点到摄像机光心的距离
平行双目视觉模型部分
空间中的一点P在左右摄像机平面的坐标为pl(xl,yl),pr(xr,yr),连接左右摄像机Ol和Or为基线，基线长度为T，可以根据三角行相似性原理和左右摄像机平面上的坐标来求得，P点在左摄像机坐标系上的XYZ坐标。
下面将仔细认真的推导以下三维点云坐标的计算公式，有不对的地方还请指正
还是需要上面的图，但是我们需要设定几个未知值，请看下图
P点与摄像机左右平面交点分别是（xl,y）和(xr,y)，并设左右平面的长度为R，x1表示从（xl,y）交点到左摄像机平面最右边的距离，x2表示两个摄像机平面之间的距离，x3表示点P与右摄像机相交点(xr,y)到最左边的距离，想要求取深度信息Z，可以利用三角型相似性原理两个三角型分别为OlOrP和xlxrP，则根据三角型相似性原理由以下公式
其中x=x1+x2+x3
x1=R-xl
x2=T-d/2-d/2=T-d
x3=xr
综合上面几个式子可以得出x=T+xr-xl
最后带入上面三角型相似性的式子可以得出
再做进一步变换
根据前面的
就可以得出
再根据
好吧！我承认我罗嗦了，但是还要啰嗦求取以下左摄像机下X的坐标：
再重新修改一下图吧！
下面我们用P1POl这个三角型和P2xlOl这两个三角型相似性原理来求解Xc，从P点到Ol和Or之间做一条垂线，Ol到这个垂线之间的距离为Xc也就是为左摄相机坐标系下的X轴上的坐标，同样根据三角型相似性原理可以求出：
根据
同理可以根据三角型相似性原理可以求出Y轴上的坐标：
最后可以总结得出三维坐标公式为：
