单片空间后方交会C#源代码

主方法：

private void Cal_Click(object sender, EventArgs e)

{

string[] lines = RichText.Text.Split('\n');

long m = lines.Length;

m = m - 1;//真实数据行数

double[] Coor_x = new double[m];//已知点x坐标

double[] Coor_y = new double[m];//已知点x坐标

double[] Coor_X = new double[m];//已知点X坐标

double[] Coor_Y = new double[m];//已知点Y坐标

double[] Coor_Z = new double[m];//已知点Z坐标

///赋值

for (int i = 0; i < m; i++)

{

string[] FJstring = Regex.Split(lines[i+1], ",");

Coor_x[i] = 0.001*(Convert.ToDouble(FJstring[0]));

Coor_y[i] = 0.001 *( Convert.ToDouble(FJstring[1]));

Coor_X[i] = Convert.ToDouble(FJstring[2]);

Coor_Y[i] = Convert.ToDouble(FJstring[3]);

Coor_Z[i] = Convert.ToDouble(FJstring[4]);

}

if (textBox_m.Text == "")

{

MessageBox.Show("请输入参数！");

}

if (textBox_m.Text != "")

{

double M = double.Parse(textBox_m.Text);//比例尺

double f = 0.001 * (double.Parse(textBox_f.Text));//焦距

double x0 = 0.001 * double.Parse(textBox_x0.Text);//内方位元素x0

double y0 = 0.001 * double.Parse(textBox_y0.Text);//内方位元素y0

double X0 = 0, Y0 = 0, Z0 = 0;//外方位坐标元素初始值

double min = (double.Parse(textBox_k.Text));//焦距

double angle1 = 0, angle2 = 0, angle3 = 0;//外方位角元素初始值

for (int i = 0; i < m; i++)

{//累加

X0 = Coor_X[i] + X0;

Y0 = Coor_Y[i] + Y0;

Z0 = Coor_Z[i] + Z0;

}

X0 = X0 / m;

Y0 = Y0 / m;

Z0 = Z0 / m + M * f;

double[,] A = new double[2 * m, 6];//A，二维：（2 * m）*6

double[,] l = new double[2 * m, 1];//l，二维：（2 * m）*1

double[,] V = new double[2 * m, 6];//V，二维：（2 * m）*6

double[,] XXX = new double[6, 1];//XXX，6*1 所求参数值，即外方位元素改正数

///循环解求误差方程，以限差为标准结束循环

Matrix ad = new Matrix();

Cls_AL ae = new Cls_AL();

int num = 0;//记录循环次数

for (int i = 0; ; i++)

{

//求A，l

ae.Cal_AL(X0, Y0, Z0, angle1, angle2, angle3, m, f, x0, y0, Coor_x, Coor_y, Coor_X, Coor_Y, Coor_Z, A, l);

//求XXX

XXX = ad.Cal_Chengfa(ad.Cal_Chengfa(ad.Cal_Qiuni(ad.Cal_Chengfa(ad.Cal_Zhuanzhi(A), A)), ad.Cal_Zhuanzhi(A)), l);

///改正外方位元素

X0 = X0 + XXX[0, 0];

Y0 = Y0 + XXX[1, 0];

Z0 = Z0 + XXX[2, 0];

angle1 = angle1 + XXX[3, 0];

angle2 = angle2 + XXX[4, 0];

angle3 = angle3 + XXX[5, 0];

num++;

if ((Math.Abs(XXX[3, 0]) < min) && (Math.Abs(XXX[4, 0]) < min) && (Math.Abs(XXX[5, 0]) < min))

{ break; }

}

///精度评定;

V = ad.Cal_Jianfa(ad.Cal_Chengfa(A, XXX), l);

double[,] c = new double[1, 1];//单位权中误差

c = ad.Cal_Chengfa(ad.Cal_Zhuanzhi(V), V);

double cc = Math.Sqrt(c[0, 0] / (2 * m - 6));

double[,] AA = new double[6, 6];//外方位元素改正数的协因数阵QQ AA = ad.Cal_Qiuni(ad.Cal_Chengfa(ad.Cal_Zhuanzhi(A), A));

RichText.Text = ("外方位元素为：" + '\n' + "Xs = " + Convert.ToString(X0) + '\n' + "Ys = " + Convert.ToString(Y0) + '\n' + "Zs = " + Convert.ToString(Z0) + '\n' + "angel_1 = " + Convert.ToString(angle1) + '\n' + "angel_2 = " + Convert.ToString(angle2) + '\n' + "angel_3 = " + Convert.ToString(angle3) + '\n' + "单位权中误差为： " + Convert.ToString(cc) + '\n' + "Xs精度为： " + Convert.ToString(cc * Math.Sqrt(AA[0, 0])) + '\n' + "Ys精度为：" + Convert.ToString(cc * Math.Sqrt(AA[1, 1])) + '\n' + "Zs精度为：" + Convert.ToString(cc * Math.Sqrt(AA[2, 2])) + '\n' + "迭代次数为：" + Convert.ToString(num));

}

}

