摄影测量后方交会-前方交会(C#)

双像解析摄影测量中的后交-前交解法，常用于已知像片的外方位元素、需确定少量待定点坐标的情况。解算过程分为两个阶段：后方交会、前方交会。

思路为利用后方交会计算外方位元素，利用前方交会解算待定地面点坐标。

后方交会的解算过程为：

（1）获取已知数据：比例尺分母m，内方位元素x0,y0,f，以及控制点的地面摄影测量坐标X、Y、Z。

（2）量测控制点的像点坐标：得到像点坐标x、y。

（3）确定未知数的初始值：在竖直摄影情况下，外方位角元素的初始值为0，即phi0=omiga0=kappa0=0;

线元素中，Zs0=H=mf，Xs0、Ys0的取值用四个控制点坐标的平均值，即Xs0=1/4*(X1+X2+X3+X4);Ys0=1/4 *（Y1+Y2+Y3+Y4）.

（4）计算旋转矩阵R：利用角元素的近似值计算方向余弦值，组成R阵。

（5）逐点计算像点坐标的近似值：利用未知数的近似值按共线方程计算控制点像点坐标的近似值（x），（y）。

（6）组成误差方程式：逐点计算误差方程式的系数和常数项。

（7）组成法方程式：计算法方程式的系数矩阵ATA与常数项ATL。

（8）解求外方位元素：根据法方程，解求外方位元素改正数，并与相应的近似值求和，得到外方位元素新的近似值。

（9）检查计算是否收敛：将求得的外方位元素的改正数与规定的限差比较，小于限差则计算终止，否则用新的近似值重复第4至第8步骤的计算，直到满足要求为止。

前方交会的解算过程：

（1）按照后方交会过程，计算出左右双片的外方位元素，用各片的外方位角元素计算左右片的方向余弦值，组成旋转矩阵R1和R2。

（2）逐点计算像点的像空间辅助坐标（u1，v1，w1）及（u2，v2，w2）。

（3）根据外方位线元素计算基线分量（Bu，Bv，Bw）。

（4）计算投影系数（N1，N2）。

（5）计算待定点在各自的像空间辅助坐标中的坐标（U1，V1，W1）及（U2，V2，W2）。

（6）最后计算待定点的地面摄影测量坐标（X，Y，Z）。

以下为C#实现代码：

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

namespace Resection
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
}
public TextBox[] mytextbox = new TextBox[41];

//矩阵打包成类，矩阵为m * n,直接调用
public class Matrix
{
double[,] A;
int m, n;
string name;
public Matrix(int am, int an)
{
m = am;
n = an;
A = new double[m, n];
name = "Result";
}
public Matrix(int am, int an, string aName)
{
m = am;
n = an;
A = new double[m, n];
name = aName;
}

public int getM
{
get { return m; }
}
public int getN
{
get { return n; }
}
public double[,] Detail
{
get { return A; }
set { A = value; }
}
public string Name
{
get { return name; }
set { name = value; }
}
}

/***********矩阵通用操作打包*************/

class MatrixOperator
{
//矩阵加法
public static Matrix MatrixAdd(Matrix Ma, Matrix Mb)
{
int m = Ma.getM;
int n = Ma.getN;
int m2 = Mb.getM;
int n2 = Mb.getN;

if ((m != m2) || (n != n2))
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}

Matrix Mc = new Matrix(m, n);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
double[,] b = Mb.Detail;

for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++)
c[i, j] = a[i, j] + b[i, j];
return Mc;
}

//矩阵减法
public static Matrix MatrixSub(Matrix Ma, Matrix Mb)
{
int m = Ma.getM;
int n = Ma.getN;
int m2 = Mb.getM;
int n2 = Mb.getN;
if ((m != m2) || (n != n2))
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
Matrix Mc = new Matrix(m, n);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
double[,] b = Mb.Detail;

for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++)
c[i, j] = a[i, j] - b[i, j];
return Mc;
}

//矩阵乘法
public static Matrix MatrixMulti(Matrix Ma, Matrix Mb)
{
int m = Ma.getM;
int n = Ma.getN;
int m2 = Mb.getM;
int n2 = Mb.getN;

if (n != m2)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}

Matrix Mc = new Matrix(m, n2);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
double[,] b = Mb.Detail;

