立体视觉四十二章经第01章：世界坐标系及不同坐标系的转换

坐标系简介

椭球种类/椭球参数	克拉索夫斯基椭球	1975国际椭球	WGS84椭球	国家2000坐标系椭球
长半轴(a)	6378245	6378140	6378137	6378137
短半轴(b)	6356863.0187730473	6356755.288157528	6356752.3142451795	6356752.3141403558

  从左到右分别对应常用的北京54坐标系、西安80坐标系、WGS84坐标系以及CGCS2000坐标系。习惯上我们用长半轴a以及扁率表示表示椭球，有了长半轴和短半轴参数信息，常见的表示参数可以下面根据公式求得。

    扁率：$\partial =\frac{a-b}{b}$

    第一偏心率：$e=\frac{\sqrt{a^2-b^2}}{a}$

    第二偏心率：$e^{\prime }=\frac{\sqrt{a^2-b^2}}{b}$

ECEF坐标系

                     $图1、地心地固坐标系$

  地心地固坐标系（Earth-Centered,Earth-Fixed，简称ECEF）简称地心坐标系，是一种以地心为原点的地固坐标系（也称地球坐标系）。原点O（0,0,0）为地球质心，z轴与地轴平行指向北极点，x轴指向本初子午线与赤道的交点，y轴垂直于xOz平面(即东经90度与赤道的交点)构成右手坐标系。

WGS84坐标系

                     $图2、GPS高度计算$

  wgs84大地坐标系是由经度(longitude)，纬度(latitude)，高度(altitude)组成的坐标系，它给出一点的大地纬度、大地经度和大地高程告诉我们该点在地球中的位置，故又被称作纬经高（LLA）坐标系。84坐标系和上面的ECEF坐标系一样，只不过坐标表示方式不一样，具体如下：

• 大地纬度是过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。
• 大地经度是过用户点P的子午面与本初子午线之间的夹角。经度值在-180°到+180°之间。
• 大地高度h是过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。

ENU局部坐标系

                     $图3、全局和局部坐标系$

  在 $P_i$ 处对应于北、东、上方向的局部（切平面）坐标系的轴 ${\underline{n}}_i$，${\underline{e}}_i$，${\underline{u}}_i$ 在全局坐标系中表示为：
n ‾ i = [ − s i n φ i c o s λ i − s i n φ i s i n λ i c o s φ i ] , e ‾ i = [ − s i n λ i c o s λ i 0 ] , u ‾ i = [ c o s φ i c o s λ i c o s φ i s i n λ i s i n φ i ] \underline{n}_i =\left[ \begin{matrix} -sin\varphi_icos\lambda_i \\ -sin\varphi_isin\lambda_i \\ cos\varphi_i \end{matrix} \right],\underline{e}_i =\left[ \begin{matrix} -sin\lambda_i \\ cos\lambda_i \\ 0 \end{matrix} \right],\underline{u}_i =\left[ \begin{matrix} cos\varphi_icos\lambda_i \\ cos\varphi_isin\lambda_i \\ sin\varphi_i \end{matrix} \right] n​i​= ​−sinφi​cosλi​−sinφi​sinλi​cosφi​​ ​,e​i​= ​−sinλi​cosλi​0​ ​,u​i​= ​cosφi​cosλi​cosφi​sinλi​sinφi​​ ​

  其中，向量 $n_i$ 和 $e_i$ 张成图3中点 $P_i$ 的切平面。局部水平系统的第三个坐标轴，即向量$u_i$，与切平面正交，指向圆周率的天顶，这个坐标轴的方向与椭球法线重合。

                     $图4、局部坐标系中的测量量$

  现在介绍局部坐标系中向量 ${\underline{x}}_{ij}$的分量 $n_{ij},e_{ij},u_{ij}$，这些坐标有时表示为ENU坐标。考虑图4，这些分量是向量 ${\underline{X}}_{ij}$在局部坐标轴 ${\underline{n}}_i$，${\underline{e}}_i$，${\underline{u}}_i$ 上投影得到的，因此：
