1、否则用新的近似值重复（4）（8）步骤的计算，直到满足要求为止。4、精度评定：通过法方程式中未知数的系数矩阵的逆阵（ATA）-1来解求，此时视像点坐标为等精度不相关观测值。因为（ATA）-1中第i个主对角线上的元素Qii就是法方程式中第i个未知数的权倒数，若单位权中误差为m0，则第i个未知数的中误差为：mi=m0当参加空间后方交会的控制点有n个时，则单位权中误差可按下式计算：M0=三、 实习思路四、 代码实现#define N 4 /控制点个数#define PI 3.1415926#define MAXITEARATION 5 /最大允许迭代次数struct EOEO/elements of
2、exterior orientation外方位元素 double Xs; double Ys; double Zs; double omega; double phi; double kappa;struct SourceData /原始数据 double x; double y; double X; double Y; double Z;/ 矩阵转置/ 参数说明：原始矩阵MatrixOrigin为mnvoid MatrixTranspose（double* MatrixOrigin, double* MatrixNew, int m, int n） int i; int j; for （i
3、= 0; i != n; i +） for （j = 0; j != m; j +） MatrixNewi*m+j = MatrixOriginj*n+i; / 矩阵求逆m：原始矩阵Matrix的行/列数void MatrixInversion（double* Matrix, int m） int i, j, k; for（k = 0; k ! k +） for（i = 0; if（i != k） Matrixi*m+k = - Matrixi*m+k / Matrixk*m+k; Matrixk*m+k = 1 / Matrixk*m+k; for（j = 0; if（j ! Matrixi
4、*m+j += Matrixk*m+j * Matrixi*m+k; for（j = 0; if（j ! Matrixk*m+j *= Matrixk*m+k;/ 矩阵相乘C=AB，ARow：A的行数，AColumn：A的列数，BColumn：B的列数void MatrixMultiply（double* MatrixA, double* MatrixB, double* MatrixC, int ARow, int AColumn, int BColumn） int k; for（i = 0;= ARow; for（j=0; jBColumn; j+） MatrixCi*BColumn+j
5、= 0.0; kAColumn; k+） MatrixCi*BColumn+j += MatrixAi*AColumn+k * MatrixBj+k*BColumn;/ 矩阵相加 重载1/ 参数说明 void MatrixAdd（double* MatrixA, double* MatrixB, double* MatrixC, int Row, int Column）= Row;= Column; MatrixCi*Column+j = MatrixAi*Column+j + MatrixBi*Column+j;/ 矩阵相加 重载2/ A + B = C/ void MatrixAdd（do
6、uble* MatrixA, double* MatrixB, double* MatrixC, int length）= length; MatrixCi = MatrixAi + MatrixBi;/ 矩阵相减，可做类似相加函数的重载/ A - B = Cvoid MatrixMinus（double* MatrixA, double* MatrixB, double* MatrixC, int length） MatrixCi = MatrixAi - MatrixBi;/ 矩阵复制/ B = Avoid MatrixCopy（double* MatrixA, double* Matri
7、xB, int length） MatrixBi = MatrixAi;/函数功能：初始化坐标数据void InitData（SourceData* sd） sd0.x=-86.15;sd0.y=-68.99;sd0.X=36589.41;sd0.Y=25273.32;sd0.Z=2195.17; sd1.x=-53.40;sd1.y=82.21;sd1.X=37631.08;sd1.Y=31324.51;sd1.Z=728.69; sd2.x=-14.78;sd2.y=-76.63;sd2.X=39100.97;sd2.Y=24934.98;sd2.Z=2386.50; sd3.x=10.4
8、6;sd3.y=64.43;sd3.X=40426.54;sd3.Y=30319.81;sd3.Z=757.31;检查改正数是否已达到精度要求/参数说明：data：保存改正数的数组bool CheckPrecision（double* data） /0.1（角度）=2.9088820866572159615394846141477e-5（弧度） bool ret; ret=（fabs（data0）0.000001&fabs（data1）fabs（data2）fabs（data3）2.9088820866572159615394846141477e-5&fabs（data4）fabs（data5
9、）2.9088820866572159615394846141477e-5）; return ret;迭代器：计算的主体部分/函数说明：sd：保存原始数据的结构体数组、PhotographicScale：摄影比例尺、focus：摄影机主距、filename：坐标数据的文件名void Iterator（SourceData sdN,double PhotographicScale,double Focus） double phi,omega,kappa,Xs,Ys,Zs; phi=omega=kappa=0.0; Xs=Ys=Zs=0.0; for（int k=0;kN;k+） sdk.x/=1
10、000.0; sdk.y/=1000.0; Xs+=sdk.X; Ys+=sdk.Y; Xs/=N; Ys/=N; double f=Focus/1000.0;/focus和m在main函数中输入 double m=PhotographicScale; Zs=m*f; printf（外方位元素的初始值：n）;Xs=%lfn Ys=%lfn Zs=%lfn,Xs,Ys,Zs）; /声明并初始化六元素改正数矩阵 double data6 = 1,1,1,1,1,1; double x0（0）; double y0（0）;/内方位元素 double X0N = 0.0; double Y0N = 0
11、.0; double Z0N = 0.0; /声明旋转矩阵 double R9; double A2*6=0.0; double AT6*2=0.0; double Buf136 = 0.0; double Buf236 = 0.0;/ATA累加 double Buf36 = 0.0; double Buf46 = 0.0;/ATL累加 double Buf58*6 = 0.0;/存储86的A矩阵，没办法 double Buf68*1 = 0.0;1的L矩阵，同上 double V8*1 = 0.0; double ATA36 = 0.0; double ATL6 = 0.0; double
12、L2 = 0.0; int iCount=0;开始迭代计算： while（!CheckPrecision（data） printf（第%d次迭代n,+iCount）; if（iCount=MAXITEARATION） printf（迭代次数超限，可能不收敛n break; /每次迭代之前必须清空两个保存累加值的矩阵ATA与ATL for（int i=0;i!=36;i+） ATAi=0.0; if（i6） ATLi=0.0;/有问题？ /计算旋转矩阵 R0 = cos（phi）*cos（kappa） - sin（phi）*sin（omega）*sin（kappa）; R1 = -cos（phi
13、）*sin（kappa） - sin（phi）*sin（omega）*cos（kappa）; R2 = -sin（phi）*cos（omega）; R3 = cos（omega）*sin（kappa）; R4 = cos（omega）*cos（kappa）; R5 = -sin（omega）; R6 = sin（phi）*cos（kappa） + cos（phi）*sin（omega）*sin（kappa）; R7 = -sin（phi）*sin（kappa） + cos（phi）*sin（omega）*cos（kappa）; R8 = cos（phi）*cos（omega）; for （in
14、t i = 0;= N; Z0i = R2*（sdi.X-Xs） + R5*（sdi.Y-Ys） + R8*（sdi.Z-Zs）; X0i = x0 - f*（R0*（sdi.X-Xs） + R3*（sdi.Y-Ys） + R6*（sdi.Z-Zs） / Z0i; Y0i = y0 - f*（R1*（sdi.X-Xs） + R4*（sdi.Y-Ys） + R7*（sdi.Z-Zs） / Z0i; A0 = （R0*f + R2*（sdi.x-x0） / Z0i）; A1 = （R3*f + R5*（sdi.x-x0） / Z0i）; A2 = （R6*f + R8*（sdi.x-x0） / Z
15、0i）; A3 = （sdi.y-y0）*sin（omega） - （sdi.x-x0）*（sdi.x-x0）*cos（kappa） - （sdi.y-y0）*sin（kappa）/f + f*cos（kappa）*cos（omega）; A4 = （-f*sin（kappa） - （sdi.x-x0）*（sdi.x-x0）*sin（kappa） + （sdi.y-y0）*cos（kappa）/f）; A5 = （sdi.y - y0）; A6 = （R1*f + R2*（sdi.y-y0） /Z0i）; A7 = （R4*f + R5*（sdi.y-y0） /Z0i）; A8 = （R7*f
16、 + R8*（sdi.y-y0） /Z0i）; A9 = （-（sdi.x-x0）*sin（omega） - （sdi.y-y0）*（sdi.x-x0）*cos（kappa） - （sdi.y-y0）*sin（kappa）/f - f*sin（kappa）*cos（omega）; A10 = （-f*cos（kappa） - （sdi.y-y0）*（sdi.x-x0）*sin（kappa） + （sdi.y-y0）*cos（kappa）/f）; A11 = （-（sdi.x-x0）; /该循环保存A矩阵，最后评定精度用 for （int l=0;l12;l+） Buf512*i+l = Al;
17、 /所谓的逐步法化，即要在循环内部就将ATA计算出来累加，下面的L矩阵类似 MatrixTranspose（A,AT,2,6）; MatrixMultiply（AT,A,Buf1,6,2,6）; MatrixCopy（ATA, Buf2, 36）; MatrixAdd（Buf1,Buf2,ATA,36）; / 为逐步法化后的ATA矩阵累加 L0 = （sdi.x - X0i）; L1 = （sdi.y - Y0i）; /保存L矩阵，最后评定精度用2; Buf62*i+l = Ll; MatrixMultiply（AT,L,Buf3,6,2,1）; MatrixCopy（ATL, Buf4, 6
18、）; MatrixAdd（Buf3,Buf4,ATL,6）; /for /“逐步法化”的另一处不同，出循环即可直接计算ATA逆乘ATL MatrixInversion（ATA,6）; MatrixMultiply（ATA,ATL,data,6,6,1）; /data即为改正数 Xs += data0; Ys += data1; Zs += data2; phi += data3; omega += data4; kappa += data5;改正数值： for （int i=0;= 6;data%d=%lf,i,datai）;六元素值:kappa=%lfn omega=%lfn phi=%lf
19、n,kappa,omega,phi）; /while EOEO eoeo; eoeo.kappa = kappa; eoeo.omega = omega; eoeo.phi = phi; eoeo.Xs = Xs; eoeo.Ys = Ys; eoeo.Zs = Zs;正常退出迭代n /精度评定 double Q6 = 0.0; for （int h=0;h6;h+） Qh = ATAh*6+h; MatrixMultiply（Buf5,data,V,8,6,1）;/V=Ax-L MatrixMinus（V,Buf6,V,8）; double m0（0）;/单位权中误差 double VSum
20、（0）;/vv，即平方和 for （i=0;i= 3） Mi = Mi*180*3600/PI; OutputResult（&eoeo, R, M, m0）;解算全部完成n/ 函数功能：输出解算结果eoeo：指向最终解算结果的结构体数组，RotationMatrix：旋转矩阵/ Precision：保存计算精度的数组，m0：单位权中误差void OutputResult（EOEO* eoeo, double* RotationMatrix, double* Precision, double m0）计算结果:六个外方位元素为：Xs = %.4fn, eoeo-Xs）;Ys = %.4fnYs）
21、;Zs = %.4fnZs）;phi = %.10fnphi）;omega = %.10fnomega）;kappa = %.10fnkappa）;旋转矩阵:%lf %lf %lfn %lf %lf %lfn %lf %lf %lfn,RotationMatrix0,RotationMatrix1,RotationMatrix2,RotationMatrix3,RotationMatrix4,RotationMatrix5,RotationMatrix6,RotationMatrix7,RotationMatrix8）;单位权中误差：六元素精度:Xs=%lfn,Precision0）;Ys=%lfn,Precision1）;Zs=%lfn,Precision2）;phi=%lfn,Precision3）;omega=%lfn,Precision4）;kap
