C#单像空间后方交会计算

空间后方交会 (Space Resection)的定义：利用地面控制点(GCP,Ground Control Point)及其在像片上的像点，确定单幅影像外方位元素的方法。

如果已知每幅影像的6个外方位元素，就能确定被摄物体与航摄影像的关系。因此，如何获取影像的外方位元素，一直是摄影测量工作者所探讨的问题。也可利用影像覆盖范围内一定数量的控制点的空间坐标与影像坐标，根据共线条件方程反求该影像的外方位元素，这种方法称为单幅影像的空间后方交会。

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

namespace 单向空间后方交会
{
    public partial class Form1 : Form
    {
        List<ImgData> Img = new List<ImgData>();//像点
        List<GroundData> Ground = new List<GroundData>();//地面点

        Matrix A = new Matrix(8, 6);//误差方程
        Matrix X = new Matrix(6, 1);
        Matrix L = new Matrix(8, 1);

        Matrix AT = new Matrix(6, 8);//A的转置矩阵
        Matrix ATA = new Matrix(6, 6);//A与转置矩阵的乘积
        Matrix inv_ATA = new Matrix(6, 6);//A与其转置的乘积的逆矩阵
        Matrix invATAmAT = new Matrix(6, 8);//A与其转置的乘积的逆矩阵与A转置的乘积

        double f = 153.24 / 1000;
        double m = 40000;
        int k = 0;//迭代次数

        Matrix xy = new Matrix(4, 2);//像点坐标
        Matrix x0y0 = new Matrix(4, 2);//像点坐标计算值(近似)
        Matrix XYZ = new Matrix(4, 3);//地面点坐标

        Matrix XsYsZs = new Matrix(3, 1);//外方位元素直线元素
        Matrix rAngle = new Matrix(3, 1);//外方位元素角元素

        Matrix R = new Matrix(3, 3);//旋转矩阵

        public Form1()
        {
            InitializeComponent();
        }

        private void button1_Click(object sender, EventArgs e)
        {
            OpenFileDialog op = new OpenFileDialog();
            op.Filter = "文本文件|*.txt";
            string filepath = null;
            if (op.ShowDialog() == DialogResult.OK)
            {
                filepath = op.FileName;
                imgData(filepath);
            }
        }
        private void button2_Click(object sender, EventArgs e)
        {
            OpenFileDialog op = new OpenFileDialog();
            op.Filter = "文本文件|*.txt";
            string filepath = null;
            if (op.ShowDialog() == DialogResult.OK)
            {
                filepath = op.FileName;
                groundData(filepath);
            }
        }

        private void imgData(string filepath)
        {
            StreamReader sr = new StreamReader(filepath);
            ImgData img;
            while (!sr.EndOfStream)
            {
                string[] info = sr.ReadLine().Trim().Split();
                double x = Convert.ToDouble(info[0]);
                double y = Convert.ToDouble(info[2]);
                img = new ImgData(x, y);
                Img.Add(img);
            }
            //矩阵赋值
            for (int i = 0; i < Img.Count; i++)
            {
                xy.arr[i, 0] = Img[i].X / 1000;
                xy.arr[i, 1] = Img[i].Y / 1000;
            }
            string text = "*****像点坐标*****\n";
            text += "x(mm)\ty(mm)\n";
            for (int i = 0; i < Img.Count; i++)
            {
                text += Img[i].X.ToString() + "\t" + Img[i].Y.ToString() + "\n";
            }
            text += "****************";
            richTextBox1.Text = text;
        }