for (int i = 0; i < m; i++)
for (int j = 0; j < n2; j++)
{
c[i, j] = 0;
for (int k = 0; k < n; k++)
c[i, j] += a[i, k] * b[k, j];
}
return Mc;

}

//矩阵数乘
public static Matrix MatrixSimpleMulti(double k, Matrix Ma)
{
int m = Ma.getM;
int n = Ma.getN;
Matrix Mc = new Matrix(m, n);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;

for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++)
c[i, j] = a[i, j] * k;
return Mc;
}

//矩阵转置
public static Matrix MatrixTrans(Matrix MatrixOrigin)
{
int m = MatrixOrigin.getM;
int n = MatrixOrigin.getN;
Matrix MatrixNew = new Matrix(n, m);
double[,] c = MatrixNew.Detail;
double[,] a = MatrixOrigin.Detail;
for (int i = 0; i < n; i++)
for (int j = 0; j < m; j++)
c[i, j] = a[j, i];
return MatrixNew;
}

//矩阵求逆（伴随矩阵法）
public static Matrix MatrixInvByCom(Matrix Ma)
{
double d = MatrixOperator.MatrixDet(Ma);
if (d == 0)
{
Exception myException = new Exception("没有逆矩阵");
throw myException;
}
Matrix Ax = MatrixOperator.MatrixCom(Ma);
Matrix An = MatrixOperator.MatrixSimpleMulti((1.0 / d), Ax);
return An;
}
//对应行列式的代数余子式矩阵
public static Matrix MatrixSpa(Matrix Ma, int ai, int aj)
{
int m = Ma.getM;
int n = Ma.getN;
if (m != n)
{
Exception myException = new Exception("矩阵不是方阵");
throw myException;
}
int n2 = n - 1;
Matrix Mc = new Matrix(n2, n2);
double[,] a = Ma.Detail;
double[,] b = Mc.Detail;

//左上
for (int i = 0; i < ai; i++)
for (int j = 0; j < aj; j++)
{
b[i, j] = a[i, j];
}
//右下
for (int i = ai; i < n2; i++)
for (int j = aj; j < n2; j++)
{
b[i, j] = a[i + 1, j + 1];
}
//右上
for (int i = 0; i < ai; i++)
for (int j = aj; j < n2; j++)
{
b[i, j] = a[i, j + 1];
}
//左下
for (int i = ai; i < n2; i++)
for (int j = 0; j < aj; j++)
{
b[i, j] = a[i + 1, j];
}
//符号位
if ((ai + aj) % 2 != 0)
{
for (int i = 0; i < n2; i++)
b[i, 0] = -b[i, 0];

}
return Mc;

}

//矩阵的行列式,矩阵必须是方阵
public static double MatrixDet(Matrix Ma)
{
int m = Ma.getM;
int n = Ma.getN;
if (m != n)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
double[,] a = Ma.Detail;
if (n == 1) return a[0, 0];

double D = 0;
for (int i = 0; i < n; i++)
{
D += a[1, i] * MatrixDet(MatrixSpa(Ma, 1, i));
}
return D;
}

//矩阵的伴随矩阵
public static Matrix MatrixCom(Matrix Ma)
{
int m = Ma.getM;
int n = Ma.getN;
Matrix Mc = new Matrix(m, n);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;

for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++)
c[i, j] = MatrixDet(MatrixSpa(Ma, j, i));

return Mc;
}
}

