免费视频|新人指南|投诉删帖|广告合作|地信网APP下载

查看: 4652|回复: 13
收起左侧

[资料] WGS84和BJ54坐标转换源程序

  [复制链接]

1万

主题

1022

铜板

1098

好友

技术员

积分
81946
QQ
发表于 2011-4-14 15:40 | 显示全部楼层 |阅读模式
两个坐标系转换一般需要平移,旋转,缩放共七参数。
Y=(1+k)*M(x,y,z)*X+dX;
X,Y为3*1矩阵,M(x,y,z)为3*3的旋转矩阵.

public class CoordTrans7Param
{
public double[,] values=new double[7,1];
//{{dx},{dy},{dz},{rx},{ry},{rz},{k}};
//public double dx,dy,dz,rx,ry,rz,k;
public void Set4Param(double dx,double dy,double dz,double k)
{
this.dx=dx;
this.dy=dy;
this.dz=dz;
this.k=k;
this.rx=this.ry=this.rz=0;
}
public void SetRotationParamRad(double rx,double ry,double rz)
{
this.rx=rx;
this.ry=ry;
this.rz=rz;
}
public void SetRotationParamMM(double rx,double ry,double rz)
{
SetRotationParamRad(rx*Math.PI/648000,ry*Math.PI/648000,rz*Math.PI/648000);
}
private double[,] GetMx()
{
double [,] Mx=new double[,]
{{1,0,0},
{0,Math.Cos(rx),Math.Sin(rx)},
{0,-Math.Sin(rx),Math.Cos(rx)}};
return Mx;
}
private double[,] GetMy()
{
double [,] My=new double[,]
{{Math.Cos(ry),0,-Math.Sin(ry)},
{0,1,0},
{Math.Sin(ry),0,Math.Cos(ry)}};
return My;
}
private double[,] GetMz()
{
double [,] Mz=new double[,]
{{Math.Cos(rz),Math.Sin(rz),0},
{-Math.Sin(rz),Math.Cos(rz),0},
{0,0,1}};
return Mz;
}

private double[,] GetM() //M=Mx*My*Mz? or M=Mz*My*Mx?
{
double [,] M=new double[3,3];

MatrixTool.Multi(GetMz(),GetMy(),ref M);
MatrixTool.Multi(M,GetMx(),ref M);
return M;
}
private double[,] GetMdx()
{
double[,] mt = {{ 0, 0, 0 },
{ 0, -Math.Sin(rx), Math.Cos(rx) },
{ 0, -Math.Cos(rx), -Math.Sin(rx) }};

double[,] m=new double[3,3];

MatrixTool.Multi(GetMz(),GetMy(),ref m);
MatrixTool.Multi(m,mt,ref m);
return m;
}
private double[,] GetMdy()
{
double[,] mt = {{ -Math.Sin(ry), 0, -Math.Cos(ry) },
{ 0, 0, 0 },
{ Math.Cos(ry), 0, -Math.Sin(ry) }};

double[,] m=new double[3,3];

MatrixTool.Multi(GetMz(),mt,ref m);
MatrixTool.Multi(m,GetMx(),ref m);
return m;
}

private double[,] GetMdz()
{
double[,] mt = {{ -Math.Sin(rz), Math.Cos(rz), 0 },
{ -Math.Cos(rz), -Math.Sin(rz), 0 },
{ 0, 0, 0 }};

double[,] m=new double[3,3];

MatrixTool.Multi(mt,GetMy(),ref m);
MatrixTool.Multi(m,GetMx(),ref m);
return m;
}
private double[,] specialMulti(double[,] m,double[,] X)
{
int rowNumM=m.GetLength(0);
int colNumM=m.GetLength(1);
int rowNumX=X.GetLength(0);
int colNumX=X.GetLength(1);
int lines=rowNumX/colNumM;
double[,] mt=MatrixTool.Init(rowNumM,colNumX);
double[,] subX=MatrixTool.Init(colNumM,colNumX);
double[,] res=MatrixTool.Init(rowNumM*lines,colNumX);

for(int i=0;i<lines;i++)
{
MatrixTool.CopySub(X,i*colNumM,0,colNumM,colNumX,ref subX,0,0);
MatrixTool.Multi(m,subX,ref mt);
MatrixTool.CopySub(mt,0,0,rowNumM,colNumX,ref res,i*rowNumM,0);
}
return res;
}
private double[,] specialSub(double[,] m,double[,] X)
{
int rowNumM=m.GetLength(0);
int colNumM=m.GetLength(1);
int rowNumX=X.GetLength(0);
int colNumX=X.GetLength(1);
int lines=rowNumX/rowNumM;
double[,] subX=MatrixTool.Init(rowNumM,colNumX);
double[,] res=MatrixTool.Init(rowNumX,colNumX);

for(int i=0;i<rowNumX;i+=rowNumM)
{
MatrixTool.CopySub(X,i,0,rowNumM,colNumX,ref subX,0,0);
MatrixTool.Sub(m,subX,ref subX);
MatrixTool.CopySub(subX,0,0,rowNumM,colNumX,ref res,i,0);
}
return res;
}

private double[,] GetF(double[,] X,double[,] Y)
{
double[,] f0;
double[,] qx=MatrixTool.Init(X.GetLength(0),1);
double[,] K={{-dx},{-dy},{-dz}};
double[,] S={{1+k}};

MatrixTool.Multi(X,S,ref qx);
double [,] M=GetM();
qx=specialMulti(M,qx);
MatrixTool.Sub(qx,Y,ref qx);
f0=specialSub(K,qx);
return f0;
}
private double[,] GetB(double[,] X)
{
int rowNum=X.GetLength(0);
double[,] B=MatrixTool.Init(rowNum,7);
double[,] M=GetM();
double[,] Mdx=GetMdx();
double[,] Mdy=GetMdy();
double[,] Mdz=GetMdz();
double[,] mi=MatrixTool.Ident(3);
double[,] MX,MY,MZ,MK;

MK=specialMulti(M,X);
MX=specialMulti(Mdx,X);
MY=specialMulti(Mdy,X);
MZ=specialMulti(Mdz,X);

for(int i=0;i<rowNum;i+=3)
MatrixTool.CopySub(mi,0,0,3,3,ref B,i,0);
MatrixTool.CopySub(MX,0,0,rowNum,1,ref B,0,3);
MatrixTool.CopySub(MY,0,0,rowNum,1,ref B,0,4);
MatrixTool.CopySub(MZ,0,0,rowNum,1,ref B,0,5);
MatrixTool.CopySub(MK,0,0,rowNum,1,ref B,0,6);
return B;
}
private double[,] GetA()
{
double[,] M=GetM();
double[,] I2=MatrixTool.Ident(3);
double[,] A=MatrixTool.Init(3,6);

MatrixTool.MutliConst(ref I2,-1);
MatrixTool.MutliConst(ref M,(1+k));
MatrixTool.CopySub(M,0,0,3,3,ref A,0,0);
MatrixTool.CopySub(I2,0,0,3,3,ref A,0,3);
return A;
}
private double[,] GetV(double[,] X,double[,] Y,CoordTrans7Param dpp)
{
int rowNum=X.GetLength(0);

double[,] B,F,A,B2,B3,F2,V;
double[,] AT=MatrixTool.Init(6,3);

A=GetA();
MatrixTool.AT(A,ref AT);
MatrixTool.MutliConst(ref AT,1/(1+(1+k)*(1+k)));

F=GetF(X,Y);
B=GetB(X);
B2=MatrixTool.Init(3,7);
B3=MatrixTool.Init(3,1);
F2=MatrixTool.Init(rowNum,1);
for(int i=0;i<rowNum/3;i++)
{
MatrixTool.CopySub(B,i*3,0,3,7,ref B2,0,0);
MatrixTool.Multi(B2,dpp.values,ref B3);
MatrixTool.CopySub(B3,0,0,3,1,ref F2,i*3,0);
}
MatrixTool.Sub(F,F2,ref F2);
V=specialMulti(AT,F2);
return V;
}
public double CalculateTrans7Param(double[,] X,double[,] Y)
{
int PtNum=X.GetLength(0)/3;
double[,] B;
double[,] F;
double[,] BT=MatrixTool.Init(7,3*PtNum);
double[,] BTB=MatrixTool.Init(7,7);
double[,] BTF=MatrixTool.Init(7,1);


//init pararm
CoordTrans7Param dpp=new CoordTrans7Param();
Set4Param(0,0,0,0);
this.SetRotationParamMM(0,0,0);
//debug
//this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2);
int round=0;
while(round++<20)
{
F=GetF(X,Y);
B=GetB(X);
MatrixTool.AT(B,ref BT);

MatrixTool.Multi(BT,B,ref BTB);
MatrixTool.Inv(BTB);
MatrixTool.Multi(BT,F,ref BTF);
MatrixTool.Multi(BTB,BTF,ref dpp.values);
if (dpp.isSmall())
break;
else
MatrixTool.Add(this.values,dpp.values,ref this.values);
}
//this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2);
double[,] V=GetV(X,Y,dpp);

double vMax=-1;
for(int i=0;i<V.GetLength(0);i++)
{
if (Math.Abs(V[i,0])>vMax)
vMax=Math.Abs(V[i,0]);
}

return vMax;
}
private bool isSmall()
{
double s=0;
for(int i=0;i<7;i++)
s+=Math.Abs(values[i,0]);
if (s<0.0000001)
return true;
else
return false;
}
public void TransCoord(double x1,double y1,double z1,out double x2,out double y2,out double z2)
{

double[,] Xi={{x1},{y1},{z1}};
double[,] DX={{dx},{dy},{dz}};
double[,] tY=new double[3,1];
double[,] K={{1+k}};

double [,] M=GetM();
MatrixTool.Multi(Xi,K,ref tY);
MatrixTool.Multi(M,tY,ref tY);
MatrixTool.Add(tY,DX,ref tY);
x2=tY[0,0];
y2=tY[1,0];
z2=tY[2,0];
}

public double dx
{
get
{
return values[0,0];
}
set
{
values[0,0]=value;
}
}
public double dy
{
get
{
return values[1,0];
}
set
{
values[1,0]=value;
}
}
public double dz
{
get
{
return values[2,0];
}
set
{
values[2,0]=value;
}
}
public double rx
{
get
{
return values[3,0];
}
set
{
values[3,0]=value;
}
}
public double ry
{
get
{
return values[4,0];
}
set
{
values[4,0]=value;
}
}
public double rz
{
get
{
return values[5,0];
}
set
{
values[5,0]=value;
}
}
public double k
{
get
{
return values[6,0];
}
set
{
values[6,0]=value;
}
}
}

