单像空间后方交会.docx
- 文档编号:30771322
- 上传时间:2023-08-23
- 格式:DOCX
- 页数:32
- 大小:21.52KB
单像空间后方交会.docx
《单像空间后方交会.docx》由会员分享,可在线阅读,更多相关《单像空间后方交会.docx(32页珍藏版)》请在冰豆网上搜索。
单像空间后方交会
交会算例
控制点:
x(mm)y(mm)X(m)Y(m)Z(m)
-11.00-13.80500017.194185066.00885.40
10.48-6.44500452.194185197.50915.82
10.788.312500470.164185494.50928.76
2.7128.32500302.164185489.00949.71
焦距:
28.1359(mm)
外方位元素初值:
X(m)Y(m)Z(m)phi(rad)omega(rad)kappa(rad)
500215.494185301.891475.560.0548820.057034-0.036175
代码
usingSystem;
usingSystem.Collections.Generic;
usingSystem.Linq;
usingSystem.Text;
usingSystem.Threading.Tasks;
usingSystem.IO;
namespace后方交会
{
classProgram
{
staticvoidMain(string[]args)
{
/////////////////初始计算///////////////////
//已知数据
double[]x={-0.011,-0.01048,-0.01078,0.002712};
double[]y={-0.0138,-0.00644,0.008312,0.00832};
double[]X={500017.19,500452.19,500470.16,500302.16};
double[]Y={4185066.00,4185197.50,4185494.50,4185489.00};
double[]Z={885.40,915.82,928.76,949.71};
doublef=0.0281359,d=0.000004;
constintc_N=100;//迭代次数
intn=1;//迭代次数统计变量
doubleH,m=0;//H为航高,m为比例尺
////求m的值
intt=0;
doublesum;
double[]mid=newdouble[6];//存储比例尺m的值
for(inti=0;i<4;i++)//求六对线段的长度比
{
for(intj=i+1;j<4;j++)
{
mid[t]=(Math.Sqrt((X[i]-X[j])*(X[i]-X[j])+(Y[i]-Y[j])*(Y[i]-Y[j])))/
(Math.Sqrt((x[i]-x[j])*(x[i]-x[j])+(y[i]-y[j])*(y[i]-y[j])));
t++;
}
}
sum=0.0;
for(inti=0;i { sum=sum+mid[i]; } m=sum/t;//比例尺为各段长度比的均值 H=f*m;//计算航高 StreamWritersw=newStreamWriter("E: \\迭代结果.txt"); sw.WriteLine("计算结果: \r\n"); sw.WriteLine("比例尺m="+m.ToString()); sw.WriteLine("航高H="+H.ToString()); sw.WriteLine("\r\n"); sw.Close(); ////定义外方位元素,并附初值 doubleXs,Ys,Zs,φ=0.054882,ω=0.057034,κ=-0.036175; Xs=500215.49; Ys=4185301.89; Zs=1475.56; ////定义x,y近似值 double[]_x=newdouble[4]; double[]_y=newdouble[4]; ////定义共线方程中的分子分母项,便于计算 double[]_X=newdouble[4]; double[]_Y=newdouble[4]; double[]_Z=newdouble[4]; ////定义旋转矩阵R double[,]R=newdouble[3,3]; ////定义解方程所用矩阵 double[,]A=newdouble[8,6];//A系数阵 double[,]AT=newdouble[6,8];//A系数阵转置 double[,]ATA=newdouble[6,6];//A的转置与A的乘积 double[,]temp=newdouble[6,12];//临时矩阵 double[,]ATAR=newdouble[6,6];//A的转置与A的乘积的逆 double[,]ATARAT=newdouble[6,8];//A的转置与A的乘积的逆的转置 double[,]L=newdouble[8,1];//误差方程中的常数项 double[,]XX=newdouble[6,1];//X向量 //////////////////开始迭代/////////////////// do { ////计算旋转矩阵 R[0,0]=Math.Cos(φ)*Math.Cos(κ)-Math.Sin(φ)*Math.Sin(ω)*Math.Sin(κ);//a1 R[0,1]=-Math.Cos(φ)*Math.Sin(κ)-Math.Sin(φ)*Math.Sin(ω)*Math.Cos(κ);//a2 R[0,2]=-Math.Sin(φ)*Math.Cos(ω);//a3 R[1,0]=Math.Cos(ω)*Math.Sin(κ);//b1 R[1,1]=Math.Cos(ω)*Math.Cos(κ);//b2 R[1,2]=-Math.Sin(ω);//b3 R[2,0]=Math.Sin(φ)*Math.Cos(κ)+Math.Cos(φ)*Math.Sin(ω)*Math.Sin(κ);//c1 R[2,1]=-Math.Sin(φ)*Math.Sin(κ)+Math.Cos(φ)*Math.Sin(ω)*Math.Cos(κ);//c2 R[2,2]=Math.Cos(φ)*Math.Cos(ω);//c3 for(inti=0;i<4;i++) { //用共线方程计算x,y的近似值 _X[i]=R[0,0]*(X[i]-Xs)+R[1,0]*(Y[i]-Ys)+R[2,0]*(Z[i]-Zs); _Y[i]=R[0,1]*(X[i]-Xs)+R[1,1]*(Y[i]-Ys)+R[2,1]*(Z[i]-Zs); _Z[i]=R[0,2]*(X[i]-Xs)+R[1,2]*(Y[i]-Ys)+R[2,2]*(Z[i]-Zs); _x[i]=-f*_X[i]/_Z[i]; _y[i]=-f*_Y[i]/_Z[i]; } for(inti=0;i<4;i++) { //计算系数矩阵 A[2*i,0]=(R[0,0]*f+R[0,2]*x[i])/_Z[i]; A[2*i,1]=(R[1,0]*f+R[1,2]*x[i])/_Z[i]; A[2*i,2]=(R[2,0]*f+R[2,2]*x[i])/_Z[i]; A[2*i,3]=y[i]*Math.Sin(ω)-((x[i]/f)*(x[i]*Math.Cos(κ)-y[i]*Math.Sin(κ))+f*Math.Cos(κ))*Math.Cos(ω); A[2*i,4]=-f*Math.Sin(κ)-(x[i]/f)*(x[i]*Math.Sin(κ)+y[i]*Math.Cos(κ)); A[2*i,5]=y[i]; A[2*i+1,0]=(R[0,1]*f+R[0,2]*y[i])/_Z[i]; A[2*i+1,1]=(R[1,1]*f+R[1,2]*y[i])/_Z[i]; A[2*i+1,2]=(R[2,1]*f+R[2,2]*y[i])/_Z[i]; A[2*i+1,3]=-x[i]*Math.Sin(ω)-((x[i]/f)*(x[i]*Math.Cos(κ)-y[i]*Math.Sin(κ))-f*Math.Sin(κ))*Math.Cos(ω); A[2*i+1,4]=-f*Math.Cos(κ)-(y[i]/f)*(x[i]*Math.Sin(κ)+y[i]*Math.Cos(κ)); A[2*i+1,5]=-x[i]; //计算常数项 L[2*i,0]=x[i]-_x[i]; L[2*i+1,0]=y[i]-_y[i]; } ////计算A的转置,存在AT中 for(inti=0;i<6;i++) for(intj=0;j<8;j++) { AT[i,j]=A[j,i]; } ////计算A的转置与A的乘积,存在ATA中 for(inti=0;i<6;i++) for(intj=0;j<6;j++) { ATA[i,j]=0; for(intl=0;l<8;l++) ATA[i,j]+=AT[i,l]*A[l,j]; } ////////计算ATA的逆矩阵//////// ////初始化temp--临时数组 for(intl=0;l<6;l++) for(intp=0;p<12;p++) { temp[l,p]=0; } ////把ATA各值赋给temp for(inti=0;i<6;i++) for(intj=0;j<6;j++) { temp[i,j]=ATA[i,j]; } ////在temp中加入初等方阵 for(intl=0;l<6;l++) temp[l,l+6]=1; ////初等变换 for(intl=0;l<6;l++) { if(temp[l,l]! =1) { doublebs=temp[l,l]; temp[l,l]=1; for(intp=l+1;p<12;p++) temp[l,p]/=bs; } for(intq=0;q<6;q++) { if(q! =l) { doublebs=temp[q,l]; for(intp=l;p<12;p++) temp[q,p]-=bs*temp[l,p]; } else continue; } } ////得到ATA的逆阵后存在ATAR中 for(inti=0;i<6;i++) for(intj=0;j<6;j++) { ATAR[i,j]=temp[i,j+6]; } ////ATAR*AT存在ATARAT中 for(inti=0;i<6;i++) for(intj=0;j<8;j++) { ATARAT[i,j]=0; for(intl=0;l<6;l++) ATARAT[i,j]+=ATAR[i,l]*AT[l,j]; } ////计算ATARAT*L,存在XX中 for(inti=0;i<6;i++) for(intj=0;j<1;j++) { XX[i,j]=0; for(intl=0;l<8;l++) XX[i,j]+=ATARAT[i,l]*L[l,0]; } ////计算外方位元素值 Xs+=XX[0,0]; Ys+=XX[1,0]; Zs+=XX[2,0]; φ+=XX[3,0]; ω+=XX[4,0]; κ+=XX[5,0]; if(n>c_N) Console.Write("超过迭代精度,此数值下可能不收敛! "); else { StreamWritermystreamwriter=File.AppendText("E: \\迭代结果.txt"); mystreamwriter.WriteLine("迭代次数: "+n.ToString()); mystreamwriter.WriteLine("\r\n"); mystreamwriter.WriteLine("Xs: "+Xs.ToString()+"m"); mystreamwriter.WriteLine("Ys: "+Ys.ToString()+"m"); mystreamwriter.WriteLine("Zs: "+Zs.ToString()+"m"); mystreamwriter.WriteLine("\r\n"); mystreamwriter.WriteLine("φ: "+φ.ToString()+"rad"); mystreamwriter.WriteLine("ω: "+ω.ToString()+"rad"); mystreamwriter.WriteLine("κ: "+κ.ToString()+"rad"); mystreamwriter.WriteLine("\r\n"); mystreamwriter.WriteLine("dXs: "+XX[0,0].ToString()+"m"); mystreamwriter.WriteLine("dYs: "+XX[1,0].ToString()+"m"); mystreamwriter.WriteLine("dZs: "+XX[2,0].ToString()+"m"); mystreamwriter.WriteLine("dφ: "+XX[3,0].ToString()+"rad"); mystreamwriter.WriteLine("dω: "+XX[4,0].ToString()+"rad"); mystreamwriter.WriteLine("dκ: "+XX[5,0].ToString()+"rad"); mystreamwriter.WriteLine("\r\n"); mystreamwriter.Close(); } n++; } while(Math.Abs(XX[0,0])>=d||Math.Abs(XX[1,0])>=d||Math.Abs(XX[2,0])>=d||Math.Abs(XX[3,0])>=1000*d||Math.Abs(XX[4,0])>=1000*d||Math.Abs(XX[5,0])>=1000*d); Console.Write("迭代结果保存在E: \\迭代结果.txt\n"); Console.ReadLine(); } } } 迭代结果 计算结果: 比例尺m=26428.5803527275 航高H=743.591893946307 迭代次数: 1 Xs: 499427.235480172m Ys: 4186554.35263872m Zs: 1368.80576213753m φ: 1.69692876951611rad ω: -2.09012267498615rad κ: 0.00548008783721364rad dXs: -788.254519827722m dYs: 1252.46263871879m dZs: -106.754237862473m dφ: 1.64204676951611rad dω: -2.14715667498615rad dκ: 0.0416550878372136rad 迭代次数: 2 Xs: 500467.532419656m Ys: 4185456.61453054m Zs: 374.95743717003m φ: -0.140958561770875rad ω: -2.23441990062963rad κ: -2.17405022753397rad dXs: 1040.29693948375m dYs: -1097.73810818188m dZs: -993.848324967497m dφ: -1.83788733128699rad dω: -0.144297225643471rad dκ: -2.17953031537119rad 迭代次数: 3 Xs: 500476.308776212m Ys: 4185607.38026842m Zs: 632.838706698654m φ: 0.954363844863929rad ω: -2.48825187782244rad κ: -1.75495905958627rad dXs: 8.77635655598289m dYs: 150.76573788617m dZs: 257.881269528624m dφ: 1.0953224066348rad dω: -0.253831977192816rad dκ: 0.419091167947699rad 迭代次数: 4 Xs: 500616.879904002m Ys: 4185401.808175m Zs: 848.284046525818m φ: 1.44377486912943rad ω: -2.98285429491134rad κ: -2.14357325579804rad dXs: 140.57112778955m dYs: -205.57209341955m dZs: 215.445339827165m dφ: 0.489411024265505rad dω: -0.494602417088902rad dκ: -0.388614196211766rad 迭代次数: 5 Xs: 500683.195952908m Ys: 4185477.26679047m Zs: 808.789082413199m φ: 1.44607124332214rad ω: -2.97732893547932rad κ: -2.69025838709439rad dXs: 66.3160489061025m dYs: 75.4586154613798m dZs: -39.4949641126197m dφ: 0.00229637419270447rad dω: 0.00552535943201912rad dκ: -0.546685131296344rad 迭代次数: 6 Xs: 500753.644193639m Ys: 4185433.01579394m Zs: 796.113642513907m φ: 1.47987733737837rad ω: -3.09574695976417rad κ: -2.62658347663032rad dXs: 70.4482407310002m dYs: -44.2509965235637m dZs: -12.6754398992914m dφ: 0.0338060940562311rad dω: -0.118418024284845rad dκ: 0.063674910464063rad 迭代次数: 7 Xs: 500756.916067628m Ys: 4185366.703189m Zs: 845.245280831676m φ: 1.6006760340191rad ω: -3.19900930959821rad κ: -2.68720934299249rad dXs: 3.27187398883541m dYs: -66.3126049409103m dZs: 49.1316383177692m dφ: 0.120798696640731rad dω: -0.103262349834041rad dκ: -0.0606258663621704rad 迭代次数: 8 Xs: 500772.94289325m Ys: 4185337.00636882m Zs: 851.880315331298m φ: 1.6373591857374rad ω: -3.26450644578666rad κ: -2.74284054263391rad dXs: 16.0268256219796m dYs: -29.6968201785239m dZs: 6.63503449962182m dφ: 0.0366831517183015rad dω: -0.0654971361884477rad dκ: -0.0556311996414136rad 迭代次数: 9 Xs: 500773.65457002m Ys: 4185312.81004467m Zs: 863.123603995748m φ: 1.66796954053432rad ω: -3.30702965770232rad κ: -2.7703342811574rad dXs: 0.711676770235456m d
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 空间 后方 交会
![提示](https://static.bdocx.com/images/bang_tan.gif)