private void button1_Click(object sender, EventArgs e)      //计算左片后方交会
{
double[] x = { Convert.ToDouble(t1.Text), Convert.ToDouble(t6.Text), Convert.ToDouble(t11.Text), Convert.ToDouble(t16.Text) };
double[] y = { Convert.ToDouble(t2.Text), Convert.ToDouble(t7.Text), Convert.ToDouble(t12.Text), Convert.ToDouble(t17.Text) };
double[] X = { Convert.ToDouble(t3.Text), Convert.ToDouble(t8.Text), Convert.ToDouble(t13.Text), Convert.ToDouble(t18.Text) };
double[] Y = { Convert.ToDouble(t4.Text), Convert.ToDouble(t9.Text), Convert.ToDouble(t14.Text), Convert.ToDouble(t19.Text) };
double[] Z = { Convert.ToDouble(t5.Text), Convert.ToDouble(t10.Text), Convert.ToDouble(t15.Text), Convert.ToDouble(t20.Text) };
double f = Convert.ToDouble(t22.Text) / 1000, _m = Convert.ToDouble(t21.Text);
for (int i = 0; i < 4; i++)    //单位换算
{
x[i] = x[i] / 1000;
y[i] = y[i] / 1000;
}

/////定义外方位元素，并附初值
double Xs, Ys, Zs, phi = 0, omiga = 0, kappa = 0;
Xs = (X[0] + X[1] + X[2] + X[3]) / 4.0;
Ys = (Y[0] + Y[1] + Y[2] + Y[3]) / 4.0;
Zs = _m * f;

/////定义x,y近似值,即计算值
double[] _x = new double[4];
double[] _y = new double[4];

/////定义共线方程中的分子分母项，便于计算
double[] _X = new double[4];
double[] _Y = new double[4];
double[] _Z = new double[4];

/////定义旋转矩阵R
double[,] R = new double[3, 3];
double[,] L = new double[8, 1];//误差方程中的常数项
double[,] XX = new double[6, 1];//X向量

/////开始迭代
do
{
/////计算旋转矩阵
R[0, 0] = Math.Cos(phi) * Math.Cos(kappa) - Math.Sin(phi) * Math.Sin(omiga) * Math.Sin(kappa);//a1
R[0, 1] = -Math.Cos(phi) * Math.Sin(kappa) - Math.Sin(phi) * Math.Sin(omiga) * Math.Cos(kappa);//a2
R[0, 2] = -Math.Sin(phi) * Math.Cos(omiga);//a3
R[1, 0] = Math.Cos(omiga) * Math.Sin(kappa);//b1
R[1, 1] = Math.Cos(omiga) * Math.Cos(kappa);//b2
R[1, 2] = -Math.Sin(omiga);//b3
R[2, 0] = Math.Sin(phi) * Math.Cos(kappa) + Math.Cos(phi) * Math.Sin(omiga) * Math.Sin(kappa);//c1
R[2, 1] = -Math.Sin(phi) * Math.Sin(kappa) + Math.Cos(phi) * Math.Sin(omiga) * Math.Cos(kappa);//c2
R[2, 2] = Math.Cos(phi) * Math.Cos(omiga);//c3

for (int i = 0; i < 4; i++)
{
//用共线方程计算 x，y 的近似值 ,即计算值
_X[i] = R[0, 0] * (X[i] - Xs) + R[1, 0] * (Y[i] - Ys) + R[2, 0] * (Z[i] - Zs);
_Y[i] = R[0, 1] * (X[i] - Xs) + R[1, 1] * (Y[i] - Ys) + R[2, 1] * (Z[i] - Zs);
_Z[i] = R[0, 2] * (X[i] - Xs) + R[1, 2] * (Y[i] - Ys) + R[2, 2] * (Z[i] - Zs);

_x[i] = -f * _X[i] / _Z[i];
_y[i] = -f * _Y[i] / _Z[i];
}

Matrix B = new Matrix(8, 6, "B");//4个控制点，每个是2行6列，4个是8行6列
int n = B.getN;
int m = B.getM;
double[,] b = B.Detail;
for (int i = 0; i < 4; i++)
{
//计算系数矩阵
b[2 * i, 0] = (R[0, 0] * f + R[0, 2] * x[i]) / _Z[i];
b[2 * i, 1] = (R[1, 0] * f + R[1, 2] * x[i]) / _Z[i];
b[2 * i, 2] = (R[2, 0] * f + R[2, 2] * x[i]) / _Z[i];
b[2 * i, 3] = y[i] * Math.Sin(omiga) - ((x[i] / f) * (x[i] * Math.Cos(kappa) - y[i] * Math.Sin(kappa)) + f * Math.Cos(kappa)) * Math.Cos(omiga);
b[2 * i, 4] = -f * Math.Sin(kappa) - (x[i] / f) * (x[i] * Math.Sin(kappa) + y[i] * Math.Cos(kappa));
b[2 * i, 5] = y[i];

b[2 * i + 1, 0] = (R[0, 1] * f + R[0, 2] * y[i]) / _Z[i];
b[2 * i + 1, 1] = (R[1, 1] * f + R[1, 2] * y[i]) / _Z[i];
b[2 * i + 1, 2] = (R[2, 1] * f + R[2, 2] * y[i]) / _Z[i];
b[2 * i + 1, 3] = -x[i] * Math.Sin(omiga) - ((x[i] / f) * (x[i] * Math.Cos(kappa) - y[i] * Math.Sin(kappa)) - f * Math.Sin(kappa)) * Math.Cos(omiga);
b[2 * i + 1, 4] = -f * Math.Cos(kappa) - (y[i] / f) * (x[i] * Math.Sin(kappa) + y[i] * Math.Cos(kappa));
b[2 * i + 1, 5] = -x[i];

//计算常数项
L[2 * i, 0] = x[i] - _x[i];
L[2 * i + 1, 0] = y[i] - _y[i];
}
Matrix C = MatrixOperator.MatrixTrans(B);          //系数矩阵的转置矩阵
C.Name = "C";
Matrix D = MatrixOperator.MatrixMulti(C, B);       //系数矩阵与其转置矩阵相乘
D.Name = "C*B";
Matrix dn = MatrixOperator.MatrixInvByCom(D);      //系数矩阵与其转置矩阵积的逆矩阵
dn.Name = "dn";
Matrix dn2 = MatrixOperator.MatrixMulti(dn, C);       //ATA的逆阵乘以A的转置
dn2.Name = "dn2";
double[,] ATARAT = dn2.Detail;

////计算ATARAT * L，存在XX中
for (int i = 0; i < 6; i++)
for (int j = 0; j < 1; j++)
{
XX[i, j] = 0;
for (int l = 0; l < 8; l++)
XX[i, j] += ATARAT[i, l] * L[l, 0];
}

////计算外方位元素值
Xs += XX[0, 0];
Ys += XX[1, 0];
Zs += XX[2, 0];
phi += XX[3, 0];
omiga += XX[4, 0];
kappa += XX[5, 0];

}
while (Math.Abs(XX[0, 0]) >= 0.000029 || Math.Abs(XX[1, 0]) >= 0.000029 || Math.Abs(XX[2, 0]) >= 0.000029 || Math.Abs(XX[3, 0]) >= 1000 * 0.000029 || Math.Abs(XX[4, 0]) >= 1000 * 0.000029 || Math.Abs(XX[5, 0]) >= 1000 * 0.000029);

t23.Text = Convert.ToString(Xs);                         //输出到文本框
t24.Text = Convert.ToString(Ys);
t25.Text = Convert.ToString(Zs);
t26.Text = Convert.ToString(phi);
t27.Text = Convert.ToString(omiga);
t28.Text = Convert.ToString(kappa);
}