动态类库：

using System;

using System.Collections.Generic;

using System.Linq;

using System.Text;

namespace dll

{

///

/// Matrix类包含三个方法：矩阵乘法，矩阵转置，矩阵求逆

///

public class Matrix

{

///

/// 矩阵乘法

///

///

///

///

public double[,] Cal_Chengfa(double[,] a, double[,] b)

{

//定义一个以a行数为行数，b列数为列数的数组

double[,] c = new double[a.GetLength(0), b.GetLength(1)];

for (int i = 0; i < c.GetLength(0); i++)

{

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

c[i, j] = 0;

for (int k = 0; k < a.GetLength(1); k++)

{

c[i, j] += a[i, k] * b[k, j]; //i行j列元素结果

}

}

}

return c;

}

///

/// 矩阵减法

///

///

///

///

public double[,] Cal_Jianfa(double[,] a, double[,] b)

{

//定义一个以a行数为行数，b列数为列数的数组

int m = a.GetLength(0);

int n = a.GetLength(1);

double[,] c = new double[m, m];

for (int i = 0; i < m; i++)

{

for (int j = 0; j < n; j++)

{

c[i, j] = a[i, j] - b[i, j];

}

}

return c;

}

///

/// 矩阵转置

///

///

///

public double[,] Cal_Zhuanzhi(double[,] a)

{

double[,] b = 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++)

b[i, j] = a[j, i];

}

}

return b;

}

///

/// 矩阵求逆

///

///

///

public double[,] Cal_Qiuni(double[,] a)

{

double[,] b = new double[a.GetLength(0), a.GetLength(1)]; int i, j, row, k;

double max, temp;

//单位矩阵

for (i = 0; i < a.GetLength(1); i++)

{

b[i, i] = 1;

}

for (k = 0; k < a.GetLength(1); k++)

{

max = 0; row = k;

//找最大元，其所在行为row

for (i = k; i < a.GetLength(1); i++)

{

temp = Math.Abs(a[i, k]);

if (max < temp)

{

max = temp;

row = i;

}

}

//交换k与row行

if (row != k)

{

for (j = 0; j < a.GetLength(0); j++)

{

temp = a[row, j];

a[row, j] = a[k, j];

a[k, j] = temp;

temp = b[row, j];

b[row, j] = b[k, j];

b[k, j] = temp;

}

}

//首元化为1

for (j = k + 1; j < a.GetLength(0); j++) a[k, j] /= a[k, k];

for (j = 0; j < a.GetLength(0); j++) b[k, j] /= a[k, k];

a[k, k] = 1;

//k列化为0

//对a

for (j = k + 1; j < a.GetLength(0); j++)

{

for (i = 0; i < k; i++) a[i, j] -= a[i, k] * a[k, j];

for (i = k + 1; i < a.GetLength(0); i++) a[i, j] -= a[i, k] * a[k, j];

}

//对b

for (j = 0; j < a.GetLength(0); j++)

{

for (i = 0; i < k; i++) b[i, j] -= a[i, k] * b[k, j];

for (i = k + 1; i < a.GetLength(0); i++) b[i, j] -= a[i, k] * b[k, j];

}

for (i = 0; i < a.GetLength(0); i++) a[i, k] = 0;

a[k, k] = 1;

}

return b;

}

}

public class Cls_AL

