齐次坐标转换算法

一、坐标系的定义（1）大地坐标系 COc XcYcZcWGS-84大地坐标系原点位于地球质心， Zc轴指向地球北极， Xc轴由原点指向格林尼治平子午面与赤道交点， Yc轴垂直XcOcZc平面与Zc轴，Xc轴共同组成右手坐标系称为大地坐标系。某点大地坐标系下坐标用 B,L,H表示，分别表示该点的经度、纬度和大地高程。（2）空间大地直角坐标系 GOg XgYgZg空间大地直角坐标系具有与大地坐标系相同的坐标轴。不同的是，空间大地直角坐标系为笛卡尔坐标系，大地坐标系为椭球坐标系。 某点的大地直角坐标系下的坐标用 xg,yg,zg表示。（3）地理坐标系SOXYZssss以无人机平台为例，将无人机平台看做质点，飞机平台的位置B,L,H即为地理坐标系的原点，可以表示为s,s,hs，Zs轴指向正北方向，Xs轴由原点指向天顶方向，Ys与Zs和Xs组合起来构成右手坐标系被称为地理坐标系。该坐标系也为笛卡尔坐标系。某点地理坐标系下坐标用xs,ys,zs表示。（4）无人机坐标系AOaXYZaaa无人机坐标系和地理坐标系具有相同的原点。无人机坐标系相当于地理坐标系经过三轴旋转而成的坐标系，当无人机姿态角均为零时，无人机坐标系与地理坐标系重合。设无人机三轴姿态角为，，，无人机坐标系即为地理坐标系绕Ya（俯仰），Za（横滚），Xa（偏航）旋转，，后形成的坐标系。某点载机坐标系下坐标用xa,ya,za表示。（5）摄像机坐标系TOXYZtttt质点位于摄像机光轴与摄像机镜头平面的交点，Zt轴定义为指向目标物体时摄像机光轴的方向，当Zt轴处于水平方向时，Xt轴指向天顶方向，Yt轴与Zt轴和Xt轴共同组成的右手坐标系称为摄像机坐标系。二、坐标系之间的转换2.1目标物体在摄像机坐标系下的坐标我们把摄像机镜头中心称为摄像机的光心， 光心即为摄像机坐标系的原点， 摄像机成像几何示意图如图 2.1所示，C点为光心， Zc轴由光心指向摄像机视轴方向，并且垂直于像平面，Xc轴与

Yc轴和像平面的

x轴与

y轴平行，像平面坐标系原点

C1定义为视轴与像平面的交点，以

C点为坐标原点，由

Xc轴、

Yc轴和

Zc轴构成空间笛卡尔直角坐标系称为摄像机坐标系，

CC1为摄像机的焦距。图

2.1

摄像机几何成像示意图目标物体在摄像机坐标系下坐标值用

T表示，目标物体像点在摄像机坐标系下的值用

K表示，

K

x,y,z,1T,其中

z=f，f为摄像机的焦距，当脱靶量很小的情况下，

T与K的比例因子近似为-R/f，R为目标物体到摄像机光心的距离，

T和

K满足式（

2-1）和式（

2-2）所示的比例关系：Q1K(2-1)R/f0000R/f00Q10R/f（2-2）0000012.2摄像机坐标系T转换为载机坐标系A图2.2摄像机坐标系T到载机坐标系A的转换示意图如图2.2所示，摄像机坐标系T绕X轴旋转（方位）后的到变换矩阵Q：t21000Q20cossin00sincos（2-3）00001绕Yt（俯仰）轴旋转后得到变换矩阵Q3：cos0-sin00100Q30cos（3-4）sin00001摄像机坐标系 B到载机坐标系 A的变换过程如式（ 2-5）所示：xa,ya,za,1T Q3Q2xt,yt,zt,1T （2-5）2.3载机坐标系 A转换为地理坐标系 S图2.3载机坐标系 A到地理坐标系 S的转换示意图如图2.3所示，载机坐标系A绕Za轴旋转后得到变换矩阵Q4：cossin00Q4sincos00001（2-6）00001绕Ys轴旋转后的得到变换矩阵Q5：cos0sin0Q50100sin0cos（2-7）00001绕X旋转后得到变换矩阵Q：s61000Q60cossin0（2-8）0sincos00001载机坐标系A到地理坐标系S的变换过程如式（2-9）所示：xs,ys,zs,1TQ6Q5Q4xa,ya,za,1T（2-9）2.4地理坐标系S转换为空间大地直角坐标系G图2.4地理坐标系 S转换为空间大地直角坐标系 G的示意图如图2.4所示，地理坐标系沿 Xs轴平移hs后得到变换矩阵 Q7:100hs0100Q701（2-10）000001绕Ys轴旋转-s后得到变换矩阵Q8：cos0Q8-sin0

ss

0 sin s00coss0 0

002-11）01绕Zs轴旋转 s后得到变换矩阵Q9：cos ssin sQ900

sincos00

ss

00（2-12）01沿Zg轴方向平移OgKOgKN0e2sins后得到变换矩阵Q10：10000100Q1001-N0e2sin（2-13）0s0001地理坐标系S到空间大地直角坐标系G的变换过程如式（2-14）所示：xg,yg,zg,1TQ7Q9Q8Q10xs,ys,zs,1T（3-14）a其中，N01（3-15）1e2sin2s2（5）空间大地直角坐标系G转换为大地坐标系C：Bzgbe2sin3Uarctanyg2xg2ae2cos3ULarctanyg(3-16)xgxg2yg2HNcosB其中，Na11e2sin2B23-17）zgaUarctanyg2bxg2Xg=g*h1*i1-a*y*(b2*(c1*(e1*(f2*i2+f1*h2*i1)+e2*h1*i1)+c2*(d1*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)+d2*(f1*i2-f2*h2*i1)))-b1*(d2*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)-d1*(f1*i2-f2*h2*i1)))+a*z*(b1*(c1*(e1*(f2*i2+f1*h2*i1)+e2*h1*i1)+c2*(d1*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)+d2*(f1*i2-f2*h2*i1)))+b2*(d2*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)-d1*(f1*i2-f2*h2*i1)))+a*x*(c2*(e1*(f2*i2+f1*h2*i1)+e2*h1*i1)-c1*(d1*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)+d2*(f1*i2-f2*h2*i1)))Yg=g*h1*i2+a*y*(b2*(c1*(e1*(f2*i1-f1*h2*i2)-e2*h1*i2)+c2*(d1*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)+d2*(f1*i1+f2*h2*i2)))-b1*(d2*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)-d1*(f1*i1+f2*h2*i2)))-a*z*(b1*(c1*(e1*(f2*i1-f1*h2*i2)-e2*h1*i2)+c2*(d1*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)+d2*(f1*i1+f2*h2*i2)))+b2*(d2*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)-d1*(f1*i1+f2*h2*i2)))-a*x*(c2*(e1*(f2*i1-f1*h2*i2)-e2*h1*i2)-c1*(d1*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)+d2*(f1*i1+f2*h2*i2)))Zg=j-g*h2 -a*x*(c1*(d1*(e1*h2 +e2*f1*h1) -d2*f2*h1) +c2*(e2*h2 -e1*f1*h1)) +a*y*(b1*(d2*(e1*h2 +e2*f1*h1) +d1*f2*h1