kongjianhoufangjiaohui.txt

giser--lu 的空间后方交会算法有写bug ,本人该过后上传

giser--lu 的空间后方交会算法有写bug ,本人该过后上传
using System;
using System.Collections.Generic;
using System.Text;

namespace _33
...{
    class Program
    ...{
        public class Data
        ...{
            //内方位元素
            double f = 0.15324, m = 50000,x0,y0;          
            //地面点坐标
            double[] X = new double[4];
            double[] Y = new double[4];
            double[] Z = new double[4];
            //X,Y,Z和
            double SX, SY, SZ;
            //像点坐标
            double[] x = new double[4];
            double[] y = new double[4];
            //x,y近似值
            double[] _x = new double[4];
            double[] _y = new double[4];
            //方便代入公式，定义_Z
            double[] _Z = new double[4];
            //外方位元素
            double XS, YS, ZS, r = 0, w = 0, k = 0;
            //旋转矩阵各值
            double a1, a2, a3, b1, b2, b3, c1, c2, c3;
            //解方程所用到矩阵
            double[,] matrix_A = new double[6, 6];
            double[,] matrix_AT = new double[6, 6];
            double[,] matrix_AA = new double[6, 6];
            double[,] matrix_AAtemp = new double[6, 12];
            double[,] matrix_AAR = new double[6, 6];
            double[,] matrix_AARAT = new double[6, 6];
            double[,] L = new double[6, 1];
            double[,] matrix_X = new double[6, 1];


            public void Input()    //坐标输入及数值初始化
            ...{
                //输入x,y,X,Y,Z坐标
                for (int i = 0; i < 3; i++)
                ...{                
                    Console.WriteLine("======= 输入第{0}个已知点坐标 =======",i+1);
                    Console.WriteLine("<像点坐标> x{0}(mm)",i+1);
                    string userInput_x = Console.ReadLine();
                    x[i] = double.Parse(userInput_x);
                    Console.WriteLine("<像点坐标> y{0}(mm)", i + 1);
                    string userInput_y = Console.ReadLine();
                    y[i] = double.Parse(userInput_y);
                    Console.WriteLine("<地面点坐标> X{0}(m)", i + 1);
                    string userInput_X = Console.ReadLine();
                    X[i] = double.Parse(userInput_X);
                    Console.WriteLine("<地面点坐标> Y{0}(m)", i + 1);
                    string userInput_Y = Console.ReadLine();
                    Y[i] = double.Parse(userInput_Y);

                    Console.WriteLine("<地面点坐标> Z{0}(m)", i + 1);
                    string userInput_Z = Console.ReadLine();
                    Z[i] = double.Parse(userInput_Z);
                    Console.WriteLine();
                    
                    //求X,Y,Z和
                   
                    SX += X[i];
                    SY += Y[i];
                    SZ += Z[i];
                }
                //x,y值换算成m
                for (int i = 0; i < 4; i++)
                ...{
                    x[i] /= 1000;
                    y[i] /= 1000;

                }
                //定义初始值
                x0 = y0 = 0;
                r = w = k = 0;
                XS = SX / 4;
                YS = SY / 4;
                ZS = SZ / 4 + m * f;
            }

            public void Matrix()//计算旋转矩阵R
              ...{
                    a1 = Math.Cos(r) * Math.Cos(k) - Math.Sin(r) * Math.Sin(w) * Math.Sin(k);
                    a2 = -Math.Cos(r) * Math.Sin(k) - Math.Sin(r) * Math.Sin(w) * Math.Cos(k);
                    a3 = -Math.Sin(r) * Math.Cos(w);
                    b1 = Math.Cos(w) * Math.Sin(k);
                    b2 = Math.Cos(w) * Math.Cos(k);
                    b3 = -Math.Sin(w);
                    c1 = Math.Sin(r) * Math.Cos(k) + Math.Cos(r) * Math.Sin(w) * Math.Sin(k);
                    c2 = -Math.Sin(r) * Math.Sin(k) + Math.Cos(r) * Math.Sin(w) * Math.Cos(k);
                    c3 = Math.Cos(r) * Math.Cos(w);

                    for (int i = 0; i < 3; i++)
                    ...{
                        //计算_Z
                        _Z[i] = a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS);

                        //计算x,y近似值
                        _x[i] = -f * (a1 * (X[i] - XS) + b1 * (Y[i] - YS) + c1 * (Z[i] - ZS)) / (a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS));
                        _y[i] = -f * (a2 * (X[i] - XS) + b2 * (Y[i] - YS) + c2 * (Z[i] - ZS)) / (a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS));
                    }

　　                //Matrix_A赋值
                    for (int i = 0; i < 3; i++)
                    ...{
                        matrix_A[2 * i, 0] = 1 / _Z[i] * (a1 * f + a3 * x[i]);
                        matrix_A[2 * i, 1] = 1 / _Z[i] * (b1 * f + b3 * x[i]);
                        matrix_A[2 * i, 2] = 1 / _Z[i] * (c1 * f + c3 * x[i]);
                        matrix_A[2 * i, 3] = y[i] * Math.Sin(w) - (x[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) + f * Math.Cos(k)) * Math.Cos(w);
                        matrix_A[2 * i, 4] = -f * Math.Sin(k) - x[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
                        matrix_A[2 * i, 5] = y[i];
                        matrix_A[2 * i + 1, 0] = 1 / _Z[i] * (c2 * f + c3 * y[i]);
                        matrix_A[2 * i + 1, 1] = 1 / _Z[i] * (b2 * f + b3 * y[i]);
                        matrix_A[2 * i + 1, 2] = 1 / _Z[i] * (c2 * f + c3 * y[i]);
                        matrix_A[2 * i + 1, 3] = -x[i] * Math.Sin(w) - (y[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) - f * Math.Sin(k)) * Math.Cos(w);
                        matrix_A[2 * i + 1, 4] = -f * Math.Cos(k) - y[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
                        matrix_A[2 * i + 1, 5] = -x[i];
                        L[2 * i, 0] = x[i] - _x[i];
                        L[2 * i + 1, 0] = y[i] - _y[i];
                    }

                    /**/////得到法方程的解

                    ////求matrix_A的转置
                    for (int i = 0; i < 6; i++)
                    ...{

                        for (int j = 0; j < 6; j++)
                        ...{

                            matrix_AT[i, j] = matrix_A[j, i];
                        }
                    }

                    /**/////A的转置与A的乘积,存在matrix_AA中

                    for (int i = 0; i < 6; i++)
                    ...{
                        for (int j = 0; j < 6; j++)
                        ...{
                            matrix_AA[i, j] = 0;

                            for (int vv = 0; vv < 6; vv++)
                            ...{
                                matrix_AA[i, j] += matrix_AT[i, vv] * matrix_A[vv, j];
                            }
                        }
                    }

                    //matrix_AA的逆阵

                    //定义matrix_AAtemp--临时数组

                    for (int vv = 0; vv < 6; vv++)
                    ...{
                        for (int t = 0; t < 12; t++)
                        ...{
                            matrix_AAtemp[vv, t] = 0;
                        }
                    }

                    //把matrix_AA各值赋给matrix_AAtemp

                    for (int i = 0; i < 6; i++)
                    ...{
                        for (int j = 0; j < 6; j++)
                        ...{
                            matrix_AAtemp[i, j] = matrix_AA[i, j];
                        }
                    }

                    /**/////在matrix_AAtemp加入初等方阵
                   
                    for (int v = 0; v < 6; v++)
                    ...{

                        matrix_AAtemp[v, v + 6] = 1;
                    }

                    //初等变换
                    for (int vv = 0; vv < 6; vv++)
                    ...{

                        if (matrix_AAtemp[vv, vv] != 1)
                        ...{

                            double bs = matrix_AAtemp[vv, vv];
                            matrix_AAtemp[vv, vv] = 1;
                            for (int p = vv + 1; p < 12; p++)
                            ...{
                                matrix_AAtemp[vv, p] /= bs;
                            }
                        }
                        for (int q = 0; q < 6; q++)
                        ...{
                            if (q != vv)