form1.cs

利用共线方程,用迭代法求解后方交会的参数

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


namespace WindowsApplication1
{
    public partial class Form1 : Form
    {

        #region//全局变量

        int i;
        int K;
        double ψ, ω, κ, m, Xs0, Ys0, Zs0, f, S1 = 0.0, S2 = 0.0, S3 = 0.0;
        double[] x = new double[4];//{ -86.15, -53.40, -14.78, 10.46 };
        double[] y = new double[4];// { -68.99, 82.21, -76.63, 64.43 };
        double[] X = new double[4];//{ 36589.41, 37631.08, 39100.97, 40426.54 };
        double[] Y = new double[4];// { 25273.32, 31324.51, 24934.98, 30319.81 };
        double[] Z = new double[4];// { 2195.17, 728.69, 2385.50, 757.31 };


        double[] H = new double[4];
        double[] I = new double[4];
      
        double[,] R = new double[3, 3];  

        double[] x_0 = new double[4];  //xy 的近似值
        double[] y_0 = new double[4];

        double[,] L = new double[6, 1];  //系数矩阵A ※ L
        double[,] A = new double[6, 6];
         
           //矩阵计算中间矩阵
        double[,] AT = new double[6, 6];
        double[,] AT_A = new double[6, 6];
        double[,] ATA_ni_AT = new double[6, 6];
        double[,] E = new double[6, 6];
        double[,] det = new double[6, 1];

        bool Iscontiue = true; //当次数超过500时 用于判断是否继续的bool值

        #endregion

        public Form1()
        {
            InitializeComponent();
        }

        private void button1_Click(object sender, EventArgs e)
        {
            try
            {
                for (int j = 0; j < 5; j++)
            {
                for (int i = 0; i < 3; i++)
                {
                    X[i] = Convert.ToDouble(dataGridView1[0, i].Value);
                    Y[i] = Convert.ToDouble(dataGridView1[1, i].Value);
                    Z[i] = Convert.ToDouble(dataGridView1[2, i].Value);
                    x[i] = (Convert.ToDouble(dataGridView1[3, i].Value));
                    y[i] = (Convert.ToDouble(dataGridView1[4, i].Value));

                }
            }

            #region// 初始化

            

                for (i = 0; i < 3; i++)
                {
                    S1 += X[i];
                    S2 += Y[i];
                    S3 += Z[i];
                }
                    //确定外方位元素的初始值
                ψ = 0;
                ω = 0;
                κ = 0;
                if (tb_m.Text == "" || tb_f.Text == "")
                {
                    MessageBox.Show("情输入m和f");
                    return;
                }
                m = Convert.ToDouble(tb_m.Text);
               // m = 50000;
                f = Convert.ToDouble(tb_f.Text);
               // f = 0.15324;
                Xs0 = S1 / 3.0;
                Ys0 = S2 / 3.0;
                Zs0 = m * f + S3 / 3.0;

                    //像片坐标的单位换算
                for (i = 0; i < 3; i++)
                {
                    x[i] /= 1000.0;
                    y[i] /= 1000.0;
                }

            }
            catch (Exception ex)
            {
                MessageBox.Show(this,"Error01" + ex.Message ," 初始化错误");
            }
            #endregion


               
            //////////////////////////////
            try
            {

                do
                {
                    K++;
                    cal_R();
                    cal_xy();
                    cal_A();
                    Matrix.Transpose(A, AT);

                    Matrix.Matrix6_6(AT, A, AT_A);

                    E = Matrix.ReverseMatrix(AT_A, 6);

                    Matrix.Matrix6_6(E, AT, ATA_ni_AT);

                    Matrix.Matrix6_1(ATA_ni_AT, L, det);

                    //计算外方位元素
                    Xs0 += det[0, 0];
                    Ys0 += det[1, 0];
                    Zs0 += det[2, 0];
                    ψ += det[3, 0];
                    ω += det[4, 0];
                    κ += det[5, 0];


                    listBox1.Items.Add(String.Format("第{0}次迭代",K));
                    listBox1.Items.Add("......");
                  
                    listBox1.Items.Add(String.Format( "    Xs0 =  {0}, Ys0={1}, Zs0={2}",Xs0,Ys0,Zs0));
                    listBox1.Items.Add(String.Format("   ψ =  {0},  ω={1}, κ={2}", ψ, ω, κ));
                    listBox1.Items.Add("......");

                    if (Math.Abs(det[0, 0]) <= 0.000029 && Math.Abs(det[1, 0]) <= 0.000029 && Math.Abs(det[2, 0]) <= 0.000029 && Math.Abs(det[3, 0]) <= 0.000029 && Math.Abs(det[4, 0]) <= 0.000029 && Math.Abs(det[5, 0]) <= 0.000029)
                    {
                        Result();

                        break;
                    }
                    if (K > 500 && Iscontiue == true)
                    {
                        if (MessageBox.Show(this, "已经超过500次\n是否继续", "提示", MessageBoxButtons.YesNo, MessageBoxIcon.Question, MessageBoxDefaultButton.Button2) == DialogResult.No)
                        {
                            Result();
                            break;
                        }
                        else if(K>1000)
                        {
                            MessageBox.Show("迭代超过1000次");
                            Result();
                            break;
                        }
                        else
                        {
                            Iscontiue = false;
                        }
                    }

                } while (true);
            }
            catch (Exception ex)
            {
                MessageBox.Show(this,"Error02 " + ex.Message,"迭代错误");
            }
            
        }
       
