我在《大地经纬度坐标与地心地固坐标的的转换》这篇文章中已经论述了地心坐标系的概念。我们知道,基于地心坐标系的坐标都是很大的值,这样的值是不太方便进行空间计算的,所以很多时候可以选取一个站心点,将这个很大的值变换成一个较小的值。以图形学的观点来看,地心坐标可以看作是世界坐标,站心坐标可以看作局部坐标。
站心坐标系以一个站心点为坐标原点,当把坐标系定义为X轴指东、Y轴指北,Z轴指天,就是ENU(东北天)站心坐标系。这样,从地心地固坐标系转换成的站心坐标系,就会成为一个符合常人对地理位置认知的局部坐标系。同时,只要站心点位置选的合理(通常可选取地理表达区域的中心点),表达的地理坐标都会是很小的值,非常便于空间计算。
令选取的站心点为P,其大地经纬度坐标为 ( B p , L p , H p ) (B_p,L_p,H_p) (Bp,Lp,Hp),对应的地心地固坐标系为 ( X p , Y p , Z p ) (X_p,Y_p,Z_p) (Xp,Yp,Zp)。地心地固坐标系简称为ECEF,站心坐标系简称为ENU。
通过第一节的图可以看出,ENU要转换到ECEF,一个很明显的图形操作是平移变换,将站心移动到地心。根据站心点P在地心坐标系下的坐标 ( X p , Y p , Z p ) (X_p,Y_p,Z_p) (Xp,Yp,Zp),可以很容易推出ENU转到ECEF的平移矩阵:
T = [ 1 0 0 X p 0 1 0 Y p 0 0 1 Z p 0 0 0 1 ] T = \begin{bmatrix} 1&0&0&X_p\\ 0&1&0&Y_p\\ 0&0&1&Z_p\\ 0&0&0&1\\ \end{bmatrix} T=⎣⎢⎢⎡100001000010XpYpZp1⎦⎥⎥⎤
反推之,ECEF转换到ENU的平移矩阵就是T的逆矩阵:
T − 1 = [ 1 0 0 − X p 0 1 0 − Y p 0 0 1 − Z p 0 0 0 1 ] T^{-1} = \begin{bmatrix} 1&0&0&-X_p\\ 0&1&0&-Y_p\\ 0&0&1&-Z_p\\ 0&0&0&1\\ \end{bmatrix} T−1=⎣⎢⎢⎡100001000010−Xp−Yp−Zp1⎦⎥⎥⎤
另外一个需要进行的图形变换是旋转变换,其旋转变换矩阵根据P点所在的经度L和纬度B确定。这个旋转变换有点难以理解,需要一定的空间想象能力,但是可以直接给出如下结论:
根据我在《WebGL简易教程(五):图形变换(模型、视图、投影变换)》提到的旋转变换,绕X轴旋转矩阵为:
R x = [ 1 0 0 0 0 c o s θ − s i n θ 0 0 s i n θ c o s θ 0 0 0 0 1 ] R_x = \begin{bmatrix} 1&0&0&0\\ 0&cosθ&-sinθ&0\\ 0&sinθ&cosθ&0\\ 0&0&0&1\\ \end{bmatrix} Rx=⎣⎢⎢⎡10000cosθsinθ00−sinθcosθ00001⎦⎥⎥⎤
绕Z轴旋转矩阵为:
R z = [ c o s θ − s i n θ 0 0 s i n θ c o s θ 0 0 0 0 1 0 0 0 0 1 ] R_z = \begin{bmatrix} cosθ&-sinθ&0&0\\ sinθ&cosθ&0&0\\ 0&0&1&0\\ 0&0&0&1\\ \end{bmatrix} Rz=⎣⎢⎢⎡cosθsinθ00−sinθcosθ0000100001⎦⎥⎥⎤
从ENU转换到ECEF的旋转矩阵为:
R = R z ( p i 2 + L ) ⋅ R x ( p i 2 − B ) (1) R = {R_z(\frac{pi}{2}+L)}\cdot{R_x(\frac{pi}{2}-B)} \tag{1} R=Rz(2pi+L)⋅Rx(2pi−B)(1)
根据三角函数公式:
s i n ( π / 2 + α ) = c o s α s i n ( π / 2 − α ) = c o s α c o s ( π / 2 + α ) = − s i n α c o s ( π / 2 − α ) = s i n α sin(π/2+α)=cosα\\ sin(π/2-α)=cosα\\ cos(π/2+α)=-sinα\\ cos(π/2-α)=sinα\\ sin(π/2+α)=cosαsin(π/2−α)=cosαcos(π/2+α)=−sinαcos(π/2−α)=sinα
有:
R z ( p i 2 + L ) = [ − s i n L − c o s L 0 0 c o s L − s i n L 0 0 0 0 1 0 0 0 0 1 ] (2) R_z(\frac{pi}{2}+L) = \begin{bmatrix} -sinL&-cosL&0&0\\ cosL&-sinL&0&0\\ 0&0&1&0\\ 0&0&0&1\\ \end{bmatrix} \tag{2} Rz(2pi+L)=⎣⎢⎢⎡−sinLcosL00−cosL−sinL0000100001⎦⎥⎥⎤(2)
R x ( p i 2 − B ) = [ 1 0 0 0 0 s i n B − c o s B 0 0 c o s B s i n B 0 0 0 0 1 ] (3) R_x(\frac{pi}{2}-B) = \begin{bmatrix} 1&0&0&0\\ 0&sinB&-cosB&0\\ 0&cosB&sinB&0\\ 0&0&0&1\\ \end{bmatrix} \tag{3} Rx(2pi−B)=⎣⎢⎢⎡10000sinBcosB00−cosBsinB00001⎦⎥⎥⎤(3)
将(2)、(3)带入(1)中,则有:
R = [ − s i n L − s i n B c o s L c o s B c o s L 0 c o s L − s i n B s i n L c o s B s i n L 0 0 c o s B s i n B 0 0 0 0 1 ] (4) R = \begin{bmatrix} -sinL&-sinBcosL&cosBcosL&0\\ cosL&-sinBsinL&cosBsinL&0\\ 0&cosB&sinB&0\\ 0&0&0&1\\ \end{bmatrix} \tag{4} R=⎣⎢⎢⎡−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001⎦⎥⎥⎤(4)
而从ECEF转换到ENU的旋转矩阵为:
R − 1 = R x ( − ( p i 2 − B ) ) ⋅ R z ( − ( p i 2 + L ) ) (5) R^{-1} = {R_x(-(\frac{pi}{2}-B))} \cdot {R_z(-(\frac{pi}{2}+L))} \tag{5} R−1=Rx(−(2pi−B))⋅Rz(−(2pi+L))(5)
旋转矩阵是正交矩阵,根据正交矩阵的性质:正交矩阵的逆矩阵等于其转置矩阵,那么可直接得:
R − 1 = [ − s i n L c o s L 0 0 − s i n B c o s L − s i n B s i n L c o s B 0 c o s B c o s L c o s B s i n L s i n B 0 0 0 0 1 ] (6) R^{-1} = \begin{bmatrix} -sinL&cosL&0&0\\ -sinBcosL&-sinBsinL&cosB&0\\ cosBcosL&cosBsinL&sinB&0\\ 0&0&0&1\\ \end{bmatrix} \tag{6} R−1=⎣⎢⎢⎡−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001⎦⎥⎥⎤(6)
将上述公式展开,可得从ENU转换到ECEF的图形变换矩阵为:
M = T ⋅ R = [ 1 0 0 X p 0 1 0 Y p 0 0 1 Z p 0 0 0 1 ] [ − s i n L − s i n B c o s L c o s B c o s L 0 c o s L − s i n B s i n L c o s B s i n L 0 0 c o s B s i n B 0 0 0 0 1 ] M = T \cdot R = \begin{bmatrix} 1&0&0&X_p\\ 0&1&0&Y_p\\ 0&0&1&Z_p\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} -sinL&-sinBcosL&cosBcosL&0\\ cosL&-sinBsinL&cosBsinL&0\\ 0&cosB&sinB&0\\ 0&0&0&1\\ \end{bmatrix} M=T⋅R=⎣⎢⎢⎡100001000010XpYpZp1⎦⎥⎥⎤⎣⎢⎢⎡−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001⎦⎥⎥⎤
而从ECEF转换到ENU的图形变换矩阵为:
M − 1 = R − 1 ∗ T − 1 = [ − s i n L c o s L 0 0 − s i n B c o s L − s i n B s i n L c o s B 0 c o s B c o s L c o s B s i n L s i n B 0 0 0 0 1 ] [ 1 0 0 − X p 0 1 0 − Y p 0 0 1 − Z p 0 0 0 1 ] M^{-1} = R^{-1} * T^{-1} = \begin{bmatrix} -sinL&cosL&0&0\\ -sinBcosL&-sinBsinL&cosB&0\\ cosBcosL&cosBsinL&sinB&0\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} 1&0&0&-X_p\\ 0&1&0&-Y_p\\ 0&0&1&-Z_p\\ 0&0&0&1\\ \end{bmatrix} M−1=R−1∗T−1=⎣⎢⎢⎡−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001⎦⎥⎥⎤⎣⎢⎢⎡100001000010−Xp−Yp−Zp1⎦⎥⎥⎤
接下来用代码实现这个坐标转换,选取一个站心点,以这个站心点为原点,获取某个点在这个站心坐标系下的坐标:
#include
#include
#include
using namespace std;
const double epsilon = 0.000000000000001;
const double pi = 3.14159265358979323846;
const double d2r = pi / 180;
const double r2d = 180 / pi;
const double a = 6378137.0; //椭球长半轴
const double f_inverse = 298.257223563; //扁率倒数
const double b = a - a / f_inverse;
//const double b = 6356752.314245; //椭球短半轴
const double e = sqrt(a * a - b * b) / a;
void Blh2Xyz(double &x, double &y, double &z)
{
double L = x * d2r;
double B = y * d2r;
double H = z;
double N = a / sqrt(1 - e * e * sin(B) * sin(B));
x = (N + H) * cos(B) * cos(L);
y = (N + H) * cos(B) * sin(L);
z = (N * (1 - e * e) + H) * sin(B);
}
void Xyz2Blh(double &x, double &y, double &z)
{
double tmpX = x;
double temY = y ;
double temZ = z;
double curB = 0;
double N = 0;
double calB = atan2(temZ, sqrt(tmpX * tmpX + temY * temY));
int counter = 0;
while (abs(curB - calB) * r2d > epsilon && counter < 25)
{
curB = calB;
N = a / sqrt(1 - e * e * sin(curB) * sin(curB));
calB = atan2(temZ + N * e * e * sin(curB), sqrt(tmpX * tmpX + temY * temY));
counter++;
}
x = atan2(temY, tmpX) * r2d;
y = curB * r2d;
z = temZ / sin(curB) - N * (1 - e * e);
}
void TestBLH2XYZ()
{
//double x = 113.6;
//double y = 38.8;
//double z = 100;
//
//printf("原大地经纬度坐标:%.10lf\t%.10lf\t%.10lf\n", x, y, z);
//Blh2Xyz(x, y, z);
//printf("地心地固直角坐标:%.10lf\t%.10lf\t%.10lf\n", x, y, z);
//Xyz2Blh(x, y, z);
//printf("转回大地经纬度坐标:%.10lf\t%.10lf\t%.10lf\n", x, y, z);
double x = -2318400.6045575836;
double y = 4562004.801366804;
double z = 3794303.054150639;
//116.9395751953 36.7399177551
printf("地心地固直角坐标:%.10lf\t%.10lf\t%.10lf\n", x, y, z);
Xyz2Blh(x, y, z);
printf("转回大地经纬度坐标:%.10lf\t%.10lf\t%.10lf\n", x, y, z);
}
void CalEcef2Enu(Eigen::Vector3d& topocentricOrigin, Eigen::Matrix4d& resultMat)
{
double rzAngle = -(topocentricOrigin.x() * d2r + pi / 2);
Eigen::AngleAxisd rzAngleAxis(rzAngle, Eigen::Vector3d(0, 0, 1));
Eigen::Matrix3d rZ = rzAngleAxis.matrix();
double rxAngle = -(pi / 2 - topocentricOrigin.y() * d2r);
Eigen::AngleAxisd rxAngleAxis(rxAngle, Eigen::Vector3d(1, 0, 0));
Eigen::Matrix3d rX = rxAngleAxis.matrix();
Eigen::Matrix4d rotation;
rotation.setIdentity();
rotation.block<3, 3>(0, 0) = (rX * rZ);
//cout << rotation << endl;
double tx = topocentricOrigin.x();
double ty = topocentricOrigin.y();
double tz = topocentricOrigin.z();
Blh2Xyz(tx, ty, tz);
Eigen::Matrix4d translation;
translation.setIdentity();
translation(0, 3) = -tx;
translation(1, 3) = -ty;
translation(2, 3) = -tz;
resultMat = rotation * translation;
}
void CalEnu2Ecef(Eigen::Vector3d& topocentricOrigin, Eigen::Matrix4d& resultMat)
{
double rzAngle = (topocentricOrigin.x() * d2r + pi / 2);
Eigen::AngleAxisd rzAngleAxis(rzAngle, Eigen::Vector3d(0, 0, 1));
Eigen::Matrix3d rZ = rzAngleAxis.matrix();
double rxAngle = (pi / 2 - topocentricOrigin.y() * d2r);
Eigen::AngleAxisd rxAngleAxis(rxAngle, Eigen::Vector3d(1, 0, 0));
Eigen::Matrix3d rX = rxAngleAxis.matrix();
Eigen::Matrix4d rotation;
rotation.setIdentity();
rotation.block<3, 3>(0, 0) = (rZ * rX);
//cout << rotation << endl;
double tx = topocentricOrigin.x();
double ty = topocentricOrigin.y();
double tz = topocentricOrigin.z();
Blh2Xyz(tx, ty, tz);
Eigen::Matrix4d translation;
translation.setIdentity();
translation(0, 3) = tx;
translation(1, 3) = ty;
translation(2, 3) = tz;
resultMat = translation * rotation;
}
void TestXYZ2ENU()
{
double L = 116.9395751953;
double B = 36.7399177551;
double H = 0;
cout << fixed << endl;
Eigen::Vector3d topocentricOrigin(L, B, H);
Eigen::Matrix4d wolrd2localMatrix;
CalEcef2Enu(topocentricOrigin, wolrd2localMatrix);
cout << "地心转站心矩阵:" << endl;
cout << wolrd2localMatrix << endl<<endl;
cout << "站心转地心矩阵:" << endl;
Eigen::Matrix4d local2WolrdMatrix;
CalEnu2Ecef(topocentricOrigin, local2WolrdMatrix);
cout << local2WolrdMatrix << endl;
double x = 117;
double y = 37;
double z = 10.3;
Blh2Xyz(x, y, z);
cout << "ECEF坐标(世界坐标):";
Eigen::Vector4d xyz(x, y, z, 1);
cout << xyz << endl;
cout << "ENU坐标(局部坐标):";
Eigen::Vector4d enu = wolrd2localMatrix * xyz;
cout << enu << endl;
}
void TestOE()
{
double L = 116.9395751953;
double B = 36.7399177551;
double H = 0;
osgEarth::SpatialReference *spatialReference = osgEarth::SpatialReference::create("epsg:4326");
osgEarth::GeoPoint centerPoint(spatialReference, L, B, H);
osg::Matrixd worldToLocal;
centerPoint.createWorldToLocal(worldToLocal);
cout << fixed << endl;
cout << "地心转站心矩阵:" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
printf("%lf\t", worldToLocal.ptr()[j * 4 + i]);
}
cout << endl;
}
cout << endl;
osg::Matrixd localToWorld;
centerPoint.createLocalToWorld(localToWorld);
cout << "站心转地心矩阵:" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
printf("%lf\t", localToWorld.ptr()[j * 4 + i]);
}
cout << endl;
}
cout << endl;
double x = 117;
double y = 37;
double z = 10.3;
osgEarth::GeoPoint geoPoint(spatialReference, x, y, z);
cout << "ECEF坐标(世界坐标):";
osg::Vec3d out_world;
geoPoint.toWorld(out_world);
cout << out_world.x() <<'\t'<< out_world.y() << '\t' << out_world.z() << endl;
cout << "ENU坐标(局部坐标):";
osg::Vec3d localCoord = worldToLocal.preMult(out_world);
cout << localCoord.x() << '\t' << localCoord.y() << '\t' << localCoord.z() << endl;
}
int main()
{
//TestBLH2XYZ();
cout << "使用Eigen进行转换实现:" << endl;
TestXYZ2ENU();
cout <<"---------------------------------------"<< endl;
cout << "通过OsgEarth进行验证:" << endl;
TestOE();
}
这个示例先用Eigen矩阵库,计算了坐标转换需要的矩阵和转换结果;然后通过osgEarth进行了验证,两者的结果基本一致。运行结果如下: