C#单张像片后方交会(有点错误).doc

using System;using System.Collections.Generic;using System.Linq;using System.Text;namespace spaceresection class SpaceResection double SR_R = new double9;/ 旋转矩阵 double SR_A;/误差方程系数 double SR_AA=new double36;/ATPA double SR_DX = new double6;/参数增量 double SR_X0 = new double6;/参数初值 double SR_X = new double6;/ 参数变量 double SR_L; double SR_AL = new double6; /原始数据 double R_X; double R_Y; double R_Z; double R_x; double R_y; double f;/主距 double dd;/限差 int Repeat;/迭代次数 int count;/迭代计数器 int result;/函数返回值，临时变量 double m;/摄影比例尺 public double SR_XS/外方位元素 get return SR_X; set value = SR_X; public double SR_M/比例尺 get return m; set value = m; public double SR_N/实际迭代次数 get return count; set value = count; public double SR_DXS/外方位元素内部精度 get return SR_DX; set value = SR_DX; /初始化各元素，计算外方位元素及精度。参数依次为物点地面坐标，像点框标坐标，摄影机主距，迭代上限，迭代精度，均以米为单位 public int SR(double R_X0, double R_Y0, double R_Z0, double R_x0, double R_y0, double f0, int n, double dd) try this.R_X = new doubleR_X0.Length; this.R_X = R_X0; this.R_Y = new doubleR_Y0.Length; this.R_Y = R_Y0; this.R_Z = new doubleR_Z0.Length; this.R_Z = R_Z0; this.R_x = new doubleR_x0.Length; this.R_x = R_x0; this.R_y = new doubleR_y0.Length; this.R_y = R_y0; this.f = f0; this.Repeat = n; this.dd = dd; SR_A = new double12 * R_X0.Length; SR_L = new double2 * R_X0.Length; Get_X0(); Get_R(); Get_A(); Get_AA(); Get_AL(); Get_DX(); Get_X(); count = 1; result = 0; while (Compare_DX() Get_R(); Get_A(); Get_AA(); Get_AL(); Get_DX(); Get_X(); count+; if (count Repeat) result = 1; break; return result; catch return 2; /计算初值和摄影比例尺 void Get_X0() int k=0; double tm = new doubleR_X.Length*R_X.Length; for (int i = 1; i R_X.Length; i+) for (int j = 0; j i; j+) tmk = Math.Pow(R_xi - R_xj,2) + Math.Pow(R_yi - R_yj,2); tmk = (Math.Pow(R_Xi - R_Xj, 2) + Math.Pow(R_Yi - R_Yj, 2) / tmk; tmk = Math.Sqrt(tmk); k+; m = tm.Sum() / (k - 1);/算术平均解算摄影比例尺 SR_X00 = R_X.Sum() / R_X.Length; SR_X01 = R_Y.Sum() / R_Y.Length; SR_X02 = R_Z.Sum() / R_Z.Length+m*f; SR_X03 = 0; SR_X04 = 0; SR_X05 = 0; /计算旋转矩阵 void Get_R() double s1 = Math.Sin(SR_X03); double s2 = Math.Sin(SR_X04); double s3 = Math.Sin(SR_X05); double c1 = Math.Cos(SR_X03); double c2 = Math.Cos(SR_X04); double c3 = Math.Cos(SR_X05); SR_R0 = c1 * c3 - s1 * s2 * s3; SR_R1 = -c1 * s3 - s1 * s2 * c3; SR_R2 = -s1 * c2; SR_R3 = c2 * s3; SR_R4 = c2 * c3; SR_R5 = -s2; SR_R6 = s1 * c3 + c1 * s2 * s3; SR_R7 = -s1 * s3 + c1 * s2 * c3; SR_R8 = c1 * c2; /计算误差方程矩阵 void Get_A() double A; double B; double C; double da; double db; double dc; double t;/中间变量。 for(int i=0;iR_X.Length;i+) double s1 = Math.Sin(SR_X03); double s2 = Math.Sin(SR_X04); double s3 = Math.Sin(SR_X05); double c1 = Math.Cos(SR_X03); double c2 = Math.Cos(SR_X04); double c3 = Math.Cos(SR_X05); da = R_Xi - SR_X00; db = R_Yi - SR_X01; dc = R_Zi - SR_X02; A = SR_R0 * da + SR_R3 * db + SR_R6 * dc; B = SR_R1 * da + SR_R4 * db + SR_R7 * dc; C = SR_R2 * da + SR_R5 * db + SR_R8 * dc; SR_A12 * i = (SR_R0 * f + SR_R2*R_xi) / C; SR_A12 * i + 1 = (SR_R3 * f + SR_R5 * R_xi) / C; SR_A12 * i + 2 = (SR_R6 * f + SR_R8 * R_xi) / C; t = R_xi * c3 - R_yi * s3; t = R_xi * t / f + f * c3; SR_A12 * i + 3 = R_yi * s2 - t * c2; t = R_xi * s3 + R_yi * c3; SR_A12 * i + 4 = -f * s3 - R_xi * t / f; SR_A12 * i + 5 = R_yi; SR_A12 * i + 6 = (SR_R1 * f + SR_R2 * R_yi) / C; SR_A12 * i + 7 = (SR_R4 * f + SR_R5 * R_yi) / C; SR_A12 * i + 8 = (SR_R7 * f + SR_R8 * R_yi) / C; t = R_xi * c3 - R_yi * s3; t = R_yi * t / f - f * s3; SR_A12 * i + 9 = -R_xi * s2 - t * c2; t = R_xi * s3 + R_yi * c3; SR_A12 * i + 10 = -f * c3 - R_yi * t / f; SR_A12 * i + 11 = -R_xi; SR_L2 * i = R_xi +f * A / C; SR_L2 * i+1 = R_yi + f * B / C; /计算ATPA矩阵 void Get_AA() for (int i = 0; i 6; i+) for (int j = 0; j 6; j+) Get_AAe(i,j); /计算各元素 void Get_AAe(int i,int j) SR_AA6*i+j=0; for (int m = 0; m (R_X.Length* 2); m+) SR_AA6 * i + j += SR_A6 * m + i * SR_A6 * m + j; void Get_AL() for (int i = 0; i 6; i+) SR_ALi = 0; for (int j = 0; j R_X.Length* 2; j+) SR_ALi += SR_Lj * SR_A6 * j + i; /高斯列主元消去法计算方程组 void Get_DX() double maxmain; double a = new double6; int b=new int6; int i; int j; int k; int temp; for (i = 0; i 6; i+) maxmain = 0; bi = -1; for (j = 0; j =maxmain) temp = 0; for (k = 0; k i;k+ ) if (j = bk) temp = 1; break; if (temp = 0) maxmain = aj; bi = j; for (j = 0; j 6; j+) if (j = bi) aj = 1 / SR_AA6 * bi + i; else aj = SR_AA6*j+i / SR_AA6 * bi + i; for (j = 0; j 6; j+) if (j = bi) continue; for (k = 0; k 6; k+) if (k i) continue; SR_AA6 * j + k -= SR_AA6 * bi + k*aj; SR_ALj -= SR_ALbi*aj; for (k = 0; k 6; k+) if (k i) continue; SR_AA6 * bi + k *= abi; SR_ALbi *= abi; for (i = 0; i 6; i+) SR_DXi = SR_ALbi; /计算外方位元素