数学模型：

两个空间直角坐标系进行转换，需要7个参数来描述，分别是3个旋转角，3个平移距离，1个比例因子。
如果知道3个点在两个坐标系中分别的位置，那么这7个参数可以唯一确定。
坐标转换的数学模型为：

⎡⎣⎢XYZ⎤⎦⎥T=λ⎡⎣⎢ΔXΔYΔZ⎤⎦⎥+λR⎡⎣⎢XYZ⎤⎦⎥S(1)

{\left[ {\begin{array}{*{20}{c}} X\\ Y\\ Z \end{array}} \right]_T} = \lambda \left[ {\begin{array}{*{20}{c}} {\Delta X}\\ {\Delta Y}\\ {\Delta Z} \end{array}} \right] + \lambda R{\left[ {\begin{array}{*{20}{c}} X\\ Y\\ Z \end{array}} \right]_S}(1)
其中，T代表转换后的坐标，S代表转换前的坐标。 R是旋转矩阵，Δ是平移向量。
比例因子λ最容易确定，是两个刚体对应边长的比。平移向量Δ需要在旋转矩阵R确定后才能确定，所以旋转矩阵R的确定是参数解算的核心。R是3x3的矩阵，但实际上只用3个参数就可以表示。
这里利用反对称矩阵S来构造旋转矩阵，设反对称矩阵

S=⎡⎣⎢0cb−c0a−b−a0⎤⎦⎥

S{\rm{ = }}\left[ {\begin{array}{*{20}{c}} 0}c}\\ c&0\\ b&a&0 \end{array}} \right]

那么

R=(I+S)(I−S)−1(2)

R = (I + S){(I - S)^{ - 1}}(2)
其中I是单位矩阵。
那么只要求出a,b,c ，旋转矩阵R 就可以确定。
对于每个点，代入式（1）可以得到一组3个方程。用2点减去1点，消去平移参数Δ ，并与式（2）联立，得到：

λ(I+S)⎡⎣⎢XS2−XS1YS2−YS1ZS2−ZS1⎤⎦⎥=(I−S)⎡⎣⎢XT2−XT1YT2−YT1ZT2−ZT1⎤⎦⎥(3)

\lambda (I + S)\left[ {\begin{array}{*{20}{c}} {{X_{S2}} - {X_{S1}}}\\ {{Y_{S2}} - {Y_{S1}}}\\ {{Z_{S2}} - {Z_{S1}}} \end{array}} \right] = (I - S)\left[ {\begin{array}{*{20}{c}} {{X_{T2}} - {X_{T1}}}\\ {{Y_{T2}} - {Y_{T1}}}\\ {{Z_{T2}} - {Z_{T1}}} \end{array}} \right](3)
展开并整理（3）式，得到

⎡⎣⎢0−λZS21−ZT21λYS21+YT21−λZS21−ZT210λXS21+XT21−λYS21−YT21λXS21+XT210⎤⎦⎥⎡⎣⎢abc⎤⎦⎥=⎡⎣⎢XT21−λXS21YT21−λYS21ZT21−λZS21⎤⎦⎥(4)

\left[ {\begin{array}{*{20}{c}} 0} - {Z_{T21}}}} - {Y_{T21}}}\\ { - \lambda {Z_{S21}} - {Z_{T21}}}&0} + {X_{T21}}}\\ {\lambda {Y_{S21}} + {Y_{T21}}}} + {X_{T21}}}&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} a\\ b\\ c \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{X_{T21}} - \lambda {X_{S21}}}\\ {{Y_{T21}} - \lambda {Y_{S21}}}\\ {{Z_{T21}} - \lambda {Z_{S21}}} \end{array}} \right](4)

其中

XS21=XS2−XS1

{X_{S21}}{\rm{ = }}{X_{S2}} - {X_{S1}}
其他类似。

(4)式只有两个独立方程，不能解出 a,b,c三个参数。
用3点所列方程减去1点，得到和（4）类似的一组方程。这组方程和（4）联立，取3个独立方程，有

⎡⎣⎢0w2v3−w20u3−v2u20⎤⎦⎥⎡⎣⎢abc⎤⎦⎥=⎡⎣⎢XT21−λXS21YT21−λYS21ZT31−λZS31⎤⎦⎥(5)