private void button2_Click(object sender, EventArgs e)     //清空数据，并输入右片数据
{
for (int i = 0; i < 20; i++)
{
mytextbox[i].Text = "";
}
}

private void Form1_Load(object sender, EventArgs e)       //文本框编译成数组，方便调用
{
int i = 0, j = 0;
TextBox t;
foreach (Control c in this.Controls)
{
if (c is TextBox)
{
mytextbox[i] = (TextBox)c;
i++;
}
}
for (i = 0; i < 41; i++)
{
for (j = 0; j < 41; j++)
{
if (mytextbox[j].Name == "t" + Convert.ToString(i + 1))
{
t = mytextbox[i];
mytextbox[i] = mytextbox[j];
mytextbox[j] = t;
// mytextbox[i].Text = Convert.ToString(i);
}
}
//mytextbox[i].Text = " ";
}
}

private void button3_Click(object sender, EventArgs e)     //计算右片后方交会
{
double[] x = { Convert.ToDouble(t1.Text), Convert.ToDouble(t6.Text), Convert.ToDouble(t11.Text), Convert.ToDouble(t16.Text) };
double[] y = { Convert.ToDouble(t2.Text), Convert.ToDouble(t7.Text), Convert.ToDouble(t12.Text), Convert.ToDouble(t17.Text) };
double[] X = { Convert.ToDouble(t3.Text), Convert.ToDouble(t8.Text), Convert.ToDouble(t13.Text), Convert.ToDouble(t18.Text) };
double[] Y = { Convert.ToDouble(t4.Text), Convert.ToDouble(t9.Text), Convert.ToDouble(t14.Text), Convert.ToDouble(t19.Text) };
double[] Z = { Convert.ToDouble(t5.Text), Convert.ToDouble(t10.Text), Convert.ToDouble(t15.Text), Convert.ToDouble(t20.Text) };
double f = Convert.ToDouble(t22.Text) / 1000, _m = Convert.ToDouble(t21.Text);
for (int i = 0; i < 4; i++)    //单位换算
{
x[i] = x[i] / 1000;
y[i] = y[i] / 1000;
}

/////定义外方位元素，并附初值
double Xs, Ys, Zs, phi = 0, omiga = 0, kappa = 0;
Xs = (X[0] + X[1] + X[2] + X[3]) / 4.0;
Ys = (Y[0] + Y[1] + Y[2] + Y[3]) / 4.0;
Zs = _m * f;

/////定义x,y近似值,即计算值
double[] _x = new double[4];
double[] _y = new double[4];

/////定义共线方程中的分子分母项，便于计算
double[] _X = new double[4];
double[] _Y = new double[4];
double[] _Z = new double[4];

/////定义旋转矩阵R
double[,] R = new double[3, 3];
double[,] L = new double[8, 1];//误差方程中的常数项
double[,] XX = new double[6, 1];//X向量

/////开始迭代
do
{
/////计算旋转矩阵
R[0, 0] = Math.Cos(phi) * Math.Cos(kappa) - Math.Sin(phi) * Math.Sin(omiga) * Math.Sin(kappa);//a1
R[0, 1] = -Math.Cos(phi) * Math.Sin(kappa) - Math.Sin(phi) * Math.Sin(omiga) * Math.Cos(kappa);//a2
R[0, 2] = -Math.Sin(phi) * Math.Cos(omiga);//a3
R[1, 0] = Math.Cos(omiga) * Math.Sin(kappa);//b1
R[1, 1] = Math.Cos(omiga) * Math.Cos(kappa);//b2
R[1, 2] = -Math.Sin(omiga);//b3
R[2, 0] = Math.Sin(phi) * Math.Cos(kappa) + Math.Cos(phi) * Math.Sin(omiga) * Math.Sin(kappa);//c1
R[2, 1] = -Math.Sin(phi) * Math.Sin(kappa) + Math.Cos(phi) * Math.Sin(omiga) * Math.Cos(kappa);//c2
R[2, 2] = Math.Cos(phi) * Math.Cos(omiga);//c3

for (int i = 0; i < 4; i++)
{
//用共线方程计算 x，y 的近似值 ,即计算值
_X[i] = R[0, 0] * (X[i] - Xs) + R[1, 0] * (Y[i] - Ys) + R[2, 0] * (Z[i] - Zs);
_Y[i] = R[0, 1] * (X[i] - Xs) + R[1, 1] * (Y[i] - Ys) + R[2, 1] * (Z[i] - Zs);
_Z[i] = R[0, 2] * (X[i] - Xs) + R[1, 2] * (Y[i] - Ys) + R[2, 2] * (Z[i] - Zs);

_x[i] = -f * _X[i] / _Z[i];
_y[i] = -f * _Y[i] / _Z[i];
}

Matrix B = new Matrix(8, 6, "B");//4个控制点，每个是2行6列，4个是8行6列
int n = B.getN;
int m = B.getM;
double[,] b = B.Detail;
for (int i = 0; i < 4; i++)
{
//计算系数矩阵
b[2 * i, 0] = (R[0, 0] * f + R[0, 2] * x[i]) / _Z[i];
b[2 * i, 1] = (R[1, 0] * f + R[1, 2] * x[i]) / _Z[i];
b[2 * i, 2] = (R[2, 0] * f + R[2, 2] * x[i]) / _Z[i];
b[2 * i, 3] = y[i] * Math.Sin(omiga) - ((x[i] / f) * (x[i] * Math.Cos(kappa) - y[i] * Math.Sin(kappa)) + f * Math.Cos(kappa)) * Math.Cos(omiga);
b[2 * i, 4] = -f * Math.Sin(kappa) - (x[i] / f) * (x[i] * Math.Sin(kappa) + y[i] * Math.Cos(kappa));
b[2 * i, 5] = y[i];

b[2 * i + 1, 0] = (R[0, 1] * f + R[0, 2] * y[i]) / _Z[i];
b[2 * i + 1, 1] = (R[1, 1] * f + R[1, 2] * y[i]) / _Z[i];
b[2 * i + 1, 2] = (R[2, 1] * f + R[2, 2] * y[i]) / _Z[i];
b[2 * i + 1, 3] = -x[i] * Math.Sin(omiga) - ((x[i] / f) * (x[i] * Math.Cos(kappa) - y[i] * Math.Sin(kappa)) - f * Math.Sin(kappa)) * Math.Cos(omiga);
b[2 * i + 1, 4] = -f * Math.Cos(kappa) - (y[i] / f) * (x[i] * Math.Sin(kappa) + y[i] * Math.Cos(kappa));
b[2 * i + 1, 5] = -x[i];

//计算常数项
L[2 * i, 0] = x[i] - _x[i];
L[2 * i + 1, 0] = y[i] - _y[i];
}
Matrix C = MatrixOperator.MatrixTrans(B);          //系数矩阵的转置矩阵
C.Name = "C";
Matrix D = MatrixOperator.MatrixMulti(C, B);       //系数矩阵与其转置矩阵相乘
D.Name = "C*B";
Matrix dn = MatrixOperator.MatrixInvByCom(D);      //系数矩阵与其转置矩阵积的逆矩阵
dn.Name = "dn";
Matrix dn2 = MatrixOperator.MatrixMulti(dn, C);       //ATA的逆阵乘以A的转置
dn2.Name = "dn2";
double[,] ATARAT = dn2.Detail;

////计算ATARAT * L，存在XX中
for (int i = 0; i < 6; i++)
for (int j = 0; j < 1; j++)
{
XX[i, j] = 0;
for (int l = 0; l < 8; l++)
XX[i, j] += ATARAT[i, l] * L[l, 0];
}

////计算外方位元素值
Xs += XX[0, 0];
Ys += XX[1, 0];
Zs += XX[2, 0];
phi += XX[3, 0];
omiga += XX[4, 0];
kappa += XX[5, 0];

}
while (Math.Abs(XX[0, 0]) >= 0.000029 || Math.Abs(XX[1, 0]) >= 0.000029 || Math.Abs(XX[2, 0]) >= 0.000029 || Math.Abs(XX[3, 0]) >= 1000 * 0.000029 || Math.Abs(XX[4, 0]) >= 1000 * 0.000029 || Math.Abs(XX[5, 0]) >= 1000 * 0.000029);

t29.Text = Convert.ToString(Xs);                         //输出到文本框
t30.Text = Convert.ToString(Ys);
t31.Text = Convert.ToString(Zs);
t32.Text = Convert.ToString(phi);
t33.Text = Convert.ToString(omiga);
t34.Text = Convert.ToString(kappa);
}