        public void cal_R()       
        { 
            

            /*
             
              |a1 a2 a3|
           R =|b1 b2 b3|
              |c1 c2 c3|
            
             
           */

            R[0, 0] = Math.Cos(ψ) * Math.Cos(κ) - Math.Sin(κ) * Math.Sin(ψ) * Math.Sin(ω); //a1
            R[0, 1] = -Math.Cos(ψ) * Math.Sin(κ) - Math.Cos(κ) * Math.Sin(ψ) * Math.Sin(ω);//a2
            R[0, 2] = -Math.Sin(ψ) * Math.Cos(ω);                                             //a3
            R[1, 0] = Math.Sin(κ) * Math.Cos(ω);                                                //b1
            R[1, 1] = Math.Cos(κ) * Math.Cos(ω);                                               //b2
            R[1, 2] = -Math.Sin(ω);                                   //b3 
            R[2, 0] = Math.Sin(ψ) * Math.Cos(κ) + Math.Sin(κ) * Math.Cos(ψ) * Math.Sin(ω);//c1
            R[2, 1] = -Math.Sin(ψ) * Math.Sin(κ) + Math.Cos(κ) * Math.Cos(ψ) * Math.Sin(ω);//c2
            R[2, 2] = Math.Cos(ψ) * Math.Cos(ω);         //c3
 
        }

         
        public void cal_xy()   //计算xy得近似值
        {

            for (i = 0; i <3; i++)
            {
                H[i] = R[0, 0] * (X[i] - Xs0) + R[1, 0] * (Y[i] - Ys0) + R[2, 0] * (Z[i] - Zs0);
                I[i] = R[0, 1] * (X[i] - Xs0) + R[1, 1] * (Y[i] - Ys0) + R[2, 1] * (Z[i] - Zs0);

                
                x_0[i] = -f * (H[i]) / (R[0, 2] * (X[i] - Xs0) + R[1, 2] * (Y[i] - Ys0) + R[2, 2] * (Z[i] - Zs0));
                y_0[i] = -f * (I[i]) / (R[0, 2] * (X[i] - Xs0) + R[1, 2] * (Y[i] - Ys0) + R[2, 2] * (Z[i] - Zs0));
                
                L[2 * i, 0] = x[i] - x_0[i];
                L[2 * i + 1, 0] = y[i] - y_0[i];
            }
        }

        
         public void cal_A()//计算系数矩阵A 
        {
             for (i = 0; i < 3; i++)
            {
                //系数矩阵
                A[2 * i, 0] = (R[0, 0] * f + R[0, 2] * x[i]) / (R[0, 2] * (X[i] - Xs0) + R[1, 2] * (Y[i] - Ys0) + R[2, 2] * (Z[i] - Zs0));
                A[2 * i, 1] = (R[1, 0] * f + R[1, 0] * x[i]) / (R[0, 2] * (X[i] - Xs0) + R[1, 2] * (Y[i] - Ys0) + R[2, 2] * (Z[i] - Zs0));
                A[2 * i, 2] = (R[2, 0] * f + R[2, 2] * x[i]) / (R[0, 2] * (X[i] - Xs0) + R[1, 2] * (Y[i] - Ys0) + R[2, 2] * (Z[i] - Zs0));
                A[2 * i, 3] = y[i] * Math.Sin(ω) - (x[i] * (x[i] * Math.Cos(κ) - y[i] * Math.Sin(κ)) / f + f * Math.Cos(κ)) * Math.Cos(ω);
                A[2 * i, 4] = -f * Math.Sin(κ) - x[i] * (x[i] * Math.Sin(κ) + y[i] * Math.Cos(κ)) / f;
                A[2 * i, 5] = y[i];
                A[2 * i + 1, 0] = (R[0, 1] * f + R[0, 2] * y[i]) / (R[0, 2] * (X[i] - Xs0) + R[1, 2] * (Y[i] - Ys0) + R[2, 2] * (Z[i] - Zs0));
                A[2 * i + 1, 1] = (R[1, 1] * f + R[1, 2] * y[i]) / (R[0, 2] * (X[i] - Xs0) + R[1, 2] * (Y[i] - Ys0) + R[2, 2] * (Z[i] - Zs0));
                A[2 * i + 1, 2] = (R[2, 1] * f + R[2, 2] * y[i]) / (R[0, 2] * (X[i] - Xs0) + R[1, 2] * (Y[i] - Ys0) + R[2, 2] * (Z[i] - Zs0));
                A[2 * i + 1, 3] = -x[i] * Math.Sin(ω) - (y[i] * (x[i] * Math.Cos(κ) - y[i] * Math.Sin(κ)) / f - f * Math.Sin(κ)) * Math.Cos(ω);
                A[2 * i + 1, 4] = -f * Math.Cos(κ) - y[i] * (x[i] * Math.Sin(κ) + y[i] * Math.Cos(κ)) / f;
                A[2 * i + 1, 5] = -x[i];
            }
        }
         
        
        public void Result()//结果输出
        {
            label1.Text = Xs0.ToString() + "   " + Ys0 + "   " + Zs0 + "    \n" + ψ + ω + κ + "         \n" + K;
            
            tb_xs0.Text = Xs0.ToString();
            tb_ys0.Text = Ys0.ToString();
            tb_zs0.Text = Zs0.ToString();

            tb_.Text = ψ.ToString();
            tb_k.Text = κ.ToString();
            tb_w.Text = ω .ToString();

            lab_k.Text = K.ToString();