\left[ {\begin{array}{*{20}{c}} 0}}\\ {{w_2}}&0}\\ {{v_3}}}&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} a\\ b\\ c \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{X_{T21}} - \lambda {X_{S21}}}\\ {{Y_{T21}} - \lambda {Y_{S21}}}\\ {{Z_{T31}} - \lambda {Z_{S31}}} \end{array}} \right](5)

其中

u2=λXS21+XT21

{u_2} = \lambda {X_{S21}} + {X_{T21}}

v2=λYS21+YT21

{v_2} = \lambda {Y_{S21}} + {Y_{T21}}

w2=λZS21+ZT21

{w_2} = \lambda {Z_{S21}} + {Z_{T21}}

u3=λXS31+XT31

{u_3} = \lambda {X_{S31}} + {X_{T31}}

v3=λYS31+YT31

{v_3} = \lambda {Y_{S31}} + {Y_{T31}}

w3=λZS31+ZT31

{w_3} = \lambda {Z_{S31}} + {Z_{T31}}

由（5）式解出a,b,c 为：

⎡⎣⎢abc⎤⎦⎥=1H⎡⎣⎢u2u3−u2v3u3w2u3v2−v2v3v3w2u2w2−v2w2w2w2⎤⎦⎥⎡⎣⎢XT21−λXS21YT21−λYS21ZT31−λZS31⎤⎦⎥(6)

\left[ {\begin{array}{*{20}{c}} a\\ b\\ c \end{array}} \right] = \frac{1}{H}\left[ {\begin{array}{*{20}{c}} {{u_2}{u_3}}{v_2}}{w_2}}\\ { - {u_2}{v_3}}{v_3}}{w_2}}\\ {{u_3}{w_2}}{w_2}}{w_2}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{X_{T21}} - \lambda {X_{S21}}}\\ {{Y_{T21}} - \lambda {Y_{S21}}}\\ {{Z_{T31}} - \lambda {Z_{S31}}} \end{array}} \right](6)

其中

H=u2v3w2−u2v3w2

H = {u_2}{v_3}{w_2} - {u_2}{v_3}{w_2}
解出a,b,c后，由（2）式还原为旋转矩阵R。
再代入任意一点，由（1）式解出平移向量Δ。

算法实现

CoordinateTransformation.h

#pragma once
#include <iostream>
#include "eigen3\Eigen\Dense"
using namespace std;
using namespace Eigen;
//using Eigen::MatrixXd;class CoordinateTransformation
{
public:CoordinateTransformation();~CoordinateTransformation();private://3个对应点,用来解算7个参数Vector3d m_CalOriPosition[3];Vector3d m_CalNewPosition[3];Vector3d m_RigidBodyPoint[3];//7个参数，double m_ratio;Vector3d m_Translation; //3个参数Matrix3d m_Rotation;  //3个参数，由abc算出来，计算时解算的是abcMatrix3d m_AntiSymmetry;   //反对称矩阵，包含3个参数/*|0  -c  -b  |S   =   |c  0   -a  |    R=（I+S)*(I-S)^(-1)|b  a   0   |*///给定的输入和计算的输出Vector3d m_InputOriPosition;Vector3d m_OutputNewPosition;public://设置标定点原始点1 2 3void SetCalOriPoint1(Vector3d vector);void SetCalOriPoint2(Vector3d vector);void SetCalOriPoint3(Vector3d vector);//设置标定点当前点1 2 3void SetCalNewPoint1(Vector3d vector);void SetCalNewPoint2(Vector3d vector);void SetCalNewPoint3(Vector3d vector);//设置刚体上的3个点在刚体坐标系中的位置void SetRigidBodyPoint1(Vector3d vector);void SetRigidBodyPoint2(Vector3d vector);void SetRigidBodyPoint3(Vector3d vector);//标定void cali();//计算Vector3d calc(Vector3d input);//打印标定结果void printcaliresult();//计算刚体poseMatrix4d calcpose(Vector3d point1, Vector3d point2, Vector3d point3);private:void Init();                      void caliratio();           //计算比例m_ratiovoid caliRotation();        //计算旋转矩阵m_Rotationvoid caliTranslation();     //计算平移向量m_Translation
};

CoordinateTransformation.cpp

