东北天坐标如何显示到opencv中 北天东坐标系转换

坐标系的转换Matlab实现

常用的几种导航坐标系

1. 大地坐标系，WGS84（WorldGeodeticCoordinateSystem1984）

这是为GPS全球定位系统建立的坐标系统。WGS-84坐标系的原点在地球质心，Z轴指向BIH1984.0定义的协定地球极（CTP）方向，X轴指向BIH1984.0的零度子午面和CTP赤道的交点，Y轴和Z、X轴构成右手坐标系。其参数为经度、纬度、海拔高度。

其基本参数如下：

　　　　　　长半径：a=6378137±2（m）；

　　　　　　地球引力和地球质量的乘积：GM=3986005×108m3s-2±0.6×108m3s-2；

　　　　　　正常化二阶带谐系数：C20=-484.16685×10-6±1.3×10-9；

　　　　　　地球重力场二阶带球谐系数：J2=108263×10-8

　　　　　　地球自转角速度：ω=7292115×10-11rads-1±0.150×10-11rads-1

　　　　　　扁率f=0.003352810664

2. 地心地固坐标系

ECEF坐标系与地球固联，且随着地球转动。图中O即为坐标原点，位置在地球质心。X轴通过格林尼治线和赤道线的交点，正方向为原点指向交点方向。Z轴通过原点指向北极。Y轴与X、Z轴构成右手坐标系。

3. 东北天坐标系

也叫站心坐标系以用户所在位置P为坐标原点。

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

这三种坐标系可以相互转换，具体的转换原理这里不作详细解释，具体的理论公式可以参考这篇文章坐标系转换原理，这里只介绍三种坐标系互相转换的matlab实现，通常情况下，这三种坐标系不方便直接相互转换，但是可以通过中间某个坐标系达到互相转换的目的。具体的转换示意图如下图所示

其中，经纬度和东北天坐标系的转换需要经过地心坐标系的转换才能实现，而地心坐标系可以和其他两个坐标系进行相互的转换。

此外，对于经纬度坐标系和地心坐标系来说，地球表面的任意一个位置都有与其对应的唯一一个坐标，而东北天坐标系对于某个位置的坐标并不是唯一确定的，取决于原点的放置。因此，对于涉及到东北天坐标系的转换都要涉及到远点位置的设置。

matlab实现代码

地心坐标系转经纬度坐标系

不涉及原点的设置，坐标一一对应，只需要将xyz的坐标输入即可。

function llh = xyz2llh(xyz)
%XYZ2LLH	Convert from ECEF cartesian coordinates to 
%               latitude, longitude and height.  WGS-84
%
%	llh = XYZ2LLH(xyz)	
%
%    INPUTS
%	xyz(1) = ECEF x-coordinate in meters
%	xyz(2) = ECEF y-coordinate in meters
%	xyz(3) = ECEF z-coordinate in meters
%
%    OUTPUTS
%	llh(1) = latitude in radians
%	llh(2) = longitude in radians
%	llh(3) = height above ellipsoid in meters

	x = xyz(1);
	y = xyz(2);
	z = xyz(3);
	x2 = x^2;
	y2 = y^2;
	z2 = z^2;

	a = 6378137.0000;	% earth radius in meters
	b = 6356752.3142;	% earth semiminor in meters	
	e = sqrt (1-(b/a).^2);
	b2 = b*b;
	e2 = e^2;
	ep = e*(a/b);
	r = sqrt(x2+y2);
	r2 = r*r;
	E2 = a^2 - b^2;
	F = 54*b2*z2;
	G = r2 + (1-e2)*z2 - e2*E2;
	c = (e2*e2*F*r2)/(G*G*G);
	s = ( 1 + c + sqrt(c*c + 2*c) )^(1/3);
	P = F / (3 * (s+1/s+1)^2 * G*G);
	Q = sqrt(1+2*e2*e2*P);
	ro = -(P*e2*r)/(1+Q) + sqrt((a*a/2)*(1+1/Q) ...
                                - (P*(1-e2)*z2)/(Q*(1+Q)) - P*r2/2);
	tmp = (r - e2*ro)^2;
	U = sqrt( tmp + z2 );
	V = sqrt( tmp + (1-e2)*z2 );
	zo = (b2*z)/(a*V);

	height = U*( 1 - b2/(a*V) );
	
	lat = atan( (z + ep*ep*zo)/r );

	temp = atan(y/x);
	if x >=0	
		long = temp;
	elseif (x < 0) & (y >= 0)
		long = pi + temp;
	else
		long = temp - pi;
	end

	llh(1) = lat*180/pi;
	llh(2) = long*180/pi;
	llh(3) = height;

经纬度坐标系转地心坐标系

同样不涉及原点的设置，与上面是反变换的过程

