摄影测量后方交会求外方位元素
09地信一班 肖明梅
解题思路：定义要用到的变量并初始化，定义一个函数用于求解旋转矩阵R，系数矩阵A,近似坐标矩阵JSZB,常数矩阵L;矩阵的转置，逆，矩阵相乘，相减，求外方位元素的近似值初值以及结果输出都定义为相应的函数。最后定义一个用于循环求解的函数（程序中xhqiujie()）,在该函数中调用之前定义的函数，求出外方位元素近似值初值，改正数Dv[6,1],误差V[8,1]，用do…while语句进行循环，使精度达到20μm,限差低于20μm，然后调用结果输出函数用于输出达到要求的结果。在主函数中创建对象的实例，引用该实例的方法即xhqiujie()函数，就可以求出外方位元素。
代码：
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace 摄影测量后方交会求外方位元素
{
class Program
{
double ψ, ω, κ, Xs, Ys, Zs, m, f, Sx = 0, Sy = 0;
double n = 206265 / 3600;//*角元素的单位从m到度的转换系数*//
double[,] zuobiao = {{ -86.15, -53.40, -14.78, 10.46 }, {-68.99, 82.21,-76.63,64.43},{36589.41,37631.08,39100.97,40426.54},{ 25273.32,31324.51,24934.98,30319.81 },{ 2195.17,728.69,2386.50,757.31 }};
double[] a = new double[3];//*存储a1,a2.a3*//
double[] b = new double[3];//*存储b1,b2,b3*//
double[] c = new double[3];//*存储c1,c2,c3*//
double[] XX = new double[4];
double[] YY = new double[4];
double[] ZZ = new double[4];
double[,] L = new double[8, 1];
double[,] JSZB = new double[2, 4];//*像点近似坐标*//
double[,] A = new double[8, 6];//*系数阵*//
double[,] AT = new double[6, 8];
double[,] AB = new double[6, 6];
double[,] AN = new double[6, 6];
double[,] AL = new double[6, 1];
double[,] Dv = new double[6, 1];
double[,] AX = new double[8, 1];//*系数阵与改正数矩阵的乘积*//
double[,] V = new double[8, 1];//*中误差矩阵*//
private void Qchuzhi()
{
for (int i = 0; i < 4; i++)
{
Sx += zuobiao[2, i];
}
for (int i = 0; i < 4; i++)
{
Sy += zuobiao[3, i];
}
for (int i = 0; i < 4; i++)
{
zuobiao[0, i] = zuobiao[0, i] / 1000;
zuobiao[1, i] = zuobiao[1, i] / 1000;
}
//*求外方位元素初始值*//
ψ = ω = κ = 0;
m = 50000;
f = 0.15324;
Xs = Sx / 4;
Ys = Sy / 4;
Zs = m * f;
}
//*求旋转矩阵R,像点坐标近似值，常数项矩阵L,系数矩阵A*//
private void R()
{
//旋转矩阵R
a[0] = Math.Cos(ψ) * Math.Cos(κ) - Math.Sin(ψ) * Math.Sin(ω) * Math.Sin(κ);
a[1] = -Math.Cos(ψ) * Math.Sin(κ) - Math.Sin(ψ) * Math.Sin(ω) * Math.Cos(κ);
a[2] = -Math.Sin(ψ) * Math.Cos(ω);
b[0] = Math.Cos(ω) * Math.Sin(κ);
b[1] = Math.Cos(ω) * Math.Cos(κ);
b[2] = -Math.Sin(ω);
c[0] = Math.Sin(ψ) * Math.Cos(κ) + Math.Cos(ψ) * Math.Sin(ω) * Math.Sin(κ);
c[1] = -Math.Sin(ψ) * Math.Sin(κ) + Math.Cos(ψ) * Math.Sin(ω) * Math.Cos(κ);
c[2] = Math.Cos(ψ) * Math.Cos(ω);
for (int i = 0; i < 4; i++)
{
XX[i] = a[0] * (zuobiao[2, i] - Xs) + b[0] * (zuobiao[3, i] - Ys) + c[0] * (zuobiao[4, i] - Zs);
YY[i] = a[1] * (zuobiao[2, i] - Xs) + b[1] * (zuobiao[3, i] - Ys) + c[1] * (zuobiao[4, i] - Zs);
ZZ[i] = a[2] * (zuobiao[2, i] - Xs) + b[2] * (zuobiao[3, i] - Ys) + c[2] * (zuobiao[4, i] - Zs);
//由共线条件方程式求得xo,yo的近似值
JSZB[0, i] = -f * (XX[i]) / (ZZ[i]);
JSZB[1, i] = -f * (YY[i]) / (ZZ[i]);
//常数项矩阵
L[i * 2, 0] = zuobiao[0, i] - JSZB[0, i];
L[i * 2 + 1, 0] = zuobiao[1, i] - JSZB[1, i];
}
for (int i = 0; i < 4; i++)
{
A[2 * i, 0] = (a[0] * f + a[2] * zuobiao[0, i]) / ZZ[i];
A[2 * i, 1] = (b[0] * f + b[2] * zuobiao[0, i]) / ZZ[i];
A[2 * i, 2] = (c[0] * f + c[2] * zuobiao[0, i]) / ZZ[i];
A[2 * i, 3] = zuobiao[1, i] * Math.Sin(ω) - ((zuobiao[0, i] * (zuobiao[0, i] * Math.Cos(κ) - zuobiao[1, i] * Math.Sin(κ)) / f + f * Math.Cos(κ))) * Math.Cos(ω);
A[2 * i, 4] = -f * Math.Sin(κ) - zuobiao[0, i] * (zuobiao[0, i] * Math.Sin(κ) + zuobiao[1, i] * Math.Cos(κ)) / f;
A[2 * i, 5] = zuobiao[1, i];
A[2 * i + 1, 0] = (a[1] * f + a[2] * zuobiao[1, i]) / ZZ[i];
A[2 * i + 1, 1] = (b[1] * f + b[2] * zuobiao[1, i]) / ZZ[i];
A[2 * i + 1, 2] = (c[1] * f + c[2] * zuobiao[1, i]) / ZZ[i];
A[2 * i + 1, 3] = -zuobiao[0, i] * Math.Sin(ω) - (zuobiao[1, i] * (zuobiao[0, i] * Math.Cos(κ) - zuobiao[1, i] * Math.Sin(κ)) / f - f * Math.Sin(κ)) * Math.Cos(ω);
A[2 * i + 1, 4] = -f * Math.Cos(κ) - zuobiao[1, i] * (zuobiao[0, i] * Math.Sin(κ) + zuobiao[1, i] * Math.Cos(κ)) / f;
A[2 * i + 1, 5] = -zuobiao[0, i];
}
}
//*定义一个函数用来求矩阵的转置*//
private void T(double[,] A, double[,] B, int a, int b)
{
for (int i = 0; i < b; i++)
{
for (int j = 0; j < a; j++)
{
B[i, j] = A[j, i];
}
}
}
//*定义一个函数用来求矩阵的乘积*//
private void Multiply(double[,] A, double[,] B, double[,] C, int a, int b, int c)
{
for (int i = 0; i < a; i++)
{
for (int j = 0; j < c; j++)
{
C[i, j] = 0;
for (int m = 0; m < b; m++)
{
C[i, j] = A[i, m] * B[m, j] + C[i, j];
}
}
}
}
private void substract(double[,] A, double[,] B, double[,] C, int a, int b)
{
for(int i=0;i for (int j = 0; j < b; j++) { C[i, j] = A[i, j] - B[i, j]; } } } //*定义一个函数用来求矩阵的逆*// //矩阵的求逆函数 public double[,] ReverseMatrix(double[,] dMatrix, int Level) { double dMatrixValue = MatrixValue(dMatrix, Level); if (dMatrixValue == 0) return null; double[,] dReverseMatrix = new double[Level, 2 * Level]; double x, c; for (int i = 0; i < Level; i++) { for (int j = 0; j < 2 * Level; j++) { if (j < Level) dReverseMatrix[i, j] = dMatrix[i, j]; else dReverseMatrix[i, j] = 0; } dReverseMatrix[i, Level + i] = 1; } for (int i = 0, j = 0; i < Level && j < Level; i++, j++) { if (dReverseMatrix[i, j] == 0) { int m = i; for (; dMatrix[m, j] == 0; m++) ; if (m == Level - 1) return null; else { // Add i-row with m-row for (int n = j; n < 2 * Level; n++) dReverseMatrix[i, n] += dReverseMatrix[m, n]; } } // Format the i-row with \"1\" start x = dReverseMatrix[i, j]; if (x != 1) { for (int n = j; n < 2 * Level; n++) if (dReverseMatrix[i, n] != 0) dReverseMatrix[i, n] /= x; } // Set 0 to the current column in the rows after current row for (int s = Level - 1; s > i; s--) { x = dReverseMatrix[s, j]; for (int t = j; t < 2 * Level; t++) dReverseMatrix[s, t] -= (dReverseMatrix[i, t] * x); } } // Format the first matrix into unit-matrix for (int i = Level - 2; i >= 0; i--) { for (int j = i + 1; j < Level; j++) if (dReverseMatrix[i, j] != 0) { c = dReverseMatrix[i, j]; for (int n = j; n < 2 * Level; n++) dReverseMatrix[i, n] -= (c * dReverseMatrix[j, n]); } } double[,] dReturn = new double[Level, Level]; for (int i = 0; i < Level; i++) for (int j = 0; j < Level; j++) dReturn[i, j] = dReverseMatrix[i, j + Level]; return dReturn; } //求得矩阵行列式的值 public double MatrixValue(double[,] MatrixList, int Level) { double[,] dMatrix = new double[Level, Level]; for (int i = 0; i < Level; i++) for (int j = 0; j < Level; j++) dMatrix[i, j] = MatrixList[i, j]; double c, x; int k = 1; for (int i = 0, j = 0; i < Level && j < Level; i++, j++) { if (dMatrix[i, j] == 0) { int m = i; for (; dMatrix[m - 2, j] == 0; m++) ; if (m == Level) return 0; else { // Row change between i-row and m-row for (int n = j; n < Level; n++) { c = dMatrix[i, n]; dMatrix[i, n] = dMatrix[m, n]; dMatrix[m, n] = c; } // Change value pre-value k *= (-1); } } // Set 0 to the current column in the rows after current row for (int s = Level - 1; s > i; s--) { x = dMatrix[s, j]; for (int t = j; t < Level; t++) dMatrix[s, t] -= dMatrix[i, t] * (x / dMatrix[i, j]); } } double sn = 1; for (int i = 0; i < Level; i++) { if (dMatrix[i, i] != 0) sn *= dMatrix[i, i]; else return 0; } return k * sn; } private void output() { Console.WriteLine(\"外方位元素Xs：\{0}m\ Console.WriteLine(\"外方位元素Ys：\{0}m\ Console.WriteLine(\"外方位元素Zs：\{0}m\ Console.WriteLine(\"外方位元素ψ：\{0}度\ Console.WriteLine(\"外方位元素ω：\{0}度\); Console.WriteLine(\"外方位元素κ：\{0}度\ for (int i = 0; i < 8; i++) { Console.WriteLine(\"中误差：\{0}m\ } } //*求解过程*// private void xhqiujie() { Qchuzhi(); do { 的逆矩阵AN Dv中*// R(); T(A, AT, 8, 6);//*求A的转置*// Multiply(AT, A, AB, 6, 8, 6);//*A的转置与A的乘积*// AN = ReverseMatrix(AB, 6);//调用ReverseMatrix方法,求得矩阵AB Multiply(AT, L, AL, 6, 8, 1);//*A与L的乘积*// Multiply(AN, AL, Dv, 6, 6, 1);//*求出外方位元素的改正数存储到矩阵 Xs += Dv[0,0]; Ys += Dv[1,0]; Zs += Dv[2,0]; ψ += Dv[3,0]; ω += Dv[4,0]; κ += Dv[5,0]; //*精度评定*// Multiply(A, Dv, AX, 8, 6, 1); substract(AX, L, V, 8, 1); } while (Math.Abs(Dv[0, 0]) >= 0.00002 || Math.Abs(Dv[1, 0]) >= 0.00002 || Math.Abs(Dv[2, 0]) >= 0.00002 || Math.Abs(Dv[3, 0]) >= 0.00002 || Math.Abs(Dv[4, 0]) >= 0.00002 || Math.Abs(Dv[5, 0]) >= 0.00002 || Math.Abs(V[0, 0]) >= 0.00002 || Math.Abs(V[1, 0]) >= 0.00002 || Math.Abs(V[2, 0]) >= 0.00002 || Math.Abs(V[3, 0]) >= 0.00002 || Math.Abs(V[4, 0]) >= 0.00002 || Math.Abs(V[5, 0]) >= 0.00002 || Math.Abs(V[6, 0]) >= 0.00002 || Math.Abs(V[7, 0]) >= 0.00002); output(); } static void Main(string[] args) { Program HS = new Program(); HS.xhqiujie(); } } } 结果如下： 因篇幅问题不能全部显示，请点此查看更多更全内容