摄影测量学单像空间后方交会程序设计作业

{

System;

System.Collections.Generic;

System.Linq;

System.Text;

namespace 单像空间后方交会

{

class Program

{

static void Main( string [] args)

for (j = 0; j < 5; j++)

if (j < 3)

"请输入第 {0} 个点的第 {1} 个地面坐标： ", i + 1, j

+ 1); double .Parse( Console .ReadLine());

"请输入第 {0} 个点的第 {1} 个像点

坐标： ", i + 1, j - 2);

double .Parse( Console .ReadLine());

Console .WriteLine();

// 归算像点坐标

(i = 0; i < 4; i++)

for (j = 3; j < 5; j++)

if (j == 3)

zuobiao[i, j] = zuobiao[i, j] - x0;

else zuobiao[i, j] = zuobiao[i, j] - y0;

// 计算和确定初值

double zs0 = m * f, xs0 = 0, ys0 = 0; for (i = 0; i < 4; i++)

else

using using using using x0 = y0 = int x0, y0, i, j; double f, m;

Console .Write( " 请输入像片比例尺： ");

double .Parse( Console .ReadLine());

Console .Write( " 请输入像片的内方位元素 x0:" ); // 均以毫米为单

位 int .Parse( Console .ReadLine());

Console .Write( " 请输入像片的内方位元素 y0:" );

int .Parse( Console .ReadLine());

Console .Write( " 请输入摄影机主距 f:" );

double .Parse( Console .ReadLine());

Console .WriteLine();

// 输入坐标数据 double [,] zuobiao = new double [4, 5];

(i = 0; i < 4; i++)

for

Console .Write( zuobiao[i, j] = Console .Write( zuobiao[i, j] = for

xs0 = xs0 + zuobiao[i, 0];

ys0 = ys0 + zuobiao[i, 1];

}

xs0 = xs0 / 4;

ys0 = ys0 / 4;

// 逐点计算误差方程系数 double [,] xishu = new double [8, 6];

for (i = 0; i < 8; i += 2)

double x, y;

x = zuobiao[i / 2, 3]; y = zuobiao[i / 2, 4];

xishu[i, 0] = xishu[i + 1, 1] = -1 / m; xishu[i, 1] = xishu[i + 1, 0] = 0;

xishu[i, 2] = -x / (m * f); xishu[i, 3] = -f * (1 + x * x / (f * f));

xishu[i, 4] = xishu[i + 1, 3] = -x * y / f; xishu[i, 5] = y; xishu[i + 1,

2] = -y / (m * f); xishu[i + 1, 4] = -f * (1 + y * y / (f * f)); xishu[i + 1, 5] = -x;

}

// 计算逆阵

double [,] dMatrix =matrixChe(matrixTrans(xishu), xishu);

double [,] dReturn = ReverseMatrix(dMatrix, 6);

Console .WriteLine( " 逆矩阵为： ");

if (dReturn != null )

{

matrixOut(dReturn);

// 求解过程 double phi0 = 0, omega0 = 0, kappa0 = 0;

double [,] r = double [,] jinsi = double [] chazhi = double [] jieguo =

{

jinsi[i, 0] = -f * (r[0, 0] * (zuobiao[i, 0] - xs0) + r[1, 0] *

(zuobiao[i, 1]

- ys0) + r[2, 0] * (zuobiao[i, 2] - zs0)) / (r[0, 2] * (zuobiao[i, 0] - xs0) + r[1, 2] * (zuobiao[i, 1] - ys0) + r[2, 2] * (zuobiao[i, 2] - zs0));

jinsi[i, 1] = -f * (r[0, 1] * (zuobiao[i, 0] - xs0) + r[1, 1] *

(zuobiao[i, 1]

- ys0) + r[2, 1] * (zuobiao[i, 2] - zs0)) / (r[0, 2] * (zuobiao[i, 0] - xs0) + r[1, 2] * (zuobiao[i, 1] - ys0) + r[2, 2] * (zuobiao[i, 2] - zs0));

int q = 0;

new double [3, 3]; new double [4, 2];

new double [8];

new double [6];

r[0, 0] = Math.Sin(kappa0);

r[0, 1] = -

Math.Cos(kappa0); r[0, 2] = - r[1, 0] = r[1, 1] = r[1, 2] = - r[2,

0] = Math.Sin(kappa0);

r[2, 1] = - double [,] zhong = matrixChe(dReturn,

matrixTrans(xishu)); do

// 计算旋转矩阵 r Math.Cos(phi0) *

Math.Cos(kappa0) - Math.Sin(phi0) *

Math.Sin(omega0) * Math.Cos(phi0) * Math.Sin(kappa0) - Math.Sin(phi0) * Math.Sin(omega0) * Math.Sin(phi0) * Math.Cos(omega0) * Math.Cos(omega0) * Math.Sin(omega0);

Math.Sin(phi0) *

Math.Cos(omega0);

Math.Sin(kappa0);

Math.Cos(kappa0);

Math.Cos(kappa0) + Math.Cos(phi0) * Math.Sin(omega0) * Math.Sin(phi0) * Math.Sin(kappa0) + Math.Cos(phi0) *

Math.Sin(omega0) *

Math.Cos(phi0) * //计算x,y 的近似值

for (i = 0; i < 4; i++)

Math.Cos(omega0);

}

for ( int j = 0; j < C.GetLength(1); j++)

{

}

for (i = 0; i < 8; i += 2) chazhi[i] = zuobiao[i / 2, 3] - jinsi[i / 2, 0]; chazhi[i + 1] = zuobiao[i / 2, 4] - jinsi[i / 2, 1]; for (i = 0; i < zhong.GetLength(0); i++) double k = 0; for (j = 0; j < zhong.GetLength(1); j++) k = k + zhong[i, j] * chazhi[j]; } jieguo[i] = k;

// 求新的近似值 } xs0 += jieguo[0]; ys0 += jieguo[1]; zs0 += jieguo[2]; phi0 += jieguo[3]; omega0 += jieguo[4]; kappa0 += jieguo[5]; q++;

if (q > 1000) break ; } while (( Math.Abs(jieguo[0]) > 0.020 || Math.Abs(jieguo[2]) > 0.020); Math.Abs(jieguo[1]) > 0.020) || Console .WriteLine( "共进行了{0} 次运算", q); Console .WriteLine( " 旋转矩阵为 "); matrixOut(r);

for (i = 0; i < jieguo.GetLength(0); i++) Console .Write( "第{0} 个外方位元素为： {1}" , i + 1,

jieguo[i]); // 矩阵转置 public static double [,] matrixTrans( double [,] X) double [,] A = X; double [,] C = new double [A.GetLength(1), A.GetLength(0)]; for ( int i = 0; i < A.GetLength(1); i++) for ( int j = 0; j < A.GetLength(0); j++) C[i, j] = A[j, i]; return C; 矩阵输出 // public static void matrixOut( double [,] X) double [,] C = X; ( int i = 0; i < C.GetLength(0);

i++)