private void button5_Click(object sender, EventArgs e)     //清空待定地面点像点坐标
{
t35.Text = "";
t36.Text = "";
t37.Text = "";
t38.Text = "";
t39.Text = "";
t40.Text = "";
t41.Text = "";

}

private void button4_Click(object sender, EventArgs e)    //计算前方交会
{
double Xs1, Ys1, Zs1, phi_1, omiga_1, kappa_1, Xs2, Ys2, Zs2, phi_2, omiga_2, kappa_2;
double N1, N2, X, Y, Z;
double Bu, Bv, Bw;
Xs1 = Convert.ToDouble(t23.Text);               //十二个外方位元素
Ys1 = Convert.ToDouble(t24.Text);
Zs1 = Convert.ToDouble(t25.Text);
phi_1 = Convert.ToDouble(t26.Text);
omiga_1 = Convert.ToDouble(t27.Text);
kappa_1 = Convert.ToDouble(t28.Text);
Xs2 = Convert.ToDouble(t29.Text);
Ys2 = Convert.ToDouble(t30.Text);
Zs2 = Convert.ToDouble(t31.Text);
phi_2 = Convert.ToDouble(t32.Text);
omiga_2 = Convert.ToDouble(t33.Text);
kappa_2 = Convert.ToDouble(t34.Text);
Matrix tt1 = new Matrix(3, 1, "tt1");
Matrix tt2 = new Matrix(3, 1, "tt2");
double[,] a1 = tt1.Detail;
double[,] a2 = tt2.Detail;
int n = tt1.getN;
int m = tt1.getM;

a1[0, 0] = Convert.ToDouble(t35.Text);            //地面点A相应的像点a1、a2的像空间坐标系
a1[1, 0] = Convert.ToDouble(t36.Text);
a1[2, 0] = -Convert.ToDouble(t22.Text);
a2[0, 0] = Convert.ToDouble(t37.Text);
a2[1, 0] = Convert.ToDouble(t38.Text);

Matrix rr1 = new Matrix(3, 3, "rr1");
Matrix rr2 = new Matrix(3, 3, "rr2");
double[,] rrr1 = rr1.Detail;
double[,] rrr2 = rr2.Detail;

/////计算左片旋转矩阵
rrr1[0, 0] = Math.Cos(phi_1) * Math.Cos(kappa_1) - Math.Sin(phi_1) * Math.Sin(omiga_1) * Math.Sin(kappa_1);//a1
rrr1[0, 1] = -Math.Cos(phi_1) * Math.Sin(kappa_1) - Math.Sin(phi_1) * Math.Sin(omiga_1) * Math.Cos(kappa_1);//a2
rrr1[0, 2] = -Math.Sin(phi_1) * Math.Cos(omiga_1);//a3
rrr1[1, 0] = Math.Cos(omiga_1) * Math.Sin(kappa_1);//b1
rrr1[1, 1] = Math.Cos(omiga_1) * Math.Cos(kappa_1);//b2
rrr1[1, 2] = -Math.Sin(omiga_1);//b3
rrr1[2, 0] = Math.Sin(phi_1) * Math.Cos(kappa_1) + Math.Cos(phi_1) * Math.Sin(omiga_1) * Math.Sin(kappa_1);//c1
rrr1[2, 1] = -Math.Sin(phi_1) * Math.Sin(kappa_1) + Math.Cos(phi_1) * Math.Sin(omiga_1) * Math.Cos(kappa_1);//c2
rrr1[2, 2] = Math.Cos(phi_1) * Math.Cos(omiga_1);//c3

/////计算右片旋转矩阵
rrr2[0, 0] = Math.Cos(phi_2) * Math.Cos(kappa_2) - Math.Sin(phi_2) * Math.Sin(omiga_2) * Math.Sin(kappa_2);//a1
rrr2[0, 1] = -Math.Cos(phi_2) * Math.Sin(kappa_2) - Math.Sin(phi_2) * Math.Sin(omiga_2) * Math.Cos(kappa_2);//a2
rrr2[0, 2] = -Math.Sin(phi_2) * Math.Cos(omiga_2);//a3
rrr2[1, 0] = Math.Cos(omiga_2) * Math.Sin(kappa_2);//b1
rrr2[1, 1] = Math.Cos(omiga_2) * Math.Cos(kappa_2);//b2
rrr2[1, 2] = -Math.Sin(omiga_2);//b3
rrr2[2, 0] = Math.Sin(phi_2) * Math.Cos(kappa_2) + Math.Cos(phi_2) * Math.Sin(omiga_2) * Math.Sin(kappa_2);//c1
rrr2[2, 1] = -Math.Sin(phi_2) * Math.Sin(kappa_2) + Math.Cos(phi_2) * Math.Sin(omiga_2) * Math.Cos(kappa_2);//c2
rrr2[2, 2] = Math.Cos(phi_2) * Math.Cos(omiga_2);//c3

Matrix Ss1 = MatrixOperator.MatrixMulti(rr1, tt1);  //地面点A在像空间辅助坐标系的矩阵
Matrix Ss2 = MatrixOperator.MatrixMulti(rr2, tt2);
double[,] S1 = Ss1.Detail;
double[,] S2 = Ss2.Detail;
Bu = Xs2 - Xs1;                                     //摄影基线B的分量
Bv = Ys2 - Ys1;
Bw = Zs2 - Zs1;

N1 = (Bu * S2[2, 0] - Bw * S2[0, 0]) / (S1[0, 0] * S2[2, 0] - S2[0, 0] * S1[2, 0]);
N2 = (Bu * S1[2, 0] - Bw * S1[0, 0]) / (S1[0, 0] * S2[2, 0] - S2[0, 0] * S1[2, 0]);

X = Xs1 + N1 * S1[0, 0];
Y = 0.5 * ((Ys1 + N1 * S1[1, 0]) + (Ys2 + N2 * S2[1, 0]));
Z = Zs1 + N1 * S1[2, 0];
t39.Text = Convert.ToString(X);       //输出地面坐标
t40.Text = Convert.ToString(Y);
t41.Text = Convert.ToString(Z);
}
}

}

程序界面如下：



程序源代码链接：后方交会_前方交会 C#