坐标系转换- 大地坐标系到GPS坐标系

世界大地测量系统(World Geodetic System, WGS)，比如WGS84，是一种地理坐标系统，用于全球定位系统(GPS)。 

 ENU（东、北、天）坐标系就是分别以原点正东方、正北方和垂直地表正上方作为XYZ三轴正方向的坐标系，ENU坐标系又称为站心坐标系或站点坐标系；

 NED（北、东、地）坐标系就是分别以原点正北方、正东方和正下方作为XYZ三轴正方向的坐标系，NED坐标系ENU坐标系本质上是一体两面的。

1.1 ECEF坐标系

??也叫地心地固直角坐标系。其原点为地球的质心，x轴延伸通过本初子午线（0度经度）和赤道（0deglatitude）的交点。 z轴延伸通过的北极（即，与地球旋转轴重合）。 y轴完成右手坐标系，穿过赤道和90度经度。

 ENU坐标系和NED坐标系如图1所示： 

1.2 WGS-84坐标
??也就是也叫经纬高坐标系(经度(longitude)，纬度(latitude)和高度(altitude)LLA坐标系)。，全球地理坐标系、大地坐标系。可以说是最为广泛应用的一个地球坐标系，它给出一点的大地纬度、大地经度和大地高程而更加直观地告诉我们该点在地球中的位置，故又被称作纬经高坐标系。WGS-84坐标系的X轴指向BIH(国际时间服务机构)1984.0定义的零子午面(Greenwich)和协议地球极(CTP)赤道的交点。Z轴指向CTP方向。Y轴与X、Z轴构成右手坐标系。

??一句话解释就是：把前面提到的ECEF坐标系用在GPS中，就是WGS-84坐标系。
其中：
（1）：大地纬度是过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。

（2）：大地经度是过用户点P的子午面与本初子午线之间的夹角。经度值在-180°到+180°之间。

（3）：大地高度h是过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。

1.3 东北天坐标系（ENU）
??也叫站心坐标系以用户所在位置P为坐标原点。

??坐标系定义为： X轴：指向东边 Y轴：指向北边 Z轴：指向天顶

??ENU局部坐标系采用三维直角坐标系来描述地球表面，实际应用较为困难，因此一般使用简化后的二维投影坐标系来描述。在众多二维投影坐标系中，统一横轴墨卡托（The Universal Transverse Mercator ，UTM）坐标系是一种应用较为广泛的一种。UTM 坐标系统使用基于网格的方法表示坐标，它将地球分为 60 个经度区，每个区包含6度的经度范围，每个区内的坐标均基于横轴墨卡托投影，如下图所示：

#!/usr/bin/python
# -*- coding: UTF-8 -*-



'''


// 北东天->gps
void world2GPS(double r_lat, double r_lon, double x_in_NED, double y_in_NED, double& t_lon, double& t_lat) {
    double y_rad = x_in_NED / C_EARTH;
    t_lat = (r_lat + RAD2DEG(y_rad));
    double x_rad = y_in_NED / C_EARTH / cos(DEG2RAD(t_lat));
    t_lon = r_lon + RAD2DEG(x_rad);
}

void trans_NED2GPS(std::map& all_gps_data) {
    std::map::iterator it;
    double init_lat = 25.44993764;  // 纬度
    double init_lon = 112.71335009; // 经度
    // std::ofstream out("../data/gps_ccctrans.txt", std::ios::app);
    for (it = all_gps_data.begin(); it != all_gps_data.end(); it++) {
        double lat, lon;
        // std::cout<<"x:"<<(it->second).x<second).y<second).x, (it->second).y, lon, lat);
        it->second.x = lat;
        it->second.y = lon;
        // std::cout<<"lat:"<first<<" "<second).z<