单像空间后方交会

单像空间后方交会

数据文件

源代码

using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO;
using static System.Math;

namespace 摄影测量学4
{
    public partial class MDIParent1 : Form
    {
        private int childFormNumber = 0;

        public MDIParent1()
        {
            InitializeComponent();
        }

        private void ShowNewForm(object sender, EventArgs e)
        {
            Form childForm = new Form();
            childForm.MdiParent = this;
            childForm.Text = "窗口 " + childFormNumber++;
            childForm.Show();
        }
        List<double> x=new List<double>();
        List<double> y=new List<double>();
        List<double> X = new List<double>();
        List<double> Y=new List<double>();
        List<double> Z = new List<double>();
        double f = 0;
        double m = 0;
        double x0 = 0;
        double y0 = 0;
        double varphi = 0;
        double omega = 0;
        double kappa = 0;
        double XS0 = 0;
        double YS0 = 0;
        double ZS0 = 0;
        string line="";
        private void OpenFile(object sender, EventArgs e)
        {
            string FileName="";
            OpenFileDialog openFileDialog = new OpenFileDialog();
            openFileDialog.InitialDirectory = Environment.GetFolderPath(Environment.SpecialFolder.Personal);
            openFileDialog.Filter = "文本文件(*.txt)|*.txt|所有文件(*.*)|*.*";
            if (openFileDialog.ShowDialog(this) == DialogResult.OK)
            {
                FileName = openFileDialog.FileName;
            }
            if (FileName != "")
            {
                try
                {
                    using (StreamReader re = new StreamReader(FileName))
                    {
                        string linshidata = null;
                        int i = 0;
                        while ((linshidata = re.ReadLine()) != null)
                        {
                            line= linshidata;
                            if (i == 0)
                            {
                                f = double.Parse(line.Split(',')[0])/1000;
                                x0 = double.Parse(line.Split(',')[1]);
                                y0 = double.Parse(line.Split(',')[2]);
                                m = double.Parse(line.Split(',')[3]);
                                i += 1;
                            }
                            else
                            {
                                x.Add(double.Parse(line.Split(',')[0])/1000);
                                y.Add(double.Parse(line.Split(',')[1])/1000);
                                X.Add(double.Parse(line.Split(',')[2]));
                                Y.Add(double.Parse(line.Split(',')[3]));
                                Z.Add(double.Parse(line.Split(',')[4]));
                            }
                        }
                        //textBox1.Text = X[0].ToString();
                        XS0 = X.Sum() / X.Count();
                        YS0 = Y.Sum() / Y.Count();
                        ZS0 = m * f;
                    }
                }
                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message);
                }
                //chuzhi();
            }
        }

        List<string> savedata=new List<string>();

        private void SaveAsToolStripMenuItem_Click(object sender, EventArgs e)
        {
            string FileName = "";
            SaveFileDialog saveFileDialog = new SaveFileDialog();
            saveFileDialog.InitialDirectory = Environment.GetFolderPath(Environment.SpecialFolder.Personal);
            saveFileDialog.Filter = "文本文件(*.txt)|*.txt|所有文件(*.*)|*.*";
            if (saveFileDialog.ShowDialog(this) == DialogResult.OK)
            {
                FileName = saveFileDialog.FileName;
            }
            if(FileName!="")
            {
                try
                {
                    using (StreamWriter sw= new StreamWriter(FileName))
                    {
                        for(int i=0;i<savedata.Count();i++)
                        {
                            sw.WriteLine(savedata[i]);
                        }
                    }
                }
                catch(Exception ex)
                {

                }
            }
        }

        private void ExitToolsStripMenuItem_Click(object sender, EventArgs e)
        {
            this.Close();
        }

        //第四步旋转矩阵
        public double[,] R()
        {
            double a1 = Cos(varphi) * Cos(kappa) - Sin(varphi) * Sin(omega) * Sin(kappa);
            double a2 = -Cos(varphi) * Sin(kappa) - Sin(varphi) * Sin(omega) * Cos(kappa);
            double a3 = -Sin(varphi) * Cos(omega);
            double b1 = Cos(omega) * Sin(kappa);
            double b2 = Cos(omega) * Cos(kappa);
            double b3 = -Sin(omega);
            double c1 = Cos(kappa) * Sin(varphi) + Cos(varphi) * Sin(omega) * Sin(kappa);
            double c2 = -Sin(kappa) * Sin(varphi) + Cos(varphi) * Sin(omega) * Cos(kappa);
            double c3 = Cos(varphi) * Cos(omega);
            double[,] r = new double[3, 3]
            {
                {a1,a2,a3},
                {b1,b2,b3},
                {c1,c2,c3}
            };
            return r;
        }

        //第五步外方位元素坐标
        public double[] xy(double xa,double ya,double za)
        {
            double[,] r = R();
            double x = -f * (r[0, 0] * (xa - XS0) + r[1, 0] * (ya - YS0) + r[2, 0] * (za - ZS0)) /
                (r[0, 2] * (xa - XS0) + r[1, 2] * (ya - YS0) + r[2, 2] * (za - ZS0));
            double y = -f * (r[0, 1] * (xa - XS0) + r[1, 1] * (ya - YS0) + r[2, 1] * (za - ZS0)) /
                (r[0, 2] * (xa - XS0) + r[1, 2] * (ya - YS0) + r[2, 2] * (za - ZS0));
            double[] xx = new double[2] { x, y };
            return xx;
        }

        //第七步
        public double[,] A(double x,double y)
        {
            double a11 = -f / ZS0;
            double a12 = 0;
            double a13 = -x / ZS0;
            double a14 = -f * (1 + x * x / (f * f));
            double a15 = -x * y / f;
            double a16 = y;
            double a21 = 0;
            double a22 = -f / ZS0;
            double a23 = -y / ZS0;
            double a24 = -x * y / f;
            double a25 = -f * (1 + y * y / (f * f));
            double a26 = -x;
            double[,] AA = new double[2, 6]
            {
                {a11,a12,a13,a14,a15,a16 },
                {a21,a22,a23,a24,a25,a26 }
            };
            return AA;
        }

        //第三步赋初值
        public void chuzhi()
        {
            XS0 = X.Sum() / X.Count();
            YS0=Y.Sum()/Y.Count();
            ZS0 = m * f;
        }

        //矩阵乘法
        public double[,] cheng(double[,] a, double[,] b)
        {
            int n = a.GetLength(1);
            double[,] result = new double[a.GetLength(0), b.GetLength(1)];

            for (int i = 0; i < a.GetLength(0); i++)
            {
                for (int j = 0; j < b.GetLength(1); j++)
                {
                    double sum = 0;
                    for (int k = 0; k < n; k++)
                    {
                        sum += a[i, k] * b[k, j];
                    }
                    result[i, j] = sum;
                }
            }

            return result;
        }
        
        //矩阵求逆
        public static double[,] InvertMatrix(double[,] matrix)
        {
            int n = matrix.GetLength(0);
            double[,] augmentedMatrix = AugmentMatrix(matrix);

            for (int i = 0; i < n; i++)
            {
                // 对角线元素为1，其他元素为0的行变换
                double divisor = augmentedMatrix[i, i];
                for (int j = 0; j < 2 * n; j++)
                {
                    augmentedMatrix[i, j] /= divisor;
                }

                // 将第i列的其他元素变为0
                for (int k = 0; k < n; k++)
                {
                    if (k != i)
                    {
                        double factor = augmentedMatrix[k, i];
                        for (int j = 0; j < 2 * n; j++)
                        {
                            augmentedMatrix[k, j] -= factor * augmentedMatrix[i, j];
                        }
                    }
                }
            }

            // 取出逆矩阵部分（右侧部分）
            double[,] inverseMatrix = new double[n, n];
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    inverseMatrix[i, j] = augmentedMatrix[i, j + n];
                }
            }

            return inverseMatrix;
        }
        private static double[,] AugmentMatrix(double[,] matrix)
        {
            int n = matrix.GetLength(0);
            double[,] augmentedMatrix = new double[n, 2 * n];

            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    augmentedMatrix[i, j] = matrix[i, j];
                    augmentedMatrix[i, j + n] = (i == j) ? 1.0 : 0.0;
                }
            }

            return augmentedMatrix;
        }
        
        //矩阵转置
        public static double[,] Transpose(double[,] m)
        {
            double[,] lsm = new double[m.GetLength(1), m.GetLength(0)];
            for (int i = 0; i < m.GetLength(0); i++)
            {
                for (int j = 0; j < m.GetLength(1); j++)
                {
                    lsm[j, i] = m[i, j];
                }
            }
            return lsm;
        }

        private void 计算ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            double[,] RR = R();
            double[,] yx = new double[x.Count * 2, 1];
            double[,] startxy = new double[x.Count() * 2, 1];
            double[,] AA = new double[x.Count() * 2, 6];

            double xs0 = 0;
            double ys0 = 0;
            double zs0 = 0;
            double varphi2 = 0;
            double omega2 = 0;
            double kappa2 = 0;

            while (true)
            {
                for (int i = 0; i < x.Count(); i++)
                {
                    yx[2 * i, 0] = xy(X[i], Y[i], Z[i])[0];
                    yx[2 * i + 1, 0] = xy(X[i], Y[i], Z[i])[1];
                    startxy[2 * i, 0] = x[i];
                    startxy[2 * i + 1, 0] = y[i];
                }
                double[,] L = SubtractMatrices(startxy, yx);


                for (int i = 0; i < x.Count();)
                {
                    double xxx = yx[i, 0];
                    double yyy = yx[i * 2, 0];
                    AA[i, 0] = A(xxx, yyy)[0, 0];
                    AA[i, 1] = A(xxx, yyy)[0, 1];
                    AA[i, 2] = A(xxx, yyy)[0, 2];
                    AA[i, 3] = A(xxx, yyy)[0, 3];
                    AA[i, 4] = A(xxx, yyy)[0, 4];
                    AA[i, 5] = A(xxx, yyy)[0, 5];
                    AA[i * 2, 0] = A(xxx, yyy)[1, 0];
                    AA[i * 2, 1] = A(xxx, yyy)[1, 1];
                    AA[i * 2, 2] = A(xxx, yyy)[1, 2];
                    AA[i * 2, 3] = A(xxx, yyy)[1, 3];
                    AA[i * 2, 4] = A(xxx, yyy)[1, 4];
                    AA[i * 2, 5] = A(xxx, yyy)[1, 5];
                    i += 2;
                }
                double[,] XX = cheng(Transpose(AA), AA);
                XX = cheng(XX, Transpose(AA));
                XX = cheng(XX, L);

                xs0 = XS0;
                ys0 = YS0;
                zs0 = ZS0;
                varphi2 = varphi;
                omega2 = omega;
                kappa2 = kappa;

                XS0 = XS0 + XX[0, 0];
                YS0 = YS0 + XX[1, 0];
                ZS0 = ZS0 + XX[2, 0];
                varphi = varphi + XX[3, 0];
                omega = omega + XX[4, 0];
                kappa = kappa + XX[5, 0];

                x.Clear();
                y.Clear();
                for (int i = 0; i < yx.Length; i++)
                {
                    if (i % 2 == 0)
                    {
                        x.Add(yx[i, 0]);
                    }
                    else
                    {
                        y.Add(yx[i, 0]);
                    }
                }

                if((XS0-xs0)<=1)
                {
                    break;
                }
                else
                {
                    xs0 = XS0;
                    ys0 = YS0;
                    zs0 = ZS0;
                    varphi2 = varphi;
                    omega2 = omega;
                    kappa2 = kappa;
                }
            }

            //textBox1.Text = XS0.ToString();
            textBox1.Clear();
            textBox1.AppendText("外方位元素\r\n");
            textBox1.AppendText("XS0: " + XS0.ToString("0.000") + "  YS0: " 
                + YS0.ToString("0.000") + "  ZS0: " + ZS0.ToString("0.000") +"\r\n"+
                " φ: "+(varphi/Math.PI*180).ToString("0.000000")+"    Ω: "+(omega / Math.PI * 180).ToString("0.000000")+
                "    κ: "+(kappa / Math.PI * 180).ToString("0.000000")+"\r\n");
            savedata.Add("外方位元素");
            savedata.Add("XS0: " + XS0.ToString("0.000") + "  YS0: "
                + YS0.ToString("0.000") + "  ZS0: " + ZS0.ToString("0.000"));
            savedata.Add(" φ: " + (varphi / Math.PI * 180).ToString("0.000000") + "    Ω: " + (omega / Math.PI * 180).ToString("0.000000") +
                "    κ: " + (kappa / Math.PI * 180).ToString("0.000000"));

            textBox1.AppendText("\r\n"+"系数矩阵"+"\r\n");
            savedata.Add("");
            savedata.Add("系数矩阵");
            for (int i=0;i<AA.GetLength(0);i++)
            {
                string line = "";
                for(int j=0;j<AA.GetLength(1);j++)
                {
                    textBox1.AppendText(AA[i, j].ToString("0.000000000") + " ");
                    line += AA[i, j].ToString("0.000000000");
                }
                savedata.Add(line);
                line = "";
                textBox1.AppendText("\r\n");
            }
            textBox1.AppendText("\r\n" + "旋转矩阵" + "\r\n");
            savedata.Add("");
            savedata.Add("旋转矩阵");
            for (int i = 0; i < RR.GetLength(0); i++)
            {
                string line = "";
                for (int j = 0; j < RR.GetLength(1); j++)
                {
                    textBox1.AppendText((RR[i, j]).ToString("0.000000") + " ");
                    line += RR[i, j].ToString("0.000000");
                }
                savedata.Add(line);
                line = "";
                textBox1.AppendText("\r\n");
            }
        }

        //矩阵相减
        public static T[,] SubtractMatrices<T>(T[,] matrix1, T[,] matrix2)
        {
            int rows = matrix1.GetLength(0);
            int columns = matrix1.GetLength(1);

            if (rows != matrix2.GetLength(0) || columns != matrix2.GetLength(1))
            {
                throw new ArgumentException("矩阵的行列数必须相等");
            }

            T[,] result = new T[rows, columns];

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < columns; j++)
                {
                    dynamic value1 = matrix1[i, j];
                    dynamic value2 = matrix2[i, j];
                    result[i, j] = value1 - value2;
                }
            }

            return result;
        }

        private void saveToolStripMenuItem_Click(object sender, EventArgs e)
        {
            string FileName = "";
            SaveFileDialog saveFileDialog = new SaveFileDialog();
            saveFileDialog.InitialDirectory = Environment.GetFolderPath(Environment.SpecialFolder.Personal);
            saveFileDialog.Filter = "文本文件(*.txt)|*.txt|所有文件(*.*)|*.*";
            if (saveFileDialog.ShowDialog(this) == DialogResult.OK)
            {
                FileName = saveFileDialog.FileName;
            }
            if (FileName != "")
            {
                try
                {
                    using (StreamWriter sw = new StreamWriter(FileName))
                    {
                        for (int i = 0; i < savedata.Count(); i++)
                        {
                            sw.WriteLine(savedata[i]);
                        }
                    }
                }
                catch (Exception ex)
                {

                }
            }
        }
    }
}

输出结果