        private void groundData(string filepath)
        {
            StreamReader sr = new StreamReader(filepath);
            GroundData ground;
            while (!sr.EndOfStream)
            {
                string[] info = sr.ReadLine().Trim().Split();
                double x = Convert.ToDouble(info[0]);
                double y = Convert.ToDouble(info[2]);
                double z = Convert.ToDouble(info[4]);
                ground = new GroundData(x, y, z);
                Ground.Add(ground);
            }
            //矩阵赋值
            for (int i = 0; i < Ground.Count; i++)
            {
                XYZ.arr[i, 0] = Ground[i]._X;
                XYZ.arr[i, 1] = Ground[i]._Y;
                XYZ.arr[i, 2] = Ground[i]._Z;
            }
            string text = "**************地面点坐标**************\n";
            text += "X(m)\t\tY(m)\t\tZ(m)\n";
            for (int i = 0; i < Ground.Count; i++)
            {
                text += Ground[i]._X.ToString() + "\t" + Ground[i]._Y.ToString() + "\t" + Ground[i]._Z.ToString() + "\n";
            }
            text += "**************************************";
            richTextBox2.Text = text;
        }

        private void button3_Click(object sender, EventArgs e)
        {
            for (int i = 0; i < 3; i++)
                rAngle.arr[i, 0] = 0;//角元素初始值为0

            for (int i = 0; i < 6; i++)
            {
                X.arr[i, 0] = 0;
            }

            double sumX = 0, sumY = 0, sumZ = 0;
            for (int i = 0; i < 4; i++)
            {
                sumX += XYZ.arr[i, 0];
                sumY += XYZ.arr[i, 1];
                sumZ += XYZ.arr[i, 2];
            }
            XsYsZs.arr[0, 0] = sumX / 4;
            XsYsZs.arr[1, 0] = sumY / 4;
            XsYsZs.arr[2, 0] = sumZ / 4 + f * m;

            Algo algo = new Algo();
            //系数矩阵A
            A = algo.Cal_A(A, m, f, xy);
            //计算A的转置
            AT = algo.Cal_AT(AT, A);
            //计算ATA
            ATA = algo.Cal_MmN(AT, A, ATA);
            //计算ATA逆阵
            inv_ATA = algo.Cal_invATA(ATA, inv_ATA);
            //计算ATA逆阵与AT乘积
            invATAmAT = algo.Cal_MmN(inv_ATA, AT, invATAmAT);

            //迭代计算
            do
            {
                XsYsZs.arr[0, 0] += X.arr[0, 0];
                XsYsZs.arr[1, 0] += X.arr[1, 0];
                XsYsZs.arr[2, 0] += X.arr[2, 0];

                rAngle.arr[0, 0] += X.arr[3, 0];
                rAngle.arr[1, 0] += X.arr[4, 0];
                rAngle.arr[2, 0] += X.arr[5, 0];

                //计算旋转矩阵
                R = algo.Cal_R(R, rAngle);

                //计算（x）（y）
                x0y0 = algo.Cal_x0y0(x0y0, R, f, XYZ, XsYsZs);

                //计算L
                L = algo.Cal_L(L, xy, x0y0);

                //计算X
                X = algo.Cal_MmN(invATAmAT, L, X);

                k++;//迭代跳出
   /*                 if (k > 5)
                    break;*/

            } while (Math.Abs(X.arr[4, 0]) > 0.0000001);
            richTextBox3.AppendText(" 直线元素          角元素\n");
            richTextBox3.AppendText("--------------------------------\n");
            richTextBox3.AppendText("Xs  " + XsYsZs.arr[0, 0].ToString("0.00").PadRight(12, ' ') + "ψ   " + rAngle.arr[0, 0].ToString("0.00000") + "\n");
            richTextBox3.AppendText("Ys  " + XsYsZs.arr[1, 0].ToString("0.00").PadRight(12, ' ') + "ω   " + rAngle.arr[1, 0].ToString("0.00000") + "\n");
            richTextBox3.AppendText("Zs  " + XsYsZs.arr[2, 0].ToString("0.00").PadRight(12, ' ') + "κ   " + rAngle.arr[2, 0].ToString("0.00000") + "\n\n");

            richTextBox3.AppendText("迭代次数：   " + k.ToString() + "\n");
            richTextBox3.AppendText("比例尺：   " + (m).ToString("0.000000") + "\n");

        }

    }
}