241

主题

29万

铜板

579

好友

地信专家组

一起把论坛搞大

Rank: 14Rank: 14Rank: 14Rank: 14

积分
49670

宣传勋章爱心勋章组织勋章地信元老名人堂勋章

发表于 2011-4-14 16:29 | 显示全部楼层
呵呵,这个我看不懂的!
经验是积累的,技巧是摸索的,软件是一样的,关键看咋用的。 鼠马象鸡之腾讯课堂:https://tsyhome.ke.qq.com/

2

主题

3万

铜板

14

好友

资深会员

Rank: 18Rank: 18Rank: 18Rank: 18Rank: 18

积分
3238
发表于 2011-4-15 09:10 | 显示全部楼层
C代码,纠结。。。
该会员没有填写今日想说内容.

21

主题

8万

铜板

83

好友

地信学员

开开心心每一天

Rank: 12Rank: 12Rank: 12

积分
11934
发表于 2011-4-15 09:27 | 显示全部楼层
哈哈,还是原代码呀,不错,最好你先编译出来给不会的人用啊,呵呵

0

主题

1282

铜板

2

好友

技术员

Rank: 3Rank: 3

积分
94
发表于 2011-4-15 10:54 | 显示全部楼层
回复 专注地信 的帖子

呵呵,这个我看不懂的
呵呵,这个我看不懂的
呵呵,这个我看不懂的