{

//计算A，L阵

public void Cal_AL(double X0, double Y0, double Z0, double angle1, double angle2, double angle3, long m, double f, double x0, double y0, double[] Coor_x, double[] Coor_y, double[] Coor_X, double[] Coor_Y, double[] Coor_Z, double[,] A, double[,] l)

{

//旋转矩阵

double a1 = Math.Cos(angle1) * Math.Cos(angle3) - Math.Sin(angle1) * Math.Sin(angle2) * Math.Sin(angle3);

double a2 = -Math.Cos(angle1) * Math.Sin(angle3) - Math.Sin(angle1) * Math.Sin(angle2) * Math.Cos(angle3);

double a3 = -Math.Sin(angle1) * Math.Cos(angle2);

double b1 = Math.Cos(angle2) * Math.Sin(angle3);

double b2 = Math.Cos(angle2) * Math.Cos(angle3);

double b3 = -Math.Sin(angle2);

double c1 = Math.Sin(angle1) * Math.Cos(angle3) + Math.Cos(angle1) * Math.Sin(angle2) * Math.Sin(angle3);

double c2 = -Math.Sin(angle1) * Math.Sin(angle3) + Math.Cos(angle1) * Math.Sin(angle2) * Math.Cos(angle3);

double c3 = Math.Cos(angle1) * Math.Cos(angle2);

///系数

double[] a11 = new double[m];

double[] a12 = new double[m];

double[] a13 = new double[m];

double[] a21 = new double[m];

double[] a22 = new double[m];

double[] a23 = new double[m];

double[] a14 = new double[m];

double[] a15 = new double[m];

double[] a16 = new double[m];

double[] a24 = new double[m];

double[] a25 = new double[m];

double[] a26 = new double[m];

for (int i = 0; i < m; i++)

{

double Z = ((a3 * (Coor_X[i] - X0)) + (b3 * (Coor_Y[i] - Y0)) + (c3 * (Coor_Z[i] - Z0)));//

a11[i] = ((a1 * f) + (a3 * Coor_x[i])) / Z;

a12[i] = ((b1 * f) + (b3 * Coor_x[i])) / Z;

a13[i] = ((c1 * f) + (c3 * Coor_x[i])) / Z;

a21[i] = ((a2 * f) + (a3 * Coor_y[i])) / Z;

a22[i] = ((b2 * f) + (b3 * Coor_y[i])) / Z;

a23[i] = ((c2 * f) + (c3 * Coor_y[i])) / Z;

a14[i] = Coor_y[i] * Math.Sin(angle2) - Math.Cos(angle2)*((Coor_x[i] / f) * (Coor_x[i] * Math.Cos(angle3) - Coor_y[i]*Math .Sin (angle3))+f*Math .Cos(angle3));

a15[i] = -f * Math.Sin(angle3) - Coor_x[i] * (Coor_x[i] * Math.Sin(angle3) + Coor_y[i] * Math.Cos(angle3)) / f;

a16[i] = Coor_y[i];

a24[i] = -Coor_x[i] * Math.Sin(angle2) - Coor_y[i] * Math.Cos(angle2) * (Coor_x[i] * Math.Cos(angle3) - Coor_y[i] * Math.Sin(angle3) - f * Math.Sin(angle3)) / f;

a25[i] = -f * Math.Cos(angle3) - Coor_y[i] * (Coor_x[i] * Math.Sin(angle3) + Coor_y[i] * Math.Cos(angle3)) / f;

a26[i] = -Coor_x[i];

A[2 * i, 0] = a11[i];

A[2 * i, 1] = a12[i];

A[2 * i, 2] = a13[i];

A[2 * i, 3] = a14[i];

A[2 * i, 4] = a15[i];

A[2 * i, 5] = a16[i];

A[2 * i + 1, 0] = a21[i];

A[2 * i + 1, 1] = a22[i];

A[2 * i + 1, 2] = a23[i];

A[2 * i + 1, 3] = a24[i];

A[2 * i + 1, 4] = a25[i];

A[2 * i + 1, 5] = a26[i];

l[2 * i, 0] = -x0+Coor_x[i] + f * ((a1 * (Coor_X[i] - X0)) + (b1 * (Coor_Y[i] - Y0)) + (c1 * (Coor_Z[i] - Z0))) / Z;

l[2 * i + 1, 0] = -y0+Coor_y[i] + f * ((a2 * (Coor_X[i] - X0)) + (b2 * (Coor_Y[i] - Y0)) + (c2 * (Coor_Z[i] - Z0))) / ((a3 * (Coor_X[i] - X0)) + (b3 * (Coor_Y[i] - Y0)) + (c3 * (Coor_Z[i] - Z0)));

}

}

}

}

测试数据：

x,y,z,X,Y,Z

-86.15,-68.99,36589.41,25273.32,2195.17

-53.40,82.21,37631.08,31324.51,728.69

-14.78,-76.63,39100.97,24934.98,2386.50

10.46,64.43,40426.54,30319.81,757.31