x ‾ i j = [ n i j e i j u i j ] = [ n ‾ i . X ‾ i j e ‾ i . X ‾ i j u ‾ i . X ‾ i j ] \underline{x}_{ij} =\left[ \begin{matrix} n_{ij} \\ e_{ij} \\ u_{ij} \end{matrix} \right]=\left[ \begin{matrix} \underline{n}_{i}.\underline{X}_{ij} \\ \underline{e}_{i}.\underline{X}_{ij} \\ \underline{u}_{i}.\underline{X}_{ij} \end{matrix} \right] x​ij​= ​nij​eij​uij​​ ​= ​n​i​.X​ij​e​i​.X​ij​u​i​.X​ij​​ ​

UTM统一横轴墨卡托坐标系

                     $图5、UTM坐标系生成方式$

  UTM系统是横向墨卡托系统的改进。首先，椭球体被划分为60个区域，每个区域的宽度为6个经度；其次，将$scale=0.9996$的比例因子应用于平面的共形坐标，比例因子的作用是为了避免一个区域的外部地区出现相当大的扭曲。横向划分完成后，纵向也按一定的规则进行划分，最终将图1的球面坐标系转化成图2的平面坐标系，具体划分规则见下文。

经度区间

  每6°被编排为一个经度区间，每一个经度区间均以一个数字表示，由西（180°W）向东（180°E）以01至60编排。

维度区间

  从南纬80°开始，每8°被编排为一个纬度区间，而最北的纬度区间（北纬74°以北之区间）则被延伸至北纬84°，以覆盖世界上大部分陆地。每一个纬度区间均以一个英文字母表示，由南向北数以"C"至"X"编排（省略"I"和"O"防止与1和0混淆），具体情况如下︰

维度	英文字母
南纬80° - 南纬72°	C
南纬72° - 南纬64°	D
南纬64° - 南纬56°	E
南纬56° - 南纬48°	F
南纬48° - 南纬40°	G
南纬40° - 南纬32°	H
南纬32° - 南纬24°	J
南纬24° - 南纬16°	K
南纬16° - 南纬8°	L
南纬8° - 赤道0°	M
赤道0° - 北纬8°	N
北纬8° - 北纬16°	P
北纬16° - 北纬24°	Q
北纬24° - 北纬32°	R
北纬32° - 北纬40°	S
北纬40° - 北纬48°	T
北纬48° - 北纬56°	U
北纬56° - 北纬64°	V
北纬64° - 北纬72°	W
北纬72° - 北纬84°	X

经纬度补划分带补充说明

  字母A、B、Y、Z表示UTM坐标系中的极地区域。指定一个UTM网格区域，区域编号（列）在区域字母（行）之前给出，例如 zone 11U。

  如上图所示，用水平带划分区域，使得每个分区具有6°经度宽和8°纬度高的维度，但是也有例外，最北的带（X）纬度高是12°而不是8°，从72°N延伸到84°N纬度。覆盖挪威西海岸的32V区由原来的6°经度拓宽为9°经度，导致 31V区缩小。33X和35X区域已经拓宽到12°，覆盖斯瓦尔巴特群岛。同样，31X和37X区域被加宽到 9°。因此，32X、34X 和36X区域被省略。

UTM坐标 – Easting, Northing

  投影带(zone)中心子午线“东值”为50万米，投影带中心子午线以东的“东值”增加，投影带中心子午线以西的“东值”减小，例如中央子午线以东8米的一点，其东坐标为500000 + 8 = 500008mE。中央子午线以西350米的一个点东坐标为500000 - 350 = 499650mE。

  北半球的“北值”表示一个点位于赤道以北的米数；南半球的“北值”等于1000万米减去该点离赤道的距离；赤道处等于1000万米。例如：位于赤道以南34米的点的北坐标9999966mN，而在赤道以北34米的点的北坐标为0000034mN。如果“北值”小于7位，通常会在“北值”前面加上0，以表示“北值”为7位数字。

坐标系转换

WGS84与ECEF的相互转换

                $图6、笛卡尔坐标系X,Y,Z和椭球坐标系\lambda ,\phi ,h$

wgs84大地坐标转ecef地心坐标

  如图1所示，坐标转换可以根据三角函数来计算，其中wgs84大地坐标系到ecef地心坐标系的转换公式如下：