Algo.cs
namespace 单向空间后方交会
{
    internal class Algo
    {
        public Matrix Cal_A(Matrix A, double m, double f, Matrix xy)
        {
            for (int i = 0; i < 4; i++)
            {
                A.arr[i * 2, 0] = -1 / m;
                A.arr[i * 2, 1] = 0;
                A.arr[i * 2, 2] = -xy.arr[i, 0] / (f * m);
                A.arr[i * 2, 3] = -f * (1 + Math.Pow(xy.arr[i, 0], 2) / (f * f));
                A.arr[i * 2, 4] = -xy.arr[i, 0] * xy.arr[i, 1] / f;
                A.arr[i * 2, 5] = xy.arr[i, 1];

                A.arr[i * 2 + 1, 0] = 0;
                A.arr[i * 2 + 1, 1] = -1 / m;
                A.arr[i * 2 + 1, 2] = -xy.arr[i, 1] / (f * m);
                A.arr[i * 2 + 1, 3] = -xy.arr[i, 0] * xy.arr[i, 1] / f;
                A.arr[i * 2 + 1, 4] = -f * (1 + Math.Pow(xy.arr[i, 1], 2) / (f * f));
                A.arr[i * 2 + 1, 5] = -xy.arr[i, 0];
            }
            return A;
        }

        public Matrix Cal_AT(Matrix AT, Matrix A)
        {
            //计算A的转置
            AT = A.Transpose(A);
            return AT;
        }

        public Matrix Cal_MmN(Matrix M, Matrix N, Matrix MmN)
        {
            //计算ATA
            MmN = M * N;
            return MmN;
        }

        public Matrix Cal_invATA(Matrix ATA, Matrix inv_ATA)
        {
            //计算ATA逆矩阵
            inv_ATA = ATA.Inverse(ATA);
            return inv_ATA;
        }


        public Matrix Cal_R(Matrix R, Matrix rAngle)
        {
            R.arr[0, 0] = Math.Cos(rAngle.arr[0, 0]) * Math.Cos(rAngle.arr[2, 0]) - Math.Sin(rAngle.arr[0, 0]) * Math.Sin(rAngle.arr[1, 0]) * Math.Sin(rAngle.arr[2, 0]);
            R.arr[0, 1] = -(Math.Cos(rAngle.arr[0, 0]) * Math.Sin(rAngle.arr[2, 0])) - Math.Sin(rAngle.arr[0, 0]) * Math.Sin(rAngle.arr[1, 0]) * Math.Cos(rAngle.arr[2, 0]);
            R.arr[0, 2] = -(Math.Sin(rAngle.arr[0, 0]) * Math.Cos(rAngle.arr[1, 0]));

            R.arr[1, 0] = Math.Cos(rAngle.arr[1, 0]) * Math.Sin(rAngle.arr[2, 0]);
            R.arr[1, 1] = Math.Cos(rAngle.arr[1, 0]) * Math.Cos(rAngle.arr[2, 0]);
            R.arr[1, 2] = -(Math.Sin(rAngle.arr[1, 0]));

            R.arr[2, 0] = Math.Sin(rAngle.arr[0, 0]) * Math.Cos(rAngle.arr[2, 0]) + Math.Cos(rAngle.arr[0, 0]) * Math.Sin(rAngle.arr[1, 0]) * Math.Sin(rAngle.arr[2, 0]);
            R.arr[2, 1] = -(Math.Sin(rAngle.arr[0, 0]) * Math.Sin(rAngle.arr[2, 0])) + Math.Cos(rAngle.arr[0, 0]) * Math.Sin(rAngle.arr[1, 0]) * Math.Cos(rAngle.arr[2, 0]);
            R.arr[2, 2] = Math.Cos(rAngle.arr[0, 0]) * Math.Cos(rAngle.arr[1, 0]);
            return R;
        }

