单张像片后方交会

using System.Diagnostics;
using System.Reflection;
using System.Text;
using System.Windows.Forms;
using MathNet.Numerics.LinearAlgebra;
using 后方交会.Properties;

namespace 后方交会
{
    public partial class Form1 : Form
    {
        List<Data> allPoints = new List<Data>();
        string[] all_lines;
        double f;
        double x0;
        double y0;
        double m;
        Matrix<double> R = Matrix<double>.Build.Dense(3, 3);
        const double thresholdRadians = 6.0 / 3600.0 * Math.PI / 180.0;
        public Form1()
        {
            InitializeComponent();
        }

        private void 打开ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            OpenFileDialog op = new OpenFileDialog();
            if (op.ShowDialog() == DialogResult.OK)
            {
                all_lines = File.ReadAllLines(op.FileName, Encoding.Default);
            }
            else
            {
                return;
            }
            try
            {
                string[] parts = all_lines[0].Split(',');
                f = double.Parse(parts[0]);
                x0 = double.Parse(parts[1]);
                y0 = double.Parse(parts[2]);
                m = double.Parse(parts[3]);
                for (int i = 1; i < all_lines.Length; i++)
                {
                    string[] others = all_lines[i].Split(',');
                    Data p = new Data()
                    {
                        num = int.Parse(others[0]),
                        x = double.Parse(others[1]),
                        y = double.Parse(others[2]),
                        X = double.Parse(others[3]),
                        Y = double.Parse(others[4]),
                        Z = double.Parse(others[5])
                    };
                    allPoints.Add(p);
                    dataGridView1.Rows.Add(others[0], others[1], others[2], others[3], others[4], others[5]);
                }
                toolStripStatusLabel1.Text = "导入成功";
                toolStripStatusLabel2.Text = $"f: {f}mm, x0: {x0}mm, y0: {y0}mm, m: {parts[3]}";
            }
            catch
            {
                MessageBox.Show("文件导入失败!");
                toolStripStatusLabel1.Text = "导入失败";
                return;
            }
        }


        private void 计算ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            #region 初值
            double Xs = allPoints.Average(data => data.X);
            double Ys = allPoints.Average(data => data.Y);
            double Zs = allPoints.Average(data => data.Z);
            f /= 1000;
            double H = m * f;
            Zs += H;
            double phi = 0;
            double omega = 0;
            double kappa = 0;



            double dXs = 0;
            double dYs = 0;
            double dZs = 0;
            double dphi = 0;
            double domege = 0;
            double dkappa = 0;
            #endregion