$$\{\begin{array}{rl}X& =(N+h)cos\phi cos\lambda \\ Y& =(N+h)cos\phi sin\lambda \\ Z& =(\frac{b^2}{a^2}N+h)sin\phi \end{array}\text{\hspace{1em}(1)}$$

  其中N=OB为卯酉圈曲率半径可由下面公式获得：

$$N=\frac{a^2}{\sqrt{a^{2}co{s}^2\phi +b^{2}si{n}^2\phi }}\text{\hspace{1em}(2)}$$

  a和b分别为wgs84椭球体的长半轴和短半轴。

ecef地心坐标转wgs84大地坐标

  定义辅助变量$p$:
$$p=\sqrt{X^2+Y^2}=(N+h)cos\phi \text{\hspace{1em}(3)}$$

  因此椭球坐标系高度为：
$$h=\frac{p}{cos\phi }-N\text{\hspace{1em}(4)}$$

  由：
$$e^2=\frac{a^2-b^2}{a^2}=>b^2/a^2=1-e^2\text{\hspace{1em}(5)}$$

  将上述公式代入（1）得：
$$Z=(N+h-e^{2}N)sin\phi =(N+h)(1-e^2\frac{N}{N+h})sin\phi \text{\hspace{1em}(6)}$$

  将该等式除以等式（3）得：
$$\frac{Z}{p}=(1-e^2\frac{N}{N+h})tan\phi \text{\hspace{1em}(7)}$$

  从而推出椭球坐标系的纬度 $\phi$：
$$tan\phi =\frac{Z}{p}(1-e^2\frac{N}{N+h}{)}^{-1}\text{\hspace{1em}(8)}$$

  至于椭球坐标系的经度值 $\lambda$ 可直接由三角公式得到：
$$tan\lambda =\frac{Y}{X}\text{\hspace{1em}(9)}$$

  高度、纬度、经度由等式（4）（8）（9）给出，注意除了经度可以直接由地心坐标计算外，高度和纬度都是了不止一个变量，为计算出最终的wgs84大地坐标系，有两种方法可以采纳：迭代法和近似法，西面逐一介绍。

迭代法ecef->wgs84

  算法流程如下：

  1、计算 $p=\sqrt{X^2+Y^2}$

  2、计算近似值 ${\phi }_{(0)}$
$$tan{\phi }_{(0)}=\frac{Z}{p}(1-e^2{)}^{-1}$$

  3、计算近似值 $N_{(0)}$
$$N_{(0)}=\frac{a^2}{\sqrt{a^{2}co{s}^2{\phi }_{(0)}+b^{2}si{n}^2{\phi }_{(0)}}}$$

  4、计算椭球坐标高度
$$h=\frac{p}{cos{\phi }_{(0)}}-N_{(0)}$$

  5、计算改进纬度
$$tan{\phi }_{(0)}=\frac{Z}{p}(1-e^2\frac{N_{(0)}}{N_{(0)}+h}{)}^{-1}$$

  6、迭代终止条件

    如果 $\phi ={\phi }_{(0)}$ 终止迭代，否则令 ${\phi }_{(0)}=\phi$ 并继续执行步骤3。

近似法ecef->wgs84

$$\phi =arctan\frac{Z+e^{\prime 2}bsi{n}^3\theta }{p-e^{2}aco{s}^3\theta }$$

$$\lambda =arctan\frac{Y}{X}$$

  其中
$$\theta =arctan\frac{Za}{pb}是辅助量，e^{{}^{\prime }2}=\frac{a^2-b^2}{b^2}是椭球第二偏心率。$$

WGS84与UTM的相互转换

wgs84大地坐标转utm坐标

      $x=Ncos\phi \ell +\frac{1}{6}Nco{s}^3\phi (1-t^2+{\eta }^2){\ell }^3$

       $+\frac{1}{120}Nco{s}^{\phi }(5-18t^2+t^4+14{\eta }^2-58t^2{\eta }^2){\ell }^5$

       $+\frac{1}{5040}Nco{s}^7\phi (61-479t^2+179t^4-t^6){\ell }^7+...$

      $y=B(\phi )+\frac{t}{2}co{s}^2\phi {\ell }^2+\frac{t}{24}Nco{s}^4\phi (5-t^2+9{\eta }^2+4{\eta }^4){\ell }^4$

       $+\frac{t}{720}Nco{s}^6\phi (61-58t^2+t^4+270{\eta }^2-330t^2{\eta }^2){\ell }^6$

       $+\frac{t}{40320}Nco{s}^8\phi (1385-3111t^2+543t^4-t^6){\ell }^8+...$

      $x=x\ast scale+500000.0$

      $y=\{\begin{array}{rl}y& \ast scale(北半球)\\ y& \ast scale+10000000(南半球)\end{array}$

  其中：
      $B(\phi )$       …子午线弧长

      $e^{{}^{\prime }2}=(a^2-b^2)/b^2$ …第二偏心率的平方

      ${\eta }^2=e^{{}^{\prime }2}co{s}^2\phi$   …辅助变量

      $N=\frac{a^2}{b\sqrt{1+{\eta }^2}}$   …radius of curvature in prime vertical

      $t=tan\phi$      …辅助变量

      $\ell =\lambda -{\lambda }_0$     …与中央经线的经度差

      ${\lambda }_0$         …中央经线的经度值

  子午线的弧长$B(\phi )$为从赤道到待映射点的椭球距离，可以通过级数展开计算：
$$B(\phi )=\alpha [\phi +\beta sin2\phi +\gamma sin4\phi +\delta sin6\phi +ϵsin8\phi +...]$$

  其中:
      $\alpha =\frac{a+b}{2}(1+\frac{1}{4}{n}^2+\frac{1}{64}{n}^4+...)$

      $\beta =-\frac{3}{2}n+\frac{9}{16}{n}^3-\frac{3}{32}{n}^5+...$

      $\gamma =\frac{15}{16}{n}^2-\frac{15}{32}{n}^4+...$

      $\delta =-\frac{35}{48}{n}^3+\frac{105}{256}{n}^5-...$

      $ϵ=\frac{315}{512}{n}^4+...$

      $n=\frac{a-b}{a+b}$

下表给出了两种椭球对应的参数：

计算好的参数	贝塞尔椭球	WGS-84椭球
$\alpha$	6366742.5203m	6367449.1458m
$\beta$	$-2.51127456\cdot 10^{-3}$	$-2.51882792\cdot 10^{-3}$
$\gamma$	$2.62771\cdot 10^{-6}$	$2.64354\cdot 10^{-6}$
$\delta$	$-3.42\cdot 10^{-9}$	$-3.45\cdot 10^{-9}$
$ϵ$	$5\cdot 10^{-12}$	$5\cdot 10^{-12}$

utm大地坐标转wgs84坐标

      $x=\frac{x-500000}{scale}$

      $y=\{\begin{array}{rl}& \frac{y}{scale}(北半球)\\ & \frac{y-10000000}{scale}(南半球)\end{array}$

      $\phi ={\phi }_f+\frac{t_f}{2N_f^2}(-1-{\eta }_f^2)x^2$

       $+\frac{t_f}{24N_f^4}(5+3t_f^2+6{\eta }_f^2-6t_f^2{\eta }_f^2-3{\eta }_f^4-9t_f^2{\eta }_f^4)x^4$

       $+\frac{t_f}{720N_f^6}(-61-90t_f^2-45t_f^4-107{\eta }_f^2+162t_f^2{\eta }^2+45t_f^4{\eta }^2)x^6$

       $+\frac{t_f}{40320N_f^8}(1385+3633t_f^2+4095t_f^4+1575t_f^6)x^8+...$

      $\lambda ={\lambda }_0+\frac{1}{N_{f}cos{\phi }_f}x+\frac{1}{6N_f^{3}cos{\phi }_f}(-1-2t_f^2-{\eta }_f^2)x^3$

       $+\frac{1}{120N_f^{5}cos{\phi }_f}(5+28t_f^2+24t_f^4+6{\eta }^2+8t_f^2{\eta }_f^2)x^5$

       $+\frac{1}{5040N_f^{7}cos{\phi }_f}(-61-662t_f^2-1320t_f^4-720t_f^6)x^7+...$

  footpoint latitude${\phi }_f$由级数展开计算：

$${\phi }_f=\overline{y}+\overline{\beta }sin2\overline{y}+\overline{\gamma }sin4\overline{y}+\overline{\delta }sin6\overline{y}+\overline{ϵ}sin8\overline{y}+...$$

  其中：

      $\overline{\alpha }=\frac{a+b}{2}(1+\frac{1}{4}{n}^2+\frac{1}{64}{n}^4+...)$

      $\overline{\beta }=\frac{3}{2}n-\frac{27}{32}{n}^3+\frac{269}{512}{n}^5+...$

      $\overline{\gamma }=\frac{21}{16}{n}^2-\frac{55}{32}{n}^4+...$

      $\overline{\delta }=\frac{151}{96}{n}^3-\frac{417}{128}{n}^5+...$

      $\overline{ϵ}=\frac{1097}{512}{n}^4+...$

      $\overline{y}=\frac{y}{\overline{\alpha }}$

  注意系数$\overline{\alpha }$与$\alpha$ in(10.23)相同，带有下标f的项必须根据 ${\phi }_f$计算。

      $t_f=tan({\phi }_f)$

      ${\eta }_f^2=e^{{}^{\prime }2}co{s}^2({\phi }_f)$

      $N_f=\frac{a^2}{b\sqrt{1+{\eta }_f^2}}$

  下表给出了两种椭球对应的参数：

计算好的参数	贝塞尔椭球	WGS-84椭球
$\overline{\alpha }$	6366742.5203m	6367449.1458m
$\overline{\beta }$	$2.51127324\cdot 10^{-3}$	$2.51882658\cdot 10^{-3}$
$\overline{\gamma }$	$3.67879\cdot 10^{-6}$	$3.70095\cdot 10^{-6}$
$\overline{\delta }$	$7.38\cdot 10^{-9}$	$7.45\cdot 10^{-9}$
$\overline{ϵ}$	$17\cdot 10^{-12}$	$17\cdot 10^{-12}$

本章代码

#ifndef __COORDINATESYSTEM_H__
#define __COORDINATESYSTEM_H__

#include 
#include 
#include 

double pi = 3.14159265358979;

// 椭球模型常数
double a = 6378137.0;                 // 长半轴(ellipsoid major axis)
double b = 6356752.3142451795;        // 短半轴(ellipsoid minor axis)
double scale = 0.9996;                // 缩放因子

// 经纬度
struct WGS84Coord
{
	double latitude;
	double longitude;
	double height;
};

// UTM坐标系
struct UTMCoord
{
	double x;
	double y;
};  

// ECEF坐标系
struct ECEFCoord
{
	double x;
	double y;
	double z;
};

// ENU坐标系
struct ENUCoord
{
	double e;
	double n;
	double u;
};

struct ENUAxis
{
	double x;
	double y;
	double z;
};

// 角度转弧度
double DegToRad(double deg)
{
	return (deg / 180.0 * pi);
}

// 弧度转角度
inline double RadToDeg(double rad)
{
	return (rad / pi * 180.0);
}

// 计算从赤道到待映射点的椭球距离，单位为米。
double ArcLengthOfMeridian(double phi)
{
	double alpha, beta, gamma, delta, epsilon, n;

	n = (a - b) / (a + b);

	alpha = ((a + b) / 2.0) * (1.0 + (pow(n, 2.0) / 4.0) + (pow(n, 4.0) / 64.0));

	beta = (-3.0 * n / 2.0) + (9.0 * pow(n, 3.0) / 16.0) + (-3.0 * pow(n, 5.0) / 32.0);

	gamma = (15.0 * pow(n, 2.0) / 16.0) + (-15.0 * pow(n, 4.0) / 32.0);

	delta = (-35.0 * pow(n, 3.0) / 48.0) + (105.0 * pow(n, 5.0) / 256.0);

	epsilon = (315.0 * pow(n, 4.0) / 512.0);

	double result = alpha * (phi + (beta * sin(2.0 * phi)) + (gamma * sin(4.0 * phi)) + (delta * sin(6.0 * phi)) + (epsilon * sin(8.0 * phi)));

	return result;
}

// 计算footpoint latitude
double FootpointLatitude(double y)
{
	double y_, alpha_, beta_, gamma_, delta_, epsilon_, n;

	n = (a - b) / (a + b);

	alpha_ = ((a + b) / 2.0) * (1 + (pow(n, 2.0) / 4) + (pow(n, 4.0) / 64));

	y_ = y / alpha_;

	beta_ = (3.0 * n / 2.0) + (-27.0 * pow(n, 3.0) / 32.0) + (269.0 * pow(n, 5.0) / 512.0);

	gamma_ = (21.0 * pow(n, 2.0) / 16.0) + (-55.0 * pow(n, 4.0) / 32.0);

	delta_ = (151.0 * pow(n, 3.0) / 96.0) + (-417.0 * pow(n, 5.0) / 128.0);

	epsilon_ = (1097.0 * pow(n, 4.0) / 512.0);

	double result = y_ + (beta_ * sin(2.0 * y_)) + (gamma_ * sin(4.0 * y_)) + (delta_ * sin(6.0 * y_)) + (epsilon_ * sin(8.0 * y_));

	return result;
}

// 计算UTM投影带中央子午线(角度制)
double CalCulateUTMCentralMeridian(double latitude, double longitude)
{
	// A区
	if (longitude < 0.0 && latitude < -80.0)
	{
		return -90.0;
	}
	// B区
	if (longitude >= 0.0 && latitude < -80.0)
	{
		return 90.0;
	}
	// 31V区
	if (longitude >= 0.0 && longitude < 3.0 && latitude >= 56.0 && latitude < 64.0)
	{
		return 1.5;
	}
	// 32V区
	if (longitude >= 3.0 && longitude < 12.0 && latitude >= 56.0 && latitude < 64.0)
	{
		return 7.5;
	}
	// 31X区
	if (longitude >= 0.0 && longitude < 9.0 && latitude >= 72.0 && latitude < 84.0)
	{
		return 4.5;
	}
	// 33X区
	if (longitude >= 9.0 && longitude < 21.0 && latitude >= 72.0 && latitude < 84.0)
	{
		return 15.0;
	}
	// 35X区
	if (longitude >= 21.0 && longitude < 33.0 && latitude >= 72.0 && latitude < 84.0)
	{
		return 27.0;
	}
	// 37X区
	if (longitude >= 33.0 && longitude < 42.0 && latitude >= 72.0 && latitude < 84.0)
	{
		return 37.5;
	}
	// Y区
	if (longitude < 0.0 && latitude >= 84.0)
	{
		return -90.0;
	}
	// A区
	if (longitude >= 0.0 && latitude >= 84.0)
	{
		return 90.0;
	}

	int zone = (int)longitude / 6 + 31;
	return (-183.0 + zone * 6.0);
}

// 计算UTM投影带中央子午线(角度制)
double CalCulateUTMCentralMeridian(std::string zone)
{
	// A区
	if (zone == "A")
	{
		return -90.0;
	}
	// B区
	if (zone == "B")
	{
		return 90.0;
	}
	// 31V区
	if (zone == "31V")
	{
		return 1.5;
	}
	// 32V区
	if (zone == "32V")
	{
		return 7.5;
	}
	// 31X区
	if (zone == "31X")
	{
		return 4.5;
	}
	// 33X区
	if (zone == "33X")
	{
		return 15.0;
	}
	// 35X区
	if (zone == "35X")
	{
		return 27.0;
	}
	// 37X区
	if (zone == "37X")
	{
		return 37.5;
	}
	// Y区
	if (zone == "Y")
	{
		return -90.0;
	}
	// Z区
	if (zone == "Z")
	{
		return 90.0;
	}

	zone.substr(0, 1);
	int zone_num = std::stoi(zone.substr(0, 1));

	return (-183.0 + zone_num * 6.0);
}

// 经纬度转UTM
void LatLonToUTM(WGS84Coord latlon, UTMCoord& xy)
{
	double phi = latlon.latitude;
	double lambda = latlon.longitude;

	// 计算投影带中央子午线
	double center_lambda_deg = CalCulateUTMCentralMeridian(phi, lambda);
	double center_lambda_rad = DegToRad(center_lambda_deg);
	double lambda0 = center_lambda_rad;

	double N, ep2, eta2, t, t2, ell;
	double x_coef1, x_coef2, x_coef3, y_coef1, y_coef2, y_coef3;
	double tmp;

	ep2 = (pow(a, 2.0) - pow(b, 2.0)) / pow(b, 2.0);
	
	eta2 = ep2 * pow(cos(phi), 2.0);

	N = pow(a, 2.0) / (b * sqrt(1 + eta2));

	t = tan(phi);
	t2 = t * t;

	ell = lambda - lambda0;

	x_coef1 = 1.0 - t2 + eta2;
	x_coef2 = 5.0 - 18.0 * t2 + (t2 * t2) + 14.0 * eta2 - 58.0 * t2 * eta2;
	x_coef3 = 61.0 - 479.0 * t2 + 179.0 * (t2 * t2) - (t2 * t2 * t2);

	xy.x = N * cos(phi) * ell + (N / 6.0 * pow(cos(phi), 3.0) * x_coef1 * pow(ell, 3.0))
		+ (N / 120.0 * pow(cos(phi), 5.0) * x_coef2 * pow(ell, 5.0))
		+ (N / 5040.0 * pow(cos(phi), 7.0) * x_coef3 * pow(ell, 7.0));

	y_coef1 = 5.0 - t2 + 9 * eta2 + 4.0 * (eta2 * eta2);
	y_coef2 = 61.0 - 58.0 * t2 + (t2 * t2) + 270.0 * eta2 - 330.0 * t2 * eta2;
	y_coef3 = 1385.0 - 3111.0 * t2 + 543.0 * (t2 * t2) - (t2 * t2 * t2);

	xy.y = ArcLengthOfMeridian(phi)
		+ (t / 2.0 * N * pow(cos(phi), 2.0) * pow(ell, 2.0))
		+ (t / 24.0 * N * pow(cos(phi), 4.0) * y_coef1 * pow(ell, 4.0))
		+ (t / 720.0 * N * pow(cos(phi), 6.0) * y_coef2 * pow(ell, 6.0))
		+ (t / 40320.0 * N * pow(cos(phi), 8.0) * y_coef3 * pow(ell, 8.0));

	// 调整UTM东和北值
	xy.x = xy.x * scale + 500000.0;
	xy.y = xy.y * scale;
	if (xy.y < 0.0)
		xy.y += 10000000.0;
}

// UTM转经纬度
void UTMToLatLon(UTMCoord xy, bool southhemi, std::string zone, WGS84Coord& latlon)
{
	double x = xy.x;
	double y = xy.y;

	x -= 500000.0;
	x /= scale;

	// 南半球
	if (southhemi)
		y -= 10000000.0;

	y /= scale;

	// 计算投影带中央子午线
	double center_lambda_deg = CalCulateUTMCentralMeridian(zone);
	double center_lambda_rad = DegToRad(center_lambda_deg);

	double lambda0 = center_lambda_rad;

	double phif, Nf, Nf2, Nf3, Nf4, Nf5, Nf6, Nf7,Nf8, etaf2, ep2, tf, tf2, tf4, cf;
	double lamba_coef1, lamba_coef2, lamba_coef3, lamba_coef4;
	double phi_coef1, phi_coef2, phi_coef3, phi_coef4;

	phif = FootpointLatitude(y);

	ep2 = (pow(a, 2.0) - pow(b, 2.0)) / pow(b, 2.0);

	cf = cos(phif);

	etaf2 = ep2 * pow(cf, 2.0);

	Nf = pow(a, 2.0) / (b * sqrt(1 + etaf2));
	Nf2 = Nf * Nf;
	Nf3 = Nf2 * Nf;
	Nf4 = Nf3 * Nf;
	Nf5 = Nf4 * Nf;
	Nf6 = Nf5 * Nf;
	Nf7 = Nf6 * Nf;
	Nf8 = Nf7 * Nf;

	tf = tan(phif);
	tf2 = tf * tf;
	tf4 = tf2 * tf2;

	lamba_coef1 = 1.0 / (Nf * cf);
	lamba_coef2 = 1.0 / (6.0 * Nf3 * cf) * (-1.0 - 2 * tf2 - etaf2);
	lamba_coef3 = 1.0 / (120.0 * Nf5 * cf) * (5.0 + 28.0 * tf2 + 24.0 * tf4 + 6.0 * etaf2 + 8.0 * tf2 * etaf2);
	lamba_coef4 = 1.0 / (5040.0 * Nf7 * cf) * (-61.0 - 662.0 * tf2 - 1320.0 * tf4 - 720.0 * (tf4 * tf2));
	latlon.longitude = lambda0 + lamba_coef1 * x + lamba_coef2 * pow(x, 3.0) + lamba_coef3 * pow(x, 5.0) + lamba_coef4 * pow(x, 7.0);

	phi_coef1 = tf / (2.0 * Nf2) * (-1.0 - etaf2);
	phi_coef2 = tf / (24.0 * Nf4) * (5.0 + 3.0 * tf2 + 6.0 * etaf2 - 6.0 * tf2 * etaf2 - 3.0 * (etaf2 * etaf2) - 9.0 * tf2 * (etaf2 * etaf2));
	phi_coef3 = tf / (720.0 * Nf6) * (-61.0 - 90.0 * tf2 - 45.0 * tf4 - 107.0 * etaf2 + 162.0 * tf2 * etaf2);
	phi_coef4 = tf / (40320.0 * Nf8) * (1385.0 + 3633.0 * tf2 + 4095.0 * tf4 + 1575 * (tf4 * tf2));
	latlon.latitude = phif + phi_coef1 * pow(x, 2.0) + phi_coef2 * pow(x, 4.0) + phi_coef3 * pow(x, 6.0) + phi_coef4 * pow(x, 8.0);
}

// 经纬度转ECEF
void LatLonHeiToECEF(WGS84Coord llh, ECEFCoord& xyz)
{
	double phi = llh.latitude;
	double lambda = llh.longitude;
	double height = llh.height;
	double sin_phi = std::sin(phi);
	double cos_phi = std::cos(phi);
	double sin_lambda = std::sin(lambda);
	double cos_lambda = std::cos(lambda);

	double N = a * a / std::sqrt(a * a * cos_phi * cos_phi + b * b * sin_phi * sin_phi);

	xyz.x = (N + height) * cos_phi * cos_lambda;
	xyz.y = (N + height) * cos_phi * sin_lambda;
	xyz.z = (b * b / (a * a) * N + height) * sin_phi;
}

// ECEF转经纬度(近似法)
void ECEFtoLatLonHei(ECEFCoord xyz,WGS84Coord& llh)
{
	double X = xyz.x;
	double Y = xyz.y;
	double Z = xyz.z;

	double p = std::sqrt(X * X + Y * Y);
	double theta = std::atan(Z * a / (p * b));
	double sin_theta = std::sin(theta);
	double cos_theta = std::cos(theta);


	// first numerical eccentricity
	double e2 = (a * a - b * b) / (a * a);
	// second numerical eccentricity
	double ep2 = (a * a - b * b) / (b * b);
	
	double phi = std::atan((Z + ep2 * b * std::pow(sin_theta, 3)) / (p - e2 * a * std::pow(cos_theta, 3)));
	double lambda = std::atan(Y / X);

	double sin_phi = std::sin(phi);
	double cos_phi = std::cos(phi);
	double N = a * a / std::sqrt(a * a * cos_phi * cos_phi + b * b * sin_phi * sin_phi);
	double h = p / std::cos(phi) - N;

	llh.latitude = phi;
	llh.longitude = lambda;
	llh.height = h;
}

// 输入局部原点和某个点的wgs84坐标，计算某个点的enu坐标
void CalculateEnuXYZ(WGS84Coord original, WGS84Coord llh, ENUCoord& xyz)
{	
	double phi_i = original.latitude;
	double lambda_i = original.longitude;
	double sin_phi_i = std::sin(phi_i);
	double cos_phi_i = std::cos(phi_i);
	double sin_lambda_i = std::sin(lambda_i);
	double cos_lambda_i = std::cos(lambda_i);

	// 局部坐标系的三个主轴
	ENUAxis e, n, u;

	e.x = -sin_lambda_i;
	e.y = cos_lambda_i;
	e.z = 0;

	n.x = -sin_phi_i * cos_lambda_i;
	n.y = -sin_phi_i * sin_lambda_i;
	n.z = cos_phi_i;

	u.x = cos_phi_i * cos_lambda_i;
	u.y = cos_phi_i * sin_lambda_i;
	u.z = sin_phi_i;

	// 计算原点和某个点的相对坐标值
	ECEFCoord X_i, X_j, X_ij;
	LatLonHeiToECEF(original, X_i);
	LatLonHeiToECEF(llh, X_j);

	X_ij.x = X_j.x - X_i.x;
	X_ij.y = X_j.y - X_i.y;
	X_ij.z = X_j.z - X_i.z;

    // 输出enu坐标值
	xyz.e = e.x * X_ij.x + e.y * X_ij.y + e.z * X_ij.z;
	xyz.n = n.x * X_ij.x + n.y * X_ij.y + n.z * X_ij.z;
	xyz.u = u.x * X_ij.x + u.y * X_ij.y + u.z * X_ij.z;
}

#endif //__COORDINATESYSTEM_H__