for Console .Write( " {0}" , C[i, j]);

for ( int s = Level - 1; s > i; s--)

{

Console .Write( "\n" );

// 二维矩阵相乘

public static double [,] matrixChe( double [,] X, double [,] Y) int i, j, n; double m; double [,] C = X; double [,] D = Y; double [,] E = new double [C.GetLength(0), C.GetLength(0)]; for (i = 0; i < C.GetLength(0); i++)

for (n = 0; n < C.GetLength(0); n++)

m = 0;

for (j = 0; j < C.GetLength(1);

j++)

{

m = m + C[i, j] * D[j,

n];

}

E[i, n] = m;

return E;

计算行列式的

值

// public static double MatrixValue( double [,] MatrixList, 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;

k = 1;

( int i = 0, j = 0; i < Level && j < Level;

i++, j++)

int for if (dMatrix[i, j] ==

0)

int m = i;

for (; dMatrix[m, j] == 0;

m++) ; if (m == Level)

return 0;

else

for ( int n = j; n < Level;

n++)

{

c = dMatrix[i, n];

dMatrix[i, n] = dMatrix[m,

n]; dMatrix[m, n] = c;

}

k *= (-1);

x = dMatrix[s, j];

for ( int t = j; t < Level;

t++)

int Level)

} for ( int s = Level - 1; s > i; s--)

{

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;

计算逆阵

public static 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) return null ; else for ( int n = j; n < 2 * Level; n++) dReverseMatrix[i, n] += dReverseMatrix[m, n]; } } x = dReverseMatrix[i, j]; if (x != 1) for ( int n = j; n < 2 * Level; n++) if (dReverseMatrix[i, n] != 0) dReverseMatrix[i, n] /= x; x = dReverseMatrix[s,

j];

} }

for ( int t = j; t < 2 * Level; t++) dReverseMatrix[s, t] -= (dReverseMatrix[i, t] * x);

( 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;

for