7

主题

4108

铜板

11

好友

高级工程师

Rank: 9Rank: 9Rank: 9

积分
655
发表于 2011-4-15 12:51 | 显示全部楼层
这个真看不懂……

0

主题

63

铜板

0

好友

实习生

Rank: 1

积分
15
发表于 2011-4-18 16:50 | 显示全部楼层
感谢啊  感谢~!~!!!

12

主题

2663

铜板

40

好友

高级工程师

Rank: 9Rank: 9Rank: 9

积分
646
QQ
发表于 2011-4-24 11:59 | 显示全部楼层
看不懂呀       特此通知
该会员没有填写今日想说内容.

2786

主题

4万

铜板

269

好友

版主

Rank: 15Rank: 15Rank: 15Rank: 15Rank: 15

积分
33468

宣传勋章灌水勋章贡献勋章斑竹勋章活跃勋章

发表于 2011-4-24 16:53 | 显示全部楼层
看看学习了解
革命尚未成功,我们还须签到! ...

53

主题

1万

铜板

37

好友

地信院士

Rank: 15Rank: 15Rank: 15Rank: 15Rank: 15

积分
2074
发表于 2011-4-24 20:33 | 显示全部楼层
牛人处处有
轻轻的我来签到了,想带走一堆铜板...
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

在线客服
快速回复 返回顶部 返回列表