            #region 迭代
            while (true)
            {

                Matrix<double> Vmatrix = Matrix<double>.Build.Dense(allPoints.Count * 2, 1);
                Matrix<double> Xmatrix = Matrix<double>.Build.Dense(6, 1);
                Matrix<double> Lmatrix = Matrix<double>.Build.Dense(allPoints.Count * 2, 1);

                CreateMatrixR(phi, omega, kappa);

                for (int i = 0; i < allPoints.Count; i++)
                {

                    Data data = allPoints[i];

                    double lx = (data.x / 1000) + f * (R[0, 0] * (data.X - Xs) + R[1, 0] * (data.Y - Ys) + R[2, 0] * (data.Z - Zs)) / (R[0, 2] * (data.X - Xs) + R[1, 2] * (data.Y - Ys) + R[2, 2] * (data.Z - Zs));
                    double ly = (data.y / 1000) + f * (R[0, 1] * (data.X - Xs) + R[1, 1] * (data.Y - Ys) + R[2, 1] * (data.Z - Zs)) / (R[0, 2] * (data.X - Xs) + R[1, 2] * (data.Y - Ys) + R[2, 2] * (data.Z - Zs));
                    double x = -f * (R[0, 0] * (data.X - Xs) + R[1, 0] * (data.Y - Ys) + R[2, 0] * (data.Z - Zs)) / (R[0, 2] * (data.X - Xs) + R[1, 2] * (data.Y - Ys) + R[2, 2] * (data.Z - Zs));
                    double y = -f * (R[0, 1] * (data.X - Xs) + R[1, 1] * (data.Y - Ys) + R[2, 1] * (data.Z - Zs)) / (R[0, 2] * (data.X - Xs) + R[1, 2] * (data.Y - Ys) + R[2, 2] * (data.Z - Zs));
                    Matrix<double> Amatrix = CreateAMatrix(f, H, x, y);
                    double vx = Amatrix[0, 0] * dXs + Amatrix[0, 1] * dYs + Amatrix[0, 2] * dZs + Amatrix[0, 3] * dphi + Amatrix[0, 4] * domege + Amatrix[0, 5] * dkappa - lx;
                    double vy = Amatrix[1, 0] * dXs + Amatrix[1, 1] * dYs + Amatrix[1, 2] * dZs + Amatrix[1, 3] * dphi + Amatrix[1, 4] * domege + Amatrix[1, 5] * dkappa - ly;

                    Vmatrix[2 * i, 0] = vx;
                    Vmatrix[2 * i + 1, 0] = vy;

                    Lmatrix[2 * i, 0] = lx;
                    Lmatrix[2 * i + 1, 0] = ly;

                }

                Matrix<double> AMatrix = Matrix<double>.Build.Dense(allPoints.Count * 2, 6);
                for (int i = 0; i < allPoints.Count; i++)
                {
                    Data data = allPoints[i];
                    double x = -f * (R[0, 0] * (data.X - Xs) + R[1, 0] * (data.Y - Ys) + R[2, 0] * (data.Z - Zs)) / (R[0, 2] * (data.X - Xs) + R[1, 2] * (data.Y - Ys) + R[2, 2] * (data.Z - Zs));
                    double y = -f * (R[0, 1] * (data.X - Xs) + R[1, 1] * (data.Y - Ys) + R[2, 1] * (data.Z - Zs)) / (R[0, 2] * (data.X - Xs) + R[1, 2] * (data.Y - Ys) + R[2, 2] * (data.Z - Zs));
                    Matrix<double> Amatrix = CreateAMatrix(f, H, x, y);
                    for (int j = 0; j < 6; j++)
                    {
                        AMatrix[2 * i, j] = Amatrix[0, j];
                        AMatrix[2 * i + 1, j] = Amatrix[1, j];
                    }
                }

                Matrix<double> AT = AMatrix.Transpose();
                Matrix<double> ATA = AT * AMatrix;
                Matrix<double> ATAInv = ATA.Inverse();
                Xmatrix = ATAInv * AT * Lmatrix;

                // 更新参数
                dXs = Xmatrix[0, 0];
                dYs = Xmatrix[1, 0];
                dZs = Xmatrix[2, 0];
                dphi = Xmatrix[3, 0];
                domege = Xmatrix[4, 0];
                dkappa = Xmatrix[5, 0];
                Xs += dXs;
                Ys += dYs;
                Zs += dZs;
                phi += dphi;
                omega += domege;
                kappa += dkappa;
                if (Math.Abs(dphi) < thresholdRadians && Math.Abs(domege) < thresholdRadians && Math.Abs(dkappa) < thresholdRadians)
                {
                    break;
                }

            }
            #endregion