        public Matrix Cal_x0y0(Matrix x0y0, Matrix R, double f, Matrix XYZ, Matrix XsYsZs)
        {
            for (int i = 0; i < 4; i++)
            {
                x0y0.arr[i, 0] = f * (R.arr[0, 0] * (XYZ.arr[i, 0] - XsYsZs.arr[0, 0]) + R.arr[1, 0] * (XYZ.arr[i, 1] - XsYsZs.arr[1, 0]) + R.arr[2, 0] * (XYZ.arr[i, 2] - XsYsZs.arr[2, 0])) / (R.arr[0, 2] * (XYZ.arr[i, 0] - XsYsZs.arr[0, 0]) + R.arr[1, 2] * (XYZ.arr[i, 1] - XsYsZs.arr[1, 0]) + R.arr[2, 2] * (XYZ.arr[i, 2] - XsYsZs.arr[2, 0]));
                x0y0.arr[i, 1] = f * (R.arr[0, 1] * (XYZ.arr[i, 0] - XsYsZs.arr[0, 0]) + R.arr[1, 1] * (XYZ.arr[i, 1] - XsYsZs.arr[1, 0]) + R.arr[2, 1] * (XYZ.arr[i, 2] - XsYsZs.arr[2, 0])) / (R.arr[0, 2] * (XYZ.arr[i, 0] - XsYsZs.arr[0, 0]) + R.arr[1, 2] * (XYZ.arr[i, 1] - XsYsZs.arr[1, 0]) + R.arr[2, 2] * (XYZ.arr[i, 2] - XsYsZs.arr[2, 0]));
            }
            return x0y0;
        }

        public Matrix Cal_L(Matrix L, Matrix xy, Matrix x0y0)
        {
            for (int i = 0; i < 4; i++)
            {
                L.arr[i * 2, 0] = xy.arr[i, 0] + x0y0.arr[i, 0];
                L.arr[i * 2 + 1, 0] = xy.arr[i, 1] + x0y0.arr[i, 1];
            }
            return L;
        }
    }
}

