单像空间后方交会实习报告级.docx
《单像空间后方交会实习报告级.docx》由会员分享,可在线阅读,更多相关《单像空间后方交会实习报告级.docx(18页珍藏版)》请在冰豆网上搜索。
单像空间后方交会实习报告级
摄影测量单像空间后方交会
实习报告
学院:
遥感信息工程学院
班级:
140X
学号:
xxxxxxxxxxx
姓名:
2016年10月24日
一、实习目的
1、掌握空间后方交会的定义和实现算法;
2、了解摄影测量平差的基本过程;
3、编程实现并进行精度评定
二、实习内容
1、定义:
空间后方交会是以单幅影像为基础,从该影像所覆盖地面范围内若干控制点的已知地面坐标和相应点的像坐标量测值出发,根据共线条件方程,解求该影像在航空摄影时刻的外方位元素Xs,Ys,Zs,φ,ω,κ。
2、算法:
由于每一对像方和物方共轭点可列出2个方程,因此若有3个已知地面坐标的控制点,则可列出6个方程,解求6个外方位元素的改正数△Xs,△Ys,△Zs,△φ,△ω,△κ。
实际应用中为了提高解算精度,常有多余观测方程,通常是在影像的四个角上选取4个或均匀地选择更多的地面控制点,因而要用最小二乘平差方法进行计算。
3、基本过程
(1)获取已知数据。
从摄影资料中查取影像比例尺1/m,平均摄影距离(航空摄影的航高)、内方位元素x0,y0,f;获取控制点的空间坐标X,Y,Z。
(2)量测控制点的像点坐标并进行必要的影像坐标系统误差改正,得到像点坐标。
(3)确定未知数的初始值。
单像空间后方交会必须给出待定参数的初始值,在竖直航空摄影且地面控制点大体对称分布的情况下,Xs0和Ys0为均值,Zs0为航高,φ、ω、κ的初值都设为0。
或者κ的初值可在航迹图上找出或根据控制点坐标通过坐标正反变换求出。
(4)计算旋转矩阵R。
利用角元素近似值计算方向余弦值,组成R阵。
(5)逐点计算像点坐标的近似值。
利用未知数的近似值按共线条件式计算控制点像点坐标的近似值(x),(y)。
(6)逐点计算误差方程式的系数和常数项,组成误差方程式。
(7)计算法方程的系数矩阵ATA与常数项ATL,组成法方程式。
(8)解求外方位元素。
根据法方程,解求外方位元素改正数,并与相应的近似值求和,得到外方位元素新的近似值。
(9)检查计算是否收敛。
将所求得的外方位元素的改正数与规定的限差比较,通常对φ,ω,κ的改正数△φ,△ω,△κ给予限差,通常为0.000001弧度,当3个改正数均小于0.000001弧度时,迭代结束。
否则用新的近似值重复(4)~(8)步骤的计算,直到满足要求为止。
4、精度评定:
通过法方程式中未知数的系数矩阵的逆阵(ATA)-1来解求,此时视像点坐标为等精度不相关观测值。
因为(ATA)-1中第i个主对角线上的元素Qii就是法方程式中第i个未知数的权倒数,若单位权中误差为m0,则第i个未知数的中误差为:
mi=
m0 当参加空间后方交会的控制点有n个时,则单位权中误差可按下式计算:
M0=
三、实习思路
四、代码实现
#defineN4//控制点个数
#definePI3.1415926
#defineMAXITEARATION5//最大允许迭代次数
structEOEO//elementsofexteriororientation外方位元素
{
doubleXs;
doubleYs;
doubleZs;
doubleomega;
doublephi;
doublekappa;
};
structSourceData//原始数据
doublex;
doubley;
doubleX;
doubleY;
doubleZ;
//矩阵转置
//参数说明:
原始矩阵MatrixOrigin为m×n
voidMatrixTranspose(double*MatrixOrigin,double*MatrixNew,intm,intn)
inti;
intj;
for(i=0;i!
=n;i++)
for(j=0;j!
=m;j++)
MatrixNew[i*m+j]=MatrixOrigin[j*n+i];
}
//矩阵求逆
m:
原始矩阵Matrix的行/列数
voidMatrixInversion(double*Matrix,intm)
inti,j,k;
for(k=0;k!
=m;k++)
=m;i++)
if(i!
=k)
Matrix[i*m+k]=-Matrix[i*m+k]/Matrix[k*m+k];
Matrix[k*m+k]=1/Matrix[k*m+k];
if(j!
Matrix[i*m+j]+=Matrix[k*m+j]*Matrix[i*m+k];
Matrix[k*m+j]*=Matrix[k*m+k];
//矩阵相乘
C=A×B,ARow:
A的行数,AColumn:
A的列数,BColumn:
B的列数
voidMatrixMultiply(double*MatrixA,double*MatrixB,double*MatrixC,intARow,intAColumn,intBColumn)
intk;
=ARow;i++)
for(j=0;j{MatrixC[i*BColumn+j]=0.0;for(k=0;kMatrixC[i*BColumn+j]+=MatrixA[i*AColumn+k]*MatrixB[j+k*BColumn];}}//矩阵相加重载1//参数说明voidMatrixAdd(double*MatrixA,double*MatrixB,double*MatrixC,intRow,intColumn){inti;intj;for(i=0;i!=Row;i++){for(j=0;j!=Column;j++){MatrixC[i*Column+j]=MatrixA[i*Column+j]+MatrixB[i*Column+j];}}}//矩阵相加重载2//A+B=C//voidMatrixAdd(double*MatrixA,double*MatrixB,double*MatrixC,intlength){inti;for(i=0;i!=length;i++){MatrixC[i]=MatrixA[i]+MatrixB[i];}}//矩阵相减,可做类似相加函数的重载//A-B=CvoidMatrixMinus(double*MatrixA,double*MatrixB,double*MatrixC,intlength){inti;for(i=0;i!=length;i++){MatrixC[i]=MatrixA[i]-MatrixB[i];}} //矩阵复制//B=AvoidMatrixCopy(double*MatrixA,double*MatrixB,intlength){inti;for(i=0;i!=length;i++){MatrixB[i]=MatrixA[i];}} //函数功能:初始化坐标数据voidInitData(SourceData*sd){sd[0].x=-86.15;sd[0].y=-68.99;sd[0].X=36589.41;sd[0].Y=25273.32;sd[0].Z=2195.17;sd[1].x=-53.40;sd[1].y=82.21;sd[1].X=37631.08;sd[1].Y=31324.51;sd[1].Z=728.69;sd[2].x=-14.78;sd[2].y=-76.63;sd[2].X=39100.97;sd[2].Y=24934.98;sd[2].Z=2386.50;sd[3].x=10.46;sd[3].y=64.43;sd[3].X=40426.54;sd[3].Y=30319.81;sd[3].Z=757.31;};//函数功能:检查改正数是否已达到精度要求//参数说明:data:保存改正数的数组boolCheckPrecision(double*data){//0.1'(角度)=2.9088820866572159615394846141477e-5(弧度)boolret;ret=(fabs(data[0])<0.000001&&fabs(data[1])<0.000001&&fabs(data[2])<0.000001&&fabs(data[3])<2.9088820866572159615394846141477e-5&&fabs(data[4])<2.9088820866572159615394846141477e-5&&fabs(data[5])<2.9088820866572159615394846141477e-5);returnret;};//函数功能:迭代器:计算的主体部分//函数说明:sd:保存原始数据的结构体数组、PhotographicScale:摄影比例尺、focus:摄影机主距、filename:坐标数据的文件名voidIterator(SourceDatasd[N],doublePhotographicScale,doubleFocus){doublephi,omega,kappa,Xs,Ys,Zs;phi=omega=kappa=0.0;Xs=Ys=Zs=0.0;for(intk=0;k{sd[k].x/=1000.0;sd[k].y/=1000.0;Xs+=sd[k].X;Ys+=sd[k].Y;}Xs/=N;Ys/=N;doublef=Focus/1000.0;//focus和m在main函数中输入doublem=PhotographicScale;Zs=m*f;printf("外方位元素的初始值:\n");printf("Xs=%lf\nYs=%lf\nZs=%lf\n",Xs,Ys,Zs);//声明并初始化六元素改正数矩阵doubledata[6]={1,1,1,1,1,1};doublex0(0);doubley0(0);//内方位元素doubleX0[N]={0.0};doubleY0[N]={0.0};doubleZ0[N]={0.0};//声明旋转矩阵doubleR[9];doubleA[2*6]={0.0};doubleAT[6*2]={0.0};doubleBuf1[36]={0.0};doubleBuf2[36]={0.0};//ATA累加doubleBuf3[6]={0.0};doubleBuf4[6]={0.0};//ATL累加doubleBuf5[8*6]={0.0};//存储8×6的A矩阵,没办法doubleBuf6[8*1]={0.0};//存储8×1的L矩阵,同上doubleV[8*1]={0.0};doubleATA[36]={0.0};doubleATL[6]={0.0};doubleL[2]={0.0};intiCount=0;printf("开始迭代计算:\n");while(!CheckPrecision(data)){printf("第%d次迭代\n",++iCount);if(iCount==MAXITEARATION){printf("迭代次数超限,可能不收敛\n");break;}//每次迭代之前必须清空两个保存累加值的矩阵ATA与ATLfor(inti=0;i!=36;i++){ATA[i]=0.0;if(i<6){ATL[i]=0.0;//有问题?}}//计算旋转矩阵R[0]=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);R[1]=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);R[2]=-sin(phi)*cos(omega);R[3]=cos(omega)*sin(kappa);R[4]=cos(omega)*cos(kappa);R[5]=-sin(omega);R[6]=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);R[7]=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa);R[8]=cos(phi)*cos(omega);for(inti=0;i!=N;i++){Z0[i]=R[2]*(sd[i].X-Xs)+R[5]*(sd[i].Y-Ys)+R[8]*(sd[i].Z-Zs);X0[i]=x0-f*(R[0]*(sd[i].X-Xs)+R[3]*(sd[i].Y-Ys)+R[6]*(sd[i].Z-Zs))/Z0[i];Y0[i]=y0-f*(R[1]*(sd[i].X-Xs)+R[4]*(sd[i].Y-Ys)+R[7]*(sd[i].Z-Zs))/Z0[i];A[0]=((R[0]*f+R[2]*(sd[i].x-x0))/Z0[i]);A[1]=((R[3]*f+R[5]*(sd[i].x-x0))/Z0[i]);A[2]=((R[6]*f+R[8]*(sd[i].x-x0))/Z0[i]);A[3]=((sd[i].y-y0)*sin(omega)-((sd[i].x-x0)*((sd[i].x-x0)*cos(kappa)\-(sd[i].y-y0)*sin(kappa))/f+f*cos(kappa))*cos(omega));A[4]=(-f*sin(kappa)-(sd[i].x-x0)*((sd[i].x-x0)*sin(kappa)+(sd[i].y-y0)*cos(kappa))/f);A[5]=(sd[i].y-y0);A[6]=((R[1]*f+R[2]*(sd[i].y-y0))/Z0[i]);A[7]=((R[4]*f+R[5]*(sd[i].y-y0))/Z0[i]);A[8]=((R[7]*f+R[8]*(sd[i].y-y0))/Z0[i]);A[9]=(-(sd[i].x-x0)*sin(omega)-((sd[i].y-y0)*((sd[i].x-x0)*cos(kappa)\-(sd[i].y-y0)*sin(kappa))/f-f*sin(kappa))*cos(omega));A[10]=(-f*cos(kappa)-(sd[i].y-y0)*((sd[i].x-x0)*sin(kappa)+(sd[i].y-y0)*cos(kappa))/f);A[11]=(-(sd[i].x-x0));//该循环保存A矩阵,最后评定精度用for(intl=0;l<12;l++){Buf5[12*i+l]=A[l];}//所谓的逐步法化,即要在循环内部就将ATA计算出来累加,下面的L矩阵类似MatrixTranspose(A,AT,2,6);MatrixMultiply(AT,A,Buf1,6,2,6);MatrixCopy(ATA,Buf2,36);MatrixAdd(Buf1,Buf2,ATA,36);//为逐步法化后的ATA矩阵累加L[0]=(sd[i].x-X0[i]);L[1]=(sd[i].y-Y0[i]);//保存L矩阵,最后评定精度用for(intl=0;l<2;l++){Buf6[2*i+l]=L[l];}MatrixMultiply(AT,L,Buf3,6,2,1);MatrixCopy(ATL,Buf4,6);MatrixAdd(Buf3,Buf4,ATL,6);}//for//“逐步法化”的另一处不同,出循环即可直接计算ATA逆乘ATLMatrixInversion(ATA,6);MatrixMultiply(ATA,ATL,data,6,6,1);//data即为改正数Xs+=data[0];Ys+=data[1];Zs+=data[2];phi+=data[3];omega+=data[4];kappa+=data[5];printf("改正数值:\n");for(inti=0;i!=6;i++){printf("data[%d]=%lf",i,data[i]);}printf("六元素值:\n");printf("Xs=%lf\nYs=%lf\nZs=%lf\n",Xs,Ys,Zs);printf("kappa=%lf\nomega=%lf\nphi=%lf\n",kappa,omega,phi);}//whileEOEOeoeo;eoeo.kappa=kappa;eoeo.omega=omega;eoeo.phi=phi;eoeo.Xs=Xs;eoeo.Ys=Ys;eoeo.Zs=Zs;printf("正常退出迭代\n");//精度评定doubleQ[6]={0.0};for(inth=0;h<6;h++){Q[h]=ATA[h*6+h];}MatrixMultiply(Buf5,data,V,8,6,1);//V=Ax-LMatrixMinus(V,Buf6,V,8); doublem0(0);//单位权中误差doubleVSum(0);//[vv],即平方和inti;for(i=0;i<8;i++){VSum+=V[i]*V[i];}m0=sqrt(VSum/(2*N-6));//中误差m0doubleM[6]={0.0};//保存六个值的中误差for(i=0;i!=6;i++){M[i]=m0*sqrt(Q[i]);if(i>=3){M[i]=M[i]*180*3600/PI;}}OutputResult(&eoeo,R,M,m0);printf("解算全部完成\n");};//函数功能:输出解算结果//参数说明:eoeo:指向最终解算结果的结构体数组,RotationMatrix:旋转矩阵//Precision:保存计算精度的数组,m0:单位权中误差voidOutputResult(EOEO*eoeo,double*RotationMatrix,double*Precision,doublem0){printf("计算结果:\n");printf("六个外方位元素为:\n");printf("Xs=%.4f\n",eoeo->Xs);printf("Ys=%.4f\n",eoeo->Ys);printf("Zs=%.4f\n",eoeo->Zs);printf("phi=%.10f\n",eoeo->phi);printf("omega=%.10f\n",eoeo->omega);printf("kappa=%.10f\n",eoeo->kappa);printf("旋转矩阵:\n");printf("%lf%lf%lf\n%lf%lf%lf\n%lf%lf%lf\n",RotationMatrix[0],RotationMatrix[1],RotationMatrix[2],RotationMatrix[3],RotationMatrix[4],RotationMatrix[5],RotationMatrix[6],RotationMatrix[7],RotationMatrix[8]);printf("单位权中误差:\n");printf("六元素精度:\n");printf("Xs=%lf\n",Precision[0]);printf("Ys=%lf\n",Precision[1]);printf("Zs=%lf\n",Precision[2]);printf("phi=%lf\n",Precision[3]);printf("omega=%lf\n",Precision[4]);printf("kap
MatrixC[i*BColumn+j]=0.0;
for(k=0;kMatrixC[i*BColumn+j]+=MatrixA[i*AColumn+k]*MatrixB[j+k*BColumn];}}//矩阵相加重载1//参数说明voidMatrixAdd(double*MatrixA,double*MatrixB,double*MatrixC,intRow,intColumn){inti;intj;for(i=0;i!=Row;i++){for(j=0;j!=Column;j++){MatrixC[i*Column+j]=MatrixA[i*Column+j]+MatrixB[i*Column+j];}}}//矩阵相加重载2//A+B=C//voidMatrixAdd(double*MatrixA,double*MatrixB,double*MatrixC,intlength){inti;for(i=0;i!=length;i++){MatrixC[i]=MatrixA[i]+MatrixB[i];}}//矩阵相减,可做类似相加函数的重载//A-B=CvoidMatrixMinus(double*MatrixA,double*MatrixB,double*MatrixC,intlength){inti;for(i=0;i!=length;i++){MatrixC[i]=MatrixA[i]-MatrixB[i];}} //矩阵复制//B=AvoidMatrixCopy(double*MatrixA,double*MatrixB,intlength){inti;for(i=0;i!=length;i++){MatrixB[i]=MatrixA[i];}} //函数功能:初始化坐标数据voidInitData(SourceData*sd){sd[0].x=-86.15;sd[0].y=-68.99;sd[0].X=36589.41;sd[0].Y=25273.32;sd[0].Z=2195.17;sd[1].x=-53.40;sd[1].y=82.21;sd[1].X=37631.08;sd[1].Y=31324.51;sd[1].Z=728.69;sd[2].x=-14.78;sd[2].y=-76.63;sd[2].X=39100.97;sd[2].Y=24934.98;sd[2].Z=2386.50;sd[3].x=10.46;sd[3].y=64.43;sd[3].X=40426.54;sd[3].Y=30319.81;sd[3].Z=757.31;};//函数功能:检查改正数是否已达到精度要求//参数说明:data:保存改正数的数组boolCheckPrecision(double*data){//0.1'(角度)=2.9088820866572159615394846141477e-5(弧度)boolret;ret=(fabs(data[0])<0.000001&&fabs(data[1])<0.000001&&fabs(data[2])<0.000001&&fabs(data[3])<2.9088820866572159615394846141477e-5&&fabs(data[4])<2.9088820866572159615394846141477e-5&&fabs(data[5])<2.9088820866572159615394846141477e-5);returnret;};//函数功能:迭代器:计算的主体部分//函数说明:sd:保存原始数据的结构体数组、PhotographicScale:摄影比例尺、focus:摄影机主距、filename:坐标数据的文件名voidIterator(SourceDatasd[N],doublePhotographicScale,doubleFocus){doublephi,omega,kappa,Xs,Ys,Zs;phi=omega=kappa=0.0;Xs=Ys=Zs=0.0;for(intk=0;k{sd[k].x/=1000.0;sd[k].y/=1000.0;Xs+=sd[k].X;Ys+=sd[k].Y;}Xs/=N;Ys/=N;doublef=Focus/1000.0;//focus和m在main函数中输入doublem=PhotographicScale;Zs=m*f;printf("外方位元素的初始值:\n");printf("Xs=%lf\nYs=%lf\nZs=%lf\n",Xs,Ys,Zs);//声明并初始化六元素改正数矩阵doubledata[6]={1,1,1,1,1,1};doublex0(0);doubley0(0);//内方位元素doubleX0[N]={0.0};doubleY0[N]={0.0};doubleZ0[N]={0.0};//声明旋转矩阵doubleR[9];doubleA[2*6]={0.0};doubleAT[6*2]={0.0};doubleBuf1[36]={0.0};doubleBuf2[36]={0.0};//ATA累加doubleBuf3[6]={0.0};doubleBuf4[6]={0.0};//ATL累加doubleBuf5[8*6]={0.0};//存储8×6的A矩阵,没办法doubleBuf6[8*1]={0.0};//存储8×1的L矩阵,同上doubleV[8*1]={0.0};doubleATA[36]={0.0};doubleATL[6]={0.0};doubleL[2]={0.0};intiCount=0;printf("开始迭代计算:\n");while(!CheckPrecision(data)){printf("第%d次迭代\n",++iCount);if(iCount==MAXITEARATION){printf("迭代次数超限,可能不收敛\n");break;}//每次迭代之前必须清空两个保存累加值的矩阵ATA与ATLfor(inti=0;i!=36;i++){ATA[i]=0.0;if(i<6){ATL[i]=0.0;//有问题?}}//计算旋转矩阵R[0]=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);R[1]=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);R[2]=-sin(phi)*cos(omega);R[3]=cos(omega)*sin(kappa);R[4]=cos(omega)*cos(kappa);R[5]=-sin(omega);R[6]=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);R[7]=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa);R[8]=cos(phi)*cos(omega);for(inti=0;i!=N;i++){Z0[i]=R[2]*(sd[i].X-Xs)+R[5]*(sd[i].Y-Ys)+R[8]*(sd[i].Z-Zs);X0[i]=x0-f*(R[0]*(sd[i].X-Xs)+R[3]*(sd[i].Y-Ys)+R[6]*(sd[i].Z-Zs))/Z0[i];Y0[i]=y0-f*(R[1]*(sd[i].X-Xs)+R[4]*(sd[i].Y-Ys)+R[7]*(sd[i].Z-Zs))/Z0[i];A[0]=((R[0]*f+R[2]*(sd[i].x-x0))/Z0[i]);A[1]=((R[3]*f+R[5]*(sd[i].x-x0))/Z0[i]);A[2]=((R[6]*f+R[8]*(sd[i].x-x0))/Z0[i]);A[3]=((sd[i].y-y0)*sin(omega)-((sd[i].x-x0)*((sd[i].x-x0)*cos(kappa)\-(sd[i].y-y0)*sin(kappa))/f+f*cos(kappa))*cos(omega));A[4]=(-f*sin(kappa)-(sd[i].x-x0)*((sd[i].x-x0)*sin(kappa)+(sd[i].y-y0)*cos(kappa))/f);A[5]=(sd[i].y-y0);A[6]=((R[1]*f+R[2]*(sd[i].y-y0))/Z0[i]);A[7]=((R[4]*f+R[5]*(sd[i].y-y0))/Z0[i]);A[8]=((R[7]*f+R[8]*(sd[i].y-y0))/Z0[i]);A[9]=(-(sd[i].x-x0)*sin(omega)-((sd[i].y-y0)*((sd[i].x-x0)*cos(kappa)\-(sd[i].y-y0)*sin(kappa))/f-f*sin(kappa))*cos(omega));A[10]=(-f*cos(kappa)-(sd[i].y-y0)*((sd[i].x-x0)*sin(kappa)+(sd[i].y-y0)*cos(kappa))/f);A[11]=(-(sd[i].x-x0));//该循环保存A矩阵,最后评定精度用for(intl=0;l<12;l++){Buf5[12*i+l]=A[l];}//所谓的逐步法化,即要在循环内部就将ATA计算出来累加,下面的L矩阵类似MatrixTranspose(A,AT,2,6);MatrixMultiply(AT,A,Buf1,6,2,6);MatrixCopy(ATA,Buf2,36);MatrixAdd(Buf1,Buf2,ATA,36);//为逐步法化后的ATA矩阵累加L[0]=(sd[i].x-X0[i]);L[1]=(sd[i].y-Y0[i]);//保存L矩阵,最后评定精度用for(intl=0;l<2;l++){Buf6[2*i+l]=L[l];}MatrixMultiply(AT,L,Buf3,6,2,1);MatrixCopy(ATL,Buf4,6);MatrixAdd(Buf3,Buf4,ATL,6);}//for//“逐步法化”的另一处不同,出循环即可直接计算ATA逆乘ATLMatrixInversion(ATA,6);MatrixMultiply(ATA,ATL,data,6,6,1);//data即为改正数Xs+=data[0];Ys+=data[1];Zs+=data[2];phi+=data[3];omega+=data[4];kappa+=data[5];printf("改正数值:\n");for(inti=0;i!=6;i++){printf("data[%d]=%lf",i,data[i]);}printf("六元素值:\n");printf("Xs=%lf\nYs=%lf\nZs=%lf\n",Xs,Ys,Zs);printf("kappa=%lf\nomega=%lf\nphi=%lf\n",kappa,omega,phi);}//whileEOEOeoeo;eoeo.kappa=kappa;eoeo.omega=omega;eoeo.phi=phi;eoeo.Xs=Xs;eoeo.Ys=Ys;eoeo.Zs=Zs;printf("正常退出迭代\n");//精度评定doubleQ[6]={0.0};for(inth=0;h<6;h++){Q[h]=ATA[h*6+h];}MatrixMultiply(Buf5,data,V,8,6,1);//V=Ax-LMatrixMinus(V,Buf6,V,8); doublem0(0);//单位权中误差doubleVSum(0);//[vv],即平方和inti;for(i=0;i<8;i++){VSum+=V[i]*V[i];}m0=sqrt(VSum/(2*N-6));//中误差m0doubleM[6]={0.0};//保存六个值的中误差for(i=0;i!=6;i++){M[i]=m0*sqrt(Q[i]);if(i>=3){M[i]=M[i]*180*3600/PI;}}OutputResult(&eoeo,R,M,m0);printf("解算全部完成\n");};//函数功能:输出解算结果//参数说明:eoeo:指向最终解算结果的结构体数组,RotationMatrix:旋转矩阵//Precision:保存计算精度的数组,m0:单位权中误差voidOutputResult(EOEO*eoeo,double*RotationMatrix,double*Precision,doublem0){printf("计算结果:\n");printf("六个外方位元素为:\n");printf("Xs=%.4f\n",eoeo->Xs);printf("Ys=%.4f\n",eoeo->Ys);printf("Zs=%.4f\n",eoeo->Zs);printf("phi=%.10f\n",eoeo->phi);printf("omega=%.10f\n",eoeo->omega);printf("kappa=%.10f\n",eoeo->kappa);printf("旋转矩阵:\n");printf("%lf%lf%lf\n%lf%lf%lf\n%lf%lf%lf\n",RotationMatrix[0],RotationMatrix[1],RotationMatrix[2],RotationMatrix[3],RotationMatrix[4],RotationMatrix[5],RotationMatrix[6],RotationMatrix[7],RotationMatrix[8]);printf("单位权中误差:\n");printf("六元素精度:\n");printf("Xs=%lf\n",Precision[0]);printf("Ys=%lf\n",Precision[1]);printf("Zs=%lf\n",Precision[2]);printf("phi=%lf\n",Precision[3]);printf("omega=%lf\n",Precision[4]);printf("kap
MatrixC[i*BColumn+j]+=MatrixA[i*AColumn+k]*MatrixB[j+k*BColumn];
//矩阵相加重载1
//参数说明
voidMatrixAdd(double*MatrixA,double*MatrixB,double*MatrixC,intRow,intColumn)
=Row;i++)
=Column;j++)
MatrixC[i*Column+j]=MatrixA[i*Column+j]+MatrixB[i*Column+j];
//矩阵相加重载2
//A+B=C
//
voidMatrixAdd(double*MatrixA,double*MatrixB,double*MatrixC,intlength)
=length;i++)
MatrixC[i]=MatrixA[i]+MatrixB[i];
//矩阵相减,可做类似相加函数的重载
//A-B=C
voidMatrixMinus(double*MatrixA,double*MatrixB,double*MatrixC,intlength)
MatrixC[i]=MatrixA[i]-MatrixB[i];
//矩阵复制
//B=A
voidMatrixCopy(double*MatrixA,double*MatrixB,intlength)
MatrixB[i]=MatrixA[i];
//函数功能:
初始化坐标数据
voidInitData(SourceData*sd)
sd[0].x=-86.15;sd[0].y=-68.99;sd[0].X=36589.41;sd[0].Y=25273.32;sd[0].Z=2195.17;
sd[1].x=-53.40;sd[1].y=82.21;sd[1].X=37631.08;sd[1].Y=31324.51;sd[1].Z=728.69;
sd[2].x=-14.78;sd[2].y=-76.63;sd[2].X=39100.97;sd[2].Y=24934.98;sd[2].Z=2386.50;
sd[3].x=10.46;sd[3].y=64.43;sd[3].X=40426.54;sd[3].Y=30319.81;sd[3].Z=757.31;
检查改正数是否已达到精度要求
data:
保存改正数的数组
boolCheckPrecision(double*data)
//0.1'(角度)=2.9088820866572159615394846141477e-5(弧度)
boolret;
ret=(fabs(data[0])<0.000001&&fabs(data[1])<0.000001&&fabs(data[2])<0.000001&&fabs(data[3])<2.9088820866572159615394846141477e-5&&fabs(data[4])<2.9088820866572159615394846141477e-5&&fabs(data[5])<2.9088820866572159615394846141477e-5);
returnret;
迭代器:
计算的主体部分
//函数说明:
sd:
保存原始数据的结构体数组、PhotographicScale:
摄影比例尺、focus:
摄影机主距、filename:
坐标数据的文件名
voidIterator(SourceDatasd[N],doublePhotographicScale,doubleFocus)
doublephi,omega,kappa,Xs,Ys,Zs;
phi=omega=kappa=0.0;
Xs=Ys=Zs=0.0;
for(intk=0;k{sd[k].x/=1000.0;sd[k].y/=1000.0;Xs+=sd[k].X;Ys+=sd[k].Y;}Xs/=N;Ys/=N;doublef=Focus/1000.0;//focus和m在main函数中输入doublem=PhotographicScale;Zs=m*f;printf("外方位元素的初始值:\n");printf("Xs=%lf\nYs=%lf\nZs=%lf\n",Xs,Ys,Zs);//声明并初始化六元素改正数矩阵doubledata[6]={1,1,1,1,1,1};doublex0(0);doubley0(0);//内方位元素doubleX0[N]={0.0};doubleY0[N]={0.0};doubleZ0[N]={0.0};//声明旋转矩阵doubleR[9];doubleA[2*6]={0.0};doubleAT[6*2]={0.0};doubleBuf1[36]={0.0};doubleBuf2[36]={0.0};//ATA累加doubleBuf3[6]={0.0};doubleBuf4[6]={0.0};//ATL累加doubleBuf5[8*6]={0.0};//存储8×6的A矩阵,没办法doubleBuf6[8*1]={0.0};//存储8×1的L矩阵,同上doubleV[8*1]={0.0};doubleATA[36]={0.0};doubleATL[6]={0.0};doubleL[2]={0.0};intiCount=0;printf("开始迭代计算:\n");while(!CheckPrecision(data)){printf("第%d次迭代\n",++iCount);if(iCount==MAXITEARATION){printf("迭代次数超限,可能不收敛\n");break;}//每次迭代之前必须清空两个保存累加值的矩阵ATA与ATLfor(inti=0;i!=36;i++){ATA[i]=0.0;if(i<6){ATL[i]=0.0;//有问题?}}//计算旋转矩阵R[0]=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);R[1]=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);R[2]=-sin(phi)*cos(omega);R[3]=cos(omega)*sin(kappa);R[4]=cos(omega)*cos(kappa);R[5]=-sin(omega);R[6]=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);R[7]=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa);R[8]=cos(phi)*cos(omega);for(inti=0;i!=N;i++){Z0[i]=R[2]*(sd[i].X-Xs)+R[5]*(sd[i].Y-Ys)+R[8]*(sd[i].Z-Zs);X0[i]=x0-f*(R[0]*(sd[i].X-Xs)+R[3]*(sd[i].Y-Ys)+R[6]*(sd[i].Z-Zs))/Z0[i];Y0[i]=y0-f*(R[1]*(sd[i].X-Xs)+R[4]*(sd[i].Y-Ys)+R[7]*(sd[i].Z-Zs))/Z0[i];A[0]=((R[0]*f+R[2]*(sd[i].x-x0))/Z0[i]);A[1]=((R[3]*f+R[5]*(sd[i].x-x0))/Z0[i]);A[2]=((R[6]*f+R[8]*(sd[i].x-x0))/Z0[i]);A[3]=((sd[i].y-y0)*sin(omega)-((sd[i].x-x0)*((sd[i].x-x0)*cos(kappa)\-(sd[i].y-y0)*sin(kappa))/f+f*cos(kappa))*cos(omega));A[4]=(-f*sin(kappa)-(sd[i].x-x0)*((sd[i].x-x0)*sin(kappa)+(sd[i].y-y0)*cos(kappa))/f);A[5]=(sd[i].y-y0);A[6]=((R[1]*f+R[2]*(sd[i].y-y0))/Z0[i]);A[7]=((R[4]*f+R[5]*(sd[i].y-y0))/Z0[i]);A[8]=((R[7]*f+R[8]*(sd[i].y-y0))/Z0[i]);A[9]=(-(sd[i].x-x0)*sin(omega)-((sd[i].y-y0)*((sd[i].x-x0)*cos(kappa)\-(sd[i].y-y0)*sin(kappa))/f-f*sin(kappa))*cos(omega));A[10]=(-f*cos(kappa)-(sd[i].y-y0)*((sd[i].x-x0)*sin(kappa)+(sd[i].y-y0)*cos(kappa))/f);A[11]=(-(sd[i].x-x0));//该循环保存A矩阵,最后评定精度用for(intl=0;l<12;l++){Buf5[12*i+l]=A[l];}//所谓的逐步法化,即要在循环内部就将ATA计算出来累加,下面的L矩阵类似MatrixTranspose(A,AT,2,6);MatrixMultiply(AT,A,Buf1,6,2,6);MatrixCopy(ATA,Buf2,36);MatrixAdd(Buf1,Buf2,ATA,36);//为逐步法化后的ATA矩阵累加L[0]=(sd[i].x-X0[i]);L[1]=(sd[i].y-Y0[i]);//保存L矩阵,最后评定精度用for(intl=0;l<2;l++){Buf6[2*i+l]=L[l];}MatrixMultiply(AT,L,Buf3,6,2,1);MatrixCopy(ATL,Buf4,6);MatrixAdd(Buf3,Buf4,ATL,6);}//for//“逐步法化”的另一处不同,出循环即可直接计算ATA逆乘ATLMatrixInversion(ATA,6);MatrixMultiply(ATA,ATL,data,6,6,1);//data即为改正数Xs+=data[0];Ys+=data[1];Zs+=data[2];phi+=data[3];omega+=data[4];kappa+=data[5];printf("改正数值:\n");for(inti=0;i!=6;i++){printf("data[%d]=%lf",i,data[i]);}printf("六元素值:\n");printf("Xs=%lf\nYs=%lf\nZs=%lf\n",Xs,Ys,Zs);printf("kappa=%lf\nomega=%lf\nphi=%lf\n",kappa,omega,phi);}//whileEOEOeoeo;eoeo.kappa=kappa;eoeo.omega=omega;eoeo.phi=phi;eoeo.Xs=Xs;eoeo.Ys=Ys;eoeo.Zs=Zs;printf("正常退出迭代\n");//精度评定doubleQ[6]={0.0};for(inth=0;h<6;h++){Q[h]=ATA[h*6+h];}MatrixMultiply(Buf5,data,V,8,6,1);//V=Ax-LMatrixMinus(V,Buf6,V,8); doublem0(0);//单位权中误差doubleVSum(0);//[vv],即平方和inti;for(i=0;i<8;i++){VSum+=V[i]*V[i];}m0=sqrt(VSum/(2*N-6));//中误差m0doubleM[6]={0.0};//保存六个值的中误差for(i=0;i!=6;i++){M[i]=m0*sqrt(Q[i]);if(i>=3){M[i]=M[i]*180*3600/PI;}}OutputResult(&eoeo,R,M,m0);printf("解算全部完成\n");};//函数功能:输出解算结果//参数说明:eoeo:指向最终解算结果的结构体数组,RotationMatrix:旋转矩阵//Precision:保存计算精度的数组,m0:单位权中误差voidOutputResult(EOEO*eoeo,double*RotationMatrix,double*Precision,doublem0){printf("计算结果:\n");printf("六个外方位元素为:\n");printf("Xs=%.4f\n",eoeo->Xs);printf("Ys=%.4f\n",eoeo->Ys);printf("Zs=%.4f\n",eoeo->Zs);printf("phi=%.10f\n",eoeo->phi);printf("omega=%.10f\n",eoeo->omega);printf("kappa=%.10f\n",eoeo->kappa);printf("旋转矩阵:\n");printf("%lf%lf%lf\n%lf%lf%lf\n%lf%lf%lf\n",RotationMatrix[0],RotationMatrix[1],RotationMatrix[2],RotationMatrix[3],RotationMatrix[4],RotationMatrix[5],RotationMatrix[6],RotationMatrix[7],RotationMatrix[8]);printf("单位权中误差:\n");printf("六元素精度:\n");printf("Xs=%lf\n",Precision[0]);printf("Ys=%lf\n",Precision[1]);printf("Zs=%lf\n",Precision[2]);printf("phi=%lf\n",Precision[3]);printf("omega=%lf\n",Precision[4]);printf("kap
sd[k].x/=1000.0;
sd[k].y/=1000.0;
Xs+=sd[k].X;
Ys+=sd[k].Y;
Xs/=N;
Ys/=N;
doublef=Focus/1000.0;//focus和m在main函数中输入
doublem=PhotographicScale;
Zs=m*f;
printf("外方位元素的初始值:
\n");
printf("Xs=%lf\nYs=%lf\nZs=%lf\n",Xs,Ys,Zs);
//声明并初始化六元素改正数矩阵
doubledata[6]={1,1,1,1,1,1};
doublex0(0);
doubley0(0);//内方位元素
doubleX0[N]={0.0};
doubleY0[N]={0.0};
doubleZ0[N]={0.0};
//声明旋转矩阵
doubleR[9];
doubleA[2*6]={0.0};
doubleAT[6*2]={0.0};
doubleBuf1[36]={0.0};
doubleBuf2[36]={0.0};//ATA累加
doubleBuf3[6]={0.0};
doubleBuf4[6]={0.0};//ATL累加
doubleBuf5[8*6]={0.0};//存储8×6的A矩阵,没办法
doubleBuf6[8*1]={0.0};//存储8×1的L矩阵,同上
doubleV[8*1]={0.0};
doubleATA[36]={0.0};
doubleATL[6]={0.0};
doubleL[2]={0.0};
intiCount=0;
printf("开始迭代计算:
while(!
CheckPrecision(data))
printf("第%d次迭代\n",++iCount);
if(iCount==MAXITEARATION)
printf("迭代次数超限,可能不收敛\n");
break;
//每次迭代之前必须清空两个保存累加值的矩阵ATA与ATL
for(inti=0;i!
=36;i++)
ATA[i]=0.0;
if(i<6)
ATL[i]=0.0;//有问题?
//计算旋转矩阵
R[0]=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);
R[1]=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);
R[2]=-sin(phi)*cos(omega);
R[3]=cos(omega)*sin(kappa);
R[4]=cos(omega)*cos(kappa);
R[5]=-sin(omega);
R[6]=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);
R[7]=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa);
R[8]=cos(phi)*cos(omega);
=N;i++)
Z0[i]=R[2]*(sd[i].X-Xs)+R[5]*(sd[i].Y-Ys)+R[8]*(sd[i].Z-Zs);
X0[i]=x0-f*(R[0]*(sd[i].X-Xs)+R[3]*(sd[i].Y-Ys)+R[6]*(sd[i].Z-Zs))/Z0[i];
Y0[i]=y0-f*(R[1]*(sd[i].X-Xs)+R[4]*(sd[i].Y-Ys)+R[7]*(sd[i].Z-Zs))/Z0[i];
A[0]=((R[0]*f+R[2]*(sd[i].x-x0))/Z0[i]);
A[1]=((R[3]*f+R[5]*(sd[i].x-x0))/Z0[i]);
A[2]=((R[6]*f+R[8]*(sd[i].x-x0))/Z0[i]);
A[3]=((sd[i].y-y0)*sin(omega)-((sd[i].x-x0)*((sd[i].x-x0)*cos(kappa)\
-(sd[i].y-y0)*sin(kappa))/f+f*cos(kappa))*cos(omega));
A[4]=(-f*sin(kappa)-(sd[i].x-x0)*((sd[i].x-x0)*sin(kappa)+(sd[i].y-y0)*cos(kappa))/f);
A[5]=(sd[i].y-y0);
A[6]=((R[1]*f+R[2]*(sd[i].y-y0))/Z0[i]);
A[7]=((R[4]*f+R[5]*(sd[i].y-y0))/Z0[i]);
A[8]=((R[7]*f+R[8]*(sd[i].y-y0))/Z0[i]);
A[9]=(-(sd[i].x-x0)*sin(omega)-((sd[i].y-y0)*((sd[i].x-x0)*cos(kappa)\
-(sd[i].y-y0)*sin(kappa))/f-f*sin(kappa))*cos(omega));
A[10]=(-f*cos(kappa)-(sd[i].y-y0)*((sd[i].x-x0)*sin(kappa)+(sd[i].y-y0)*cos(kappa))/f);
A[11]=(-(sd[i].x-x0));
//该循环保存A矩阵,最后评定精度用
for(intl=0;l<12;l++)
Buf5[12*i+l]=A[l];
//所谓的逐步法化,即要在循环内部就将ATA计算出来累加,下面的L矩阵类似
MatrixTranspose(A,AT,2,6);
MatrixMultiply(AT,A,Buf1,6,2,6);
MatrixCopy(ATA,Buf2,36);
MatrixAdd(Buf1,Buf2,ATA,36);//为逐步法化后的ATA矩阵累加
L[0]=(sd[i].x-X0[i]);
L[1]=(sd[i].y-Y0[i]);
//保存L矩阵,最后评定精度用
for(intl=0;l<2;l++)
Buf6[2*i+l]=L[l];
MatrixMultiply(AT,L,Buf3,6,2,1);
MatrixCopy(ATL,Buf4,6);
MatrixAdd(Buf3,Buf4,ATL,6);
}//for
//“逐步法化”的另一处不同,出循环即可直接计算ATA逆乘ATL
MatrixInversion(ATA,6);
MatrixMultiply(ATA,ATL,data,6,6,1);
//data即为改正数
Xs+=data[0];
Ys+=data[1];
Zs+=data[2];
phi+=data[3];
omega+=data[4];
kappa+=data[5];
printf("改正数值:
=6;i++)
printf("data[%d]=%lf",i,data[i]);
printf("六元素值:
printf("kappa=%lf\nomega=%lf\nphi=%lf\n",kappa,omega,phi);
}//while
EOEOeoeo;
eoeo.kappa=kappa;
eoeo.omega=omega;
eoeo.phi=phi;
eoeo.Xs=Xs;
eoeo.Ys=Ys;
eoeo.Zs=Zs;
printf("正常退出迭代\n");
//精度评定
doubleQ[6]={0.0};
for(inth=0;h<6;h++)
Q[h]=ATA[h*6+h];
MatrixMultiply(Buf5,data,V,8,6,1);//V=Ax-L
MatrixMinus(V,Buf6,V,8);
doublem0(0);//单位权中误差
doubleVSum(0);//[vv],即平方和
for(i=0;i<8;i++)
VSum+=V[i]*V[i];
m0=sqrt(VSum/(2*N-6));//中误差m0
doubleM[6]={0.0};//保存六个值的中误差
M[i]=m0*sqrt(Q[i]);
if(i>=3)
M[i]=M[i]*180*3600/PI;
OutputResult(&eoeo,R,M,m0);
printf("解算全部完成\n");
输出解算结果
eoeo:
指向最终解算结果的结构体数组,RotationMatrix:
旋转矩阵
//Precision:
保存计算精度的数组,m0:
单位权中误差
voidOutputResult(EOEO*eoeo,double*RotationMatrix,double*Precision,doublem0)
printf("计算结果:
printf("六个外方位元素为:
printf("Xs=%.4f\n",eoeo->Xs);
printf("Ys=%.4f\n",eoeo->Ys);
printf("Zs=%.4f\n",eoeo->Zs);
printf("phi=%.10f\n",eoeo->phi);
printf("omega=%.10f\n",eoeo->omega);
printf("kappa=%.10f\n",eoeo->kappa);
printf("旋转矩阵:
printf("%lf%lf%lf\n%lf%lf%lf\n%lf%lf%lf\n",RotationMatrix[0],RotationMatrix[1],RotationMatrix[2],RotationMatrix[3],RotationMatrix[4],RotationMatrix[5],RotationMatrix[6],RotationMatrix[7],RotationMatrix[8]);
printf("单位权中误差:
printf("六元素精度:
printf("Xs=%lf\n",Precision[0]);
printf("Ys=%lf\n",Precision[1]);
printf("Zs=%lf\n",Precision[2]);
printf("phi=%lf\n",Precision[3]);
printf("omega=%lf\n",Precision[4]);
printf("kap
copyright@ 2008-2022 冰豆网网站版权所有
经营许可证编号:鄂ICP备2022015515号-1