            #region 输出
            string re = "";
            re += "相片的外方位元素为：" + "\r\n";
            re += "Xs(m)=" + Xs.ToString("F6") + "\r\n";
            re += "Ys(m)=" + Ys.ToString("F6") + "\r\n";
            re += "Zs(m)=" + Zs.ToString("F6") + "\r\n";
            re += "phi(rad)=" + phi.ToString("F6") + "\r\n";
            re += "omega(rad)=" + omega.ToString("F6") + "\r\n";
            re += "kappa(rad)=" + kappa.ToString("F6") + "\r\n";
            richTextBox1.AppendText(re);
            toolStripStatusLabel1.Text = "计算成功！";
            #endregion
        }


        //系数矩阵
        public Matrix<double> CreateAMatrix(double f, double H, double x, double y)
        {
            Matrix<double> A = Matrix<double>.Build.Dense(2, 6);

            A[0, 0] = -f / H;
            A[0, 1] = 0;
            A[0, 2] = -x / H;
            A[0, 3] = -f * (1 + x * x / (f * f));
            A[0, 4] = -x * y / f;
            A[0, 5] = y;

            A[1, 0] = 0;
            A[1, 1] = -f / H;
            A[1, 2] = -y / H;
            A[1, 3] = -x * y / f;
            A[1, 4] = -f * (1 + y * y / (f * f));
            A[1, 5] = -x;

            return A;
        }

        //R
        private void CreateMatrixR(double phi, double omega, double k)
        {

            // 计算并填充矩阵R的元素
            R[0, 0] = Math.Cos(phi) * Math.Cos(k) - Math.Sin(phi) * Math.Sin(omega) * Math.Sin(k);
            R[0, 1] = -Math.Cos(phi) * Math.Sin(k) - Math.Sin(phi) * Math.Sin(omega) * Math.Cos(k);
            R[0, 2] = -Math.Sin(phi) * Math.Cos(omega);

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

            R[2, 0] = Math.Sin(phi) * Math.Cos(k) + Math.Cos(phi) * Math.Sin(omega) * Math.Sin(k);
            R[2, 1] = -Math.Sin(phi) * Math.Sin(k) + Math.Cos(phi) * Math.Sin(omega) * Math.Cos(k);
            R[2, 2] = Math.Cos(phi) * Math.Cos(omega);

        }

        private void 保存ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            SaveFileDialog saveFileDialog = new SaveFileDialog();
            saveFileDialog.Filter = "Text Files (*.txt)|*.txt|All Files (*.*)|*.*";
            saveFileDialog.DefaultExt = "txt";
            saveFileDialog.AddExtension = true;

            if (saveFileDialog.ShowDialog() == DialogResult.OK)
            {
                using (StreamWriter sw = new StreamWriter(saveFileDialog.FileName))
                {
                    sw.Write(richTextBox1.Text);
                }
            }
            toolStripStatusLabel1.Text = "保存成功！";

        }

        private void 退出ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Application.Exit();
        }

        private void 新建ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            string fileName = "示例.txt";
            string fileContent = "f,x0,y0,m\n点号,x,y,X,Y,Z";

            using (StreamWriter sw = new StreamWriter(fileName))
            {
                sw.WriteLine(fileContent);
            }

            
            toolStripStatusLabel1.Text = "已新建示例文件！";

        }

        private void 清除ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            allPoints.Clear();

            all_lines = new string[0];

            f = 0;
            x0 = 0;
            y0 = 0;
            m = 0;

            R.Clear();
            dataGridView1.Rows.Clear();
            richTextBox1.Clear();
            toolStripStatusLabel1.Text = "清除成功！";

        }

        private void 重启ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Application.Exit();
            Process.Start(Application.ExecutablePath);
        }

        private void 工具栏ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            工具栏ToolStripMenuItem.Checked = !工具栏ToolStripMenuItem.Checked;
            toolStrip1.Visible = 工具栏ToolStripMenuItem.Checked;
            toolStripStatusLabel1.Text = "工具栏更改成功！";

        }

        private void 状态栏ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            状态栏ToolStripMenuItem.Checked = !状态栏ToolStripMenuItem.Checked;
            statusStrip1.Visible = 状态栏ToolStripMenuItem.Checked;
            toolStripStatusLabel1.Text = "状态栏更改成功！";

        }

        private void 帮助ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            string feedbackMessage = "请查看帮助文件help.pdf";
            MessageBox.Show(feedbackMessage, "帮助信息", MessageBoxButtons.OK, MessageBoxIcon.Information);
        }

        private void 反馈ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            string feedbackMessage = "如有任何问题或建议，请联系";
            MessageBox.Show(feedbackMessage, "反馈信息", MessageBoxButtons.OK, MessageBoxIcon.Information);
        }
    }
}