Matrix.cs
namespace 单向空间后方交会
{
    internal class Matrix
    {
        public int r;//行数
        public int c;//列数
        public double[,] arr;//矩阵元素
        /// <summary>
        /// 无参构造函数
        /// </summary>
        public Matrix()
        {
            r = 0;
            c = 0;
            arr = new double[r, c];
        }
        /// <summary>
        /// 拷贝构造函数
        /// </summary>
        /// <param name="m">矩阵</param>
        public Matrix(Matrix m)
        {
            r = m.r;
            c = m.c;
            //arr = m.arr;
            for (int i = 0; i < r; i++)
            {
                for (int j = 0; j < c; j++)
                {
                    arr[i, j] = m.arr[i, j];
                }
            }
        }
        /// <summary>
        /// 有参构造函数
        /// </summary>
        /// <param name="rr">行数</param>
        /// <param name="cc">列数</param>
        /// <param name="arr1">矩阵元素</param>
        public Matrix(int rr, int cc, double[,] arr1)
        {
            r = rr;
            c = cc;
            for (int i = 0; i < r; i++)
            {
                for (int j = 0; j < c; j++)
                {
                    arr[i, j] = arr1[i, j];
                }
            }
        }
        /// <summary>
        /// 有参构造函数
        /// </summary>
        /// <param name="rr">行数</param>
        /// <param name="cc">列数</param>
        public Matrix(int rr, int cc)
        {
            r = rr;
            c = cc;
            arr = new double[r, c];
        }
        /// <summary>
        /// 单位矩阵
        /// </summary>
        /// <param name="rr">行数</param>
        /// <param name="cc">列数</param>
        /// <returns></returns>
        public Matrix eye(int rr, int cc)
        {
            Matrix matrix = new Matrix(rr, cc);
            for (int i = 0; i < rr; i++)
            {
                for (int j = 0; j < cc; j++)
                {
                    arr[i, j] = 1;
                }
            }
            return matrix;
        }
        /// <summary>
        /// 创建零矩阵
        /// </summary>
        /// <param name="rr">行数</param>
        /// <param name="cc">列数</param>
        /// <returns></returns>
        public Matrix zeros(int rr, int cc)
        {
            Matrix matrix = new Matrix(rr, cc);
            for (int i = 0; i < rr; i++)
            {
                for (int j = 0; j < cc; j++)
                {
                    arr[i, j] = 0;
                }
            }
            return matrix;
        }
        /// <summary>
        /// 加法
        /// </summary>
        /// <param name="A">矩阵A</param>
        /// <param name="B">矩阵B</param>
        /// <returns></returns>
        static public Matrix operator +(Matrix A, Matrix B)
        {
            Matrix C = new Matrix(A.r, B.c);
            if (A.r != B.r || A.c != B.c || A.r != C.r || A.c != C.c)
                MessageBox.Show("矩阵维数不同");
            for (int i = 0; i < C.r; i++)
            {
                for (int j = 0; j < C.c; j++)
                {
                    C.arr[i, j] = A.arr[i, j] + B.arr[i, j];
                }
            }
            return C;
        }
        /// <summary>
        /// 减法
        /// </summary>
        /// <param name="A">矩阵A</param>
        /// <param name="B">矩阵B</param>
        /// <returns></returns>
        static public Matrix operator -(Matrix A, Matrix B)
        {
            Matrix C = new Matrix(A.r, B.c);
            if (A.r != B.r || A.c != B.c || A.r != C.r || A.c != C.c)
                MessageBox.Show("矩阵维数不同");
            for (int i = 0; i < C.r; i++)
            {
                for (int j = 0; j < C.c; j++)
                {
                    C.arr[i, j] = A.arr[i, j] - B.arr[i, j];
                }
            }
            return C;
        }
        /// <summary>
        /// 矩阵乘法
        /// </summary>
        /// <param name="A">矩阵A</param>
        /// <param name="B">矩阵B</param>
        /// <returns></returns>
        static public Matrix operator *(Matrix A, Matrix B)
        {
            double temp = 0;
            Matrix C = new Matrix(A.r, B.c);
            if (A.r != C.r || B.c != C.c || A.c != B.r)
                MessageBox.Show("矩阵乘法错误");
            for (int i = 0; i < C.r; i++)
            {
                for (int j = 0; j < C.c; j++)
                {
                    temp = 0;
                    for (int k = 0; k < A.c; k++)
                    {
                        temp += A.arr[i, k] * B.arr[k, j];
                    }
                    C.arr[i, j] = temp;
                }
            }
            return C;
        }
        /// <summary>
        /// 矩阵转置
        /// </summary>
        /// <param name="A">待装置矩阵A</param>
        /// <returns></returns>
        public Matrix Transpose(Matrix A)
        {
            Matrix matrix = new Matrix(A.c, A.r);
            for (int i = 0; i < matrix.r; i++)
            {
                for (int j = 0; j < matrix.c; j++)
                {
                    matrix.arr[i, j] = A.arr[j, i];
                }
            }
            return matrix;
        }
        public double[,] inverseMatrix(double[,] matrix)
        {
            int n = matrix.GetLength(0);
            double[,] result = new double[n, n];
            double[,] temp = new double[n, 2 * n];

            //将矩阵和单位矩阵拼接成一个2n*n的矩阵
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    temp[i, j] = matrix[i, j];
                    temp[i, j + n] = i == j ? 1 : 0;
                }
            }
            //高斯-约旦消元法
            for (int i = 0; i < n; i++)
            {
                double tempValue = temp[i, i];
                for (int j = i; j < 2 * n; j++)
                {
                    temp[i, j] /= tempValue;
                }
                for (int j = 0; j < n; j++)
                {
                    if (j != i)
                    {
                        tempValue = temp[j, i];
                        for (int k = i; k < 2 * n; k++)
                        {
                            temp[j, k] -= tempValue * temp[i, k];
                        }
                    }
                }
            }
            //取出逆矩阵
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    result[i, j] = temp[i, j + n];
                }
            }

            return result;
        }
        /// <summary>
        /// 矩阵求逆
        /// </summary>
        /// <param name="A"></param>
        /// <returns></returns>
        public Matrix Inverse(Matrix A)
        {
            Matrix result = new Matrix(A.r, A.c);
            result.arr = inverseMatrix(A.arr);
            return result;
        }
    }
}