#include "CoordinateTransformation.h"CoordinateTransformation::CoordinateTransformation()
{//给标定的3对点赋初值Init();
}CoordinateTransformation::~CoordinateTransformation()
{
}void CoordinateTransformation::cali()
{//计算比例m_ratiocaliratio();//计算旋转矩阵caliRotation();//计算平移向量caliTranslation();
}//初始化
void CoordinateTransformation::Init()
{Vector3d temp1(1, 0, 0);SetCalOriPoint1(temp1);Vector3d temp11(1, 0, 0);SetCalNewPoint1(temp11);Vector3d temp2(0, 1, 0);SetCalOriPoint2(temp2);Vector3d temp22(0, 1, 0);SetCalNewPoint2(temp22);Vector3d temp3(0, 0, 1);SetCalOriPoint3(temp3);Vector3d temp33(0, 0, 1);SetCalNewPoint3(temp33);m_ratio = 1;m_Translation = Vector3d(0, 0, 0);m_Rotation <<1, 0, 0,0, 1, 0,0, 0, 1;
}//给6个标定点赋初值
void CoordinateTransformation::SetCalOriPoint1(Vector3d vector)
{m_CalOriPosition[0] = vector;
}void CoordinateTransformation::SetCalOriPoint2(Vector3d vector)
{m_CalOriPosition[1] = vector;
}void CoordinateTransformation::SetCalOriPoint3(Vector3d vector)
{m_CalOriPosition[2] = vector;
}void CoordinateTransformation::SetCalNewPoint1(Vector3d vector)
{m_CalNewPosition[0] = vector;
}void CoordinateTransformation::SetCalNewPoint2(Vector3d vector)
{m_CalNewPosition[1] = vector;
}void CoordinateTransformation::SetCalNewPoint3(Vector3d vector)
{m_CalNewPosition[2] = vector;
}void CoordinateTransformation::SetRigidBodyPoint1(Vector3d vector)
{m_RigidBodyPoint[0] = vector;
}void CoordinateTransformation::SetRigidBodyPoint2(Vector3d vector)
{m_RigidBodyPoint[1] = vector;
}void CoordinateTransformation::SetRigidBodyPoint3(Vector3d vector)
{m_RigidBodyPoint[2] = vector;//赋值完成后把3个数都赋值给标定用的点for (int i = 0; i < 3; i++){m_CalOriPosition[i] = m_RigidBodyPoint[i];}}//计算结果
Vector3d CoordinateTransformation::calc(Vector3d input)
{m_InputOriPosition = input;//计算m_OutputNewPosition = m_ratio*(m_Translation+ m_Rotation*m_InputOriPosition);return m_OutputNewPosition;
}//计算刚体pose
Matrix4d CoordinateTransformation::calcpose(Vector3d point1, Vector3d point2, Vector3d point3)
{m_CalNewPosition[0] = point1;m_CalNewPosition[1] = point2;m_CalNewPosition[2] = point3;cali();Matrix4d result;for (int i = 0; i < 3; i++){for (int j = 0; j < 3; j++){result(i, j) = m_Rotation(i, j);}}for (int i = 0; i < 3; i++){result(i, 3) = m_Translation(i, 0);}result(3, 0) = 0;result(3, 1) = 0;result(3, 2) = 0;result(3, 3) = 1;return result;
}//标定比例
void CoordinateTransformation::caliratio()
{//计算比例m_ratiodouble ratiotemp[3];double temp1 = 0;double temp2 = 0;//0和1点temp1 = 0;temp2 = 0;for (int i = 0; i < 3; i++){temp1 += pow((m_CalNewPosition[0](i, 0) - m_CalNewPosition[1](i, 0)), 2);temp2 += pow((m_CalOriPosition[0](i, 0) - m_CalOriPosition[1](i, 0)), 2);}ratiotemp[0] = sqrt(temp1 / temp2);//1和2点temp1 = 0;temp2 = 0;for (int i = 0; i < 3; i++){temp1 += pow((m_CalNewPosition[2](i, 0) - m_CalNewPosition[1](i, 0)), 2);temp2 += pow((m_CalOriPosition[2](i, 0) - m_CalOriPosition[1](i, 0)), 2);}ratiotemp[1] = sqrt(temp1 / temp2);//0和2点temp1 = 0;temp2 = 0;for (int i = 0; i < 3; i++){temp1 += pow((m_CalNewPosition[0](i, 0) - m_CalNewPosition[2](i, 0)), 2);temp2 += pow((m_CalOriPosition[0](i, 0) - m_CalOriPosition[2](i, 0)), 2);}ratiotemp[2] = sqrt(temp1 / temp2);//取平均值m_ratio = (ratiotemp[0] + ratiotemp[1] + ratiotemp[2]) / 3;
}void CoordinateTransformation::caliRotation()
{//计算旋转矩阵double w2, u2, v2, w3, u3, v3;//2点减1点w2 = m_ratio*(m_CalOriPosition[1](2, 0) - m_CalOriPosition[0](2, 0))    //(2,0)代表z，[0]代表1点，[1]代表2点+ (m_CalNewPosition[1](2, 0) - m_CalNewPosition[0](2, 0));u2 = m_ratio*(m_CalOriPosition[1](0, 0) - m_CalOriPosition[0](0, 0))    //(0,0)代表x，[0]代表1点，[1]代表2点+ (m_CalNewPosition[1](0, 0) - m_CalNewPosition[0](0, 0));v2 = m_ratio*(m_CalOriPosition[1](1, 0) - m_CalOriPosition[0](1, 0))    //(1,0)代表y，[0]代表1点，[1]代表2点+ (m_CalNewPosition[1](1, 0) - m_CalNewPosition[0](1, 0));//3点减1点w3 = m_ratio*(m_CalOriPosition[2](2, 0) - m_CalOriPosition[0](2, 0))    //(2,0)代表z，[0]代表1点，[2]代表3点+ (m_CalNewPosition[2](2, 0) - m_CalNewPosition[0](2, 0));u3 = m_ratio*(m_CalOriPosition[2](0, 0) - m_CalOriPosition[0](0, 0))    //(0,0)代表x，[0]代表1点，[2]代表3点+ (m_CalNewPosition[2](0, 0) - m_CalNewPosition[0](0, 0));v3 = m_ratio*(m_CalOriPosition[2](1, 0) - m_CalOriPosition[0](1, 0))    //(1,0)代表y，[0]代表1点，[2]代表3点+ (m_CalNewPosition[2](1, 0) - m_CalNewPosition[0](1, 0));double H = -u3*v2*w2 + u2*v3*w2;      //如果选择矩阵是单位矩阵，那么计算出的H是0Matrix3d uvw;uvw(0, 0) = u2*u3;uvw(0, 1) = u3*v2;uvw(0, 2) = u2*w2;uvw(1, 0) = -u2*v3;uvw(1, 1) = -v2*v3;uvw(1, 2) = -v2*w2;uvw(2, 0) = u3*w2;uvw(2, 1) = v3*w2;uvw(2, 2) = w2*w2;Vector3d abc;Vector3d xyz;xyz(0, 0) = -m_ratio*(m_CalOriPosition[1](0, 0) - m_CalOriPosition[0](0, 0))    //(0,0)代表x，[0]代表1点，[1]代表2点+ (m_CalNewPosition[1](0, 0) - m_CalNewPosition[0](0, 0));xyz(1, 0) = -m_ratio*(m_CalOriPosition[1](1, 0) - m_CalOriPosition[0](1, 0))    //(1,0)代表y，[0]代表1点，[1]代表2点+ (m_CalNewPosition[1](1, 0) - m_CalNewPosition[0](1, 0));xyz(2, 0) = -m_ratio*(m_CalOriPosition[2](2, 0) - m_CalOriPosition[0](2, 0))    //(2,0)代表z，[0]代表1点，[2]代表3点+ (m_CalNewPosition[2](2, 0) - m_CalNewPosition[0](2, 0));abc = (1 / H) * uvw * xyz;Matrix3d S;S(0, 0) = 0;S(0, 1) = -abc(2, 0);S(0, 2) = -abc(1, 0);S(1, 0) = abc(2, 0);S(1, 1) = 0;S(1, 2) = -abc(0, 0);S(2, 0) = abc(1, 0);S(2, 1) = abc(0, 0);S(2, 2) = 0;Matrix3d I;I.setIdentity();m_Rotation = (I + S)*((I - S).inverse());    //结果有问题，还需要再调试。再推导公式
}void CoordinateTransformation::caliTranslation()
{//计算平移矩阵m_Translation = (m_CalNewPosition[2] - m_ratio*m_Rotation*m_CalOriPosition[2]) / m_ratio;
}void CoordinateTransformation::printcaliresult()
{cout << "标定结果是" << endl;cout << "比例是" << m_ratio << endl;cout << "旋转矩阵是" << endl;cout << m_Rotation << endl;cout << "平移矩阵是" << endl;cout << m_Translation << endl;
}

使用例程

#include <iostream>
#include "eigen3\Eigen\Dense"
#include "CoordinateTransformation.h"
using namespace std;
using namespace Eigen;int main()
{//CoordinateTransformation类的使用例程//1.定义类的对象CoordinateTransformation my;//2.设置标定需要的3对点，其中 SetCalOriPoint1是变换之前的点，SetCalNewPoint1是变换之后的点//=================模拟三对点=================//Vector3d InputOriPosition1(1, 0, 0);   //变换之前Vector3d InputOriPosition2(0, 1, 0);Vector3d InputOriPosition3(0, 0, 1);Vector3d Translation(0.5, 1.05, 2.200);double ratio = 1;Matrix3d Rotation;       //旋转矩阵不能随便构造，它满足一定的约束关系，因为本质上旋转由3个参数决定，旋转矩阵中有9个数//旋转矩阵也不能是不转动Rotation <<1, 0, 0,0, 0.866, 0.5,0, -0.5, 0.866;cout << "test:" << Rotation(2,1)<<endl;  //-0.5Vector3d OutputNewPosition1 = ratio*(Translation + Rotation*InputOriPosition1);     //变换之后Vector3d OutputNewPosition2 = ratio*(Translation + Rotation*InputOriPosition2);Vector3d OutputNewPosition3 = ratio*(Translation + Rotation*InputOriPosition3);cout << "输入的坐标是" << endl << InputOriPosition1 << endl << InputOriPosition2 << endl << InputOriPosition3 << endl;cout << "输出的坐标是" << endl << OutputNewPosition1 << endl << OutputNewPosition2 << endl << OutputNewPosition3 << endl;//=================模拟三对点=================//my.SetCalOriPoint1(InputOriPosition1);my.SetCalOriPoint2(InputOriPosition2);my.SetCalOriPoint3(InputOriPosition3);my.SetCalNewPoint1(OutputNewPosition1);my.SetCalNewPoint2(OutputNewPosition2);my.SetCalNewPoint3(OutputNewPosition3);//实际标定时，新坐标上3个点的坐标是事先确定的，可以先赋值。然后把新坐标中这3个点在测量坐标系下的坐标在每次测量后由按钮触发赋值给SetCalOriPoint1//3.标定my.cali();//4.验证结果是否正确，是否和模拟3对点时用的7个参数相同my.printcaliresult();//5.测试一对点Vector3d input(1, 0, 0);cout << "输入的点是" << endl << input << endl;Vector3d output = my.calc(input);cout << "输出的点是" << endl <<output<< endl;//由固定在刚体上的3个点在全局坐标系下的位置来确定刚体的姿态//设点在刚体坐标系下的坐标为X(O)=(x,y,z,1),在全局坐标系下的位置是X(G)=(x1,y1,z1,1);//则转换矩阵M使M*X(O)=X(G);//本质上还是3对坐标点解算两个坐标系相对位置的关系//把这个类改造一下，使用标定的结果作为结果。//怎么解算飞行器的位姿CoordinateTransformation CT;//1.设置刚体上固定的3个点的坐标Vector3d RigidBodyPosition1(1, 0, 0);   //固定在刚体上的点在刚体坐标系中的位置CT.SetRigidBodyPoint1(RigidBodyPosition1);Vector3d RigidBodyPosition2(0, 1, 0);   //固定在刚体上的点在刚体坐标系中的位置CT.SetRigidBodyPoint2(RigidBodyPosition2);Vector3d RigidBodyPosition3(0, 0, 1);   //固定在刚体上的点在刚体坐标系中的位置CT.SetRigidBodyPoint3(RigidBodyPosition3);//2.模拟计算出的坐标系中的点Vector3d RigidBodyPosition11 = ratio*(Translation + Rotation*RigidBodyPosition1);     //变换之后Vector3d RigidBodyPosition22 = ratio*(Translation + Rotation*RigidBodyPosition2);Vector3d RigidBodyPosition33 = ratio*(Translation + Rotation*RigidBodyPosition3);//3.使用模拟坐标系中的点可以解算出飞行器的位姿Matrix4d temp = CT.calcpose(RigidBodyPosition11, RigidBodyPosition22, RigidBodyPosition33);    //设置3个点按顺序的坐标cout << temp << endl;while (1);return 0;
}

依赖的库

在里面用到了开源的矩阵计算工具库eigen。可以在http://eigen.tuxfamily.org/index.php?title=Main_Page下载。这个工具库全部由头文件组成，所以只要复制到工程目录下就可以，不需要复杂的配置库目录和可执行文件目录。

任意两空间直角坐标系的转换的数学模型和算法实现相关推荐

c语言大地坐标系和空间直角坐标系的转换,空间直角坐标系与大地坐标系转换程序.doc...
空间直角坐标系与大地坐标系转换程序.doc 空间直角坐标系与大地坐标系转换程序includeiostream includecmathincludeiomanip using namespace st ...
解析几何：第一章坐标系与坐标变换：平面直角坐标系、空间直角坐标系及其变换、极坐标系，圆柱面坐标系，球面坐标系
§1 平面直角坐标系及其变换 1.直角坐标系(笛卡儿坐标系) 坐标系:O-XY 点M(x,y),x为横坐标,y为纵坐标. 在Ⅰ.Ⅱ.Ⅲ.Ⅳ四个象限中坐标x,y的符号为: 象限 Ⅰ Ⅱ Ⅲ Ⅳ x + ...
空间直角坐标系与大地坐标系坐标的转换(mfc)
1.程序主界面 2.控件变量说明 public:CString X;//空间直角坐标系X坐标CString Y;//空间直角坐标系Y坐标CString Z;//空间直角坐标系Z坐标CString dB ...
c语言直角坐标系与大地坐标系转换,空间直角坐标系与大地坐标系转换程序
<空间直角坐标系与大地坐标系转换程序>由会员分享,可在线阅读,更多相关<空间直角坐标系与大地坐标系转换程序(5页珍藏版)>请在人人文库网上搜索. 1.空间直角坐标系与大地坐标系 ...
空间直角坐标系、大地坐标系、平面直角坐标、投影坐标系的区别
前言坐标系转换流程如下:因此我们先要搞清楚不同坐标系的概念区别. ( 一)空间直角坐标系空间直角坐标系的坐标原点位于参考椭球的中心,Z轴指向参考椭球的北极,X轴指向起始子午面与赤道的交点, ...
html中如何华直角坐标系,空间直角坐标系的画法．.ppt
空间直角坐标系的画法．空间直角坐标系的画法．图片电影图片问题2:如何建立空间直角坐标系? 问题3:如何将空间直角坐标系呈现在纸上? 问题4:什么叫右手直角坐标系? 问题5:如何表示空间直角坐标 ...
空间直角坐标系、左手坐标系、右手坐标系
这里写目录标题 1.空间直角坐标系 2.右手坐标系 3.左手坐标系 4.左手坐标系和右手坐标系比较 1.空间直角坐标系过空间定点O作三条互相垂直的数轴,它们都以O为原点,具有相同的单位长度.这三条数 ...
空间直角坐标系、大地坐标系、平面坐标系、高斯平面直角坐标系
1.空间直角坐标系空间直角坐标系的坐标原点位于参考椭球的中心,Z轴指向参考椭球的北极,X轴指向起始子午面与赤道的交点,Y轴位于赤道面上切按右手系于X轴呈90度夹角,某点中的坐标可用该点在此坐标系的各 ...
空间直角坐标系、大地坐标系、平面坐标系、高斯平面直角坐标系（转）
本篇学习了空间直角坐标系.大地坐标系.平面坐标系.高斯平面直角坐标系.这个个坐标系有时很容易弄混淆! ( 一)空间直角坐标系空间直角坐标系的坐标原点位于参考椭球的中心,Z轴指向参考椭球的北极 ...

任意两空间直角坐标系的转换的数学模型和算法实现