摄影测量后方交会-前方交会(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#