function xyz = llh2xyz(llh)
%LLH2XYZ  Convert from latitude, longitude and height
%         to ECEF cartesian coordinates.  WGS-84
%
%	xyz = LLH2XYZ(llh)	
%
%	llh(1) = latitude in degrees 纬度
%	llh(2) = longitude in degrees 经度
%	llh(3) = height above ellipsoid in meters
%
%	xyz(1) = ECEF x-coordinate in meters
%	xyz(2) = ECEF y-coordinate in meters
%	xyz(3) = ECEF z-coordinate in meters
	phi = llh(1);
	lambda = llh(2);
	h = llh(3);

	a = 6378137.0000;	% earth semimajor axis in meters
	b = 6356752.3142;	% earth semiminor axis in meters	
	e = sqrt (1-(b/a).^2);

	sinphi = sind(phi);
	cosphi = cosd(phi);
	coslam = cosd(lambda);
	sinlam = sind(lambda);
	tan2phi = (tand(phi))^2;
	tmp = 1 - e*e;
	tmpden = sqrt( 1 + tmp*tan2phi );

	x = (a*coslam)/tmpden + h*coslam*cosphi;

	y = (a*sinlam)/tmpden + h*sinlam*cosphi;

	tmp2 = sqrt(1 - e*e*sinphi*sinphi);
	z = (a*tmp*sinphi)/tmp2 + h*sinphi;

	xyz(1) = x;
	xyz(2) = y;
	xyz(3) = z;

地心坐标系转东北天坐标系

这里涉及到原点的设置，orgxyz参数表示的是将orgxyz这个地心坐标作为东北天坐标系的原点位置

function enu = xyz2enu(xyz,orgxyz)
%XYZ2ENU	Convert from WGS-84 ECEF cartesian coordinates to 
%               rectangular local-level-tangent ('East'-'North'-Up)
%               coordinates.
%
%	enu = XYZ2ENU(xyz,orgxyz)	
%
%    INPUTS
%	xyz(1) = ECEF x-coordinate in meters
%	xyz(2) = ECEF y-coordinate in meters
%	xyz(3) = ECEF z-coordinate in meters
%
%	orgxyz(1) = ECEF x-coordinate of local origin in meters
%	orgxyz(2) = ECEF y-coordinate of local origin in meters
%	orgxyz(3) = ECEF z-coordinate of local origin in meters
%
%    OUTPUTS
%       enu:  Column vector
%		enu(1,1) = 'East'-coordinate relative to local origin (meters)
%		enu(2,1) = 'North'-coordinate relative to local origin (meters)
%		enu(3,1) = Up-coordinate relative to local origin (meters)

if nargin<2,error('insufficient number of input arguments'),end
tmpxyz = xyz;
tmporg = orgxyz;
% if size(tmpxyz) ~= size(tmporg), tmporg=tmporg'; end,
% difxyz = zeros(size(tmpxyz),3);
difxyz(:,1) = tmpxyz(:,1) - tmporg(1);
difxyz(:,2) = tmpxyz(:,2) - tmporg(2);
difxyz(:,3) = tmpxyz(:,3) - tmporg(3);
% [m,n] = size(difxyz); if m<n, difxyz=difxyz'; end,
orgllh = xyz2llh(orgxyz);
phi = orgllh(1);
lam = orgllh(2);
sinphi = sind(phi);
cosphi = cosd(phi);
sinlam = sind(lam);
coslam = cosd(lam);
R = [ -sinlam          coslam         0     ; ...
      -sinphi*coslam  -sinphi*sinlam  cosphi; ...
       cosphi*coslam   cosphi*sinlam  sinphi];
enu = (R*difxyz')';

东北天坐标系转地心坐标系

这里同样是要确定一个地心坐标系下的坐标作为东北天坐标系下的原点

function xyz = enu2xyz(enu, orgxyz)
%ENU2XYZ	Convert from rectangular local-level-tangent ('East'-'North'-Up) to WGS-84 ECEF cartesian coordinates coordinates.
% xyz = ENU2XYZ(enu,orgenu)
tmporg = orgxyz;
orgllh = xyz2llh(orgxyz);
phi = orgllh(1);
lam = orgllh(2);
sinphi = sind(phi);
cosphi = cosd(phi);
sinlam = sind(lam);
coslam = cosd(lam);
R = [ -sinlam          coslam         0     ; ...
      -sinphi*coslam  -sinphi*sinlam  cosphi; ...
       cosphi*coslam   cosphi*sinlam  sinphi];

difxyz = (R^(-1) * enu')';
tmpxyz(:,1) = difxyz(:,1) + tmporg(1);
tmpxyz(:,2) = difxyz(:,2) + tmporg(2);
tmpxyz(:,3) = difxyz(:,3) + tmporg(3);
xyz = tmpxyz;
end

通过以上四个函数，可以很容易地写出东北天坐标系和经纬度坐标系地转换，这也是最经常用到的，只需要先将llh坐标转换成xyz坐标再确定某一个原点位置将xyz坐标转换成东北天坐标，注意这里的llh2xyz是针对一个坐标转换的，所以这里有一个循环，而xyz2enu是可以一次性处理多个坐标。

% 确定一个初始位置
orgllh = [39.994074,-0.06945642,0];
orgxyz = llh2xyz(orgllh);

% llh 这里赋值你的经纬度矩阵，所以下面用一个循环
for i=1:size(llh,1)
    xyz(i,:)=llh2xyz(llh(i,:));
end
enu = xyz2enu(xyz,orgxyz);