摄影测量后方交会法求解外方位元素.doc

摄影测量后方交会求外方位元素
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<a;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：\t{0}m", Xs);
Console.WriteLine("外方位元素Ys：\t{0}m", Ys);
Console.WriteLine("外方位元素Zs：\t{0}m", Zs);
Console.WriteLine("外方位元素ψ：\t{0}度", ψ*n);
Console.WriteLine("外方位元素ω：\t{0}度", ω*n);
Console.WriteLine("外方位元素κ：\t{0}度", κ*n);
for (int i = 0; i < 8; i++)
{
Console.WriteLine("中误差：\t{0}m", V[i, 0]);
}
}
//*求解过程*//
private void xhqiujie()
{
Qchuzhi();
do
{
R();
T(A, AT, 8, 6);//*求A的转置*//
Multiply(AT, A, AB, 6, 8, 6);//*A的转置与A的乘积*//
AN = ReverseMatrix(AB, 6);//调用ReverseMatrix方法,求得矩阵AB的逆矩阵AN
Multiply(AT, L, AL, 6, 8, 1);//*A与L的乘积*//
Multiply(AN, AL, Dv, 6, 6, 1);//*求出外方位元素的改正数存储到矩阵Dv中*//
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();
}
}
}
结果如下：