【无标题】

求帮忙解决Index was outside the bounds of the array问题

在这里插入代码片using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
 
namespace HFJH
{
    class Program
    {
        public static void Main()//主方法
        {
            Console.WriteLine("输入参与平差控制点的个数");
            string str_n = Console.ReadLine();
            int n = (int)Convert.ToSingle(str_n);
            //内方位元素
            double f = 153.24, m = 50000;
            //外方位元素
            double Xs, Ys, Zs, u, w, k;
            //旋转矩阵各值
            double a1, a2, a3, b1, b2, b3, c1, c2, c3;
            //外方位元素初始值
            double Xs0 = 0;
            double Ys0 = 0;
            double Zs0 = 0;
            double u0, w0, k0;
            double[] arr1 = new double[2 * n];//像点坐标
                                              //输入x,y坐标
            for (int i = 0; i < n; i++)
            {
                Console.WriteLine("======= 输入第{0}个已知点坐标 =======", i + 1);
                Console.WriteLine("<像点坐标> x{0}(mm)", i + 1);
                string userInput_x = Console.ReadLine();
                for (int j = 0; j < n; j += 2)
                {
                    arr1[j] = (double)Convert.ToSingle(userInput_x);
                }
                Console.WriteLine("<像点坐标> y{0}(mm)", i + 1);
                string userInput_y = Console.ReadLine();
                for (int j = 0; j < n; j += 2)
                {
                    arr1[j] = (double)Convert.ToSingle(userInput_y);
                }
            }
            double[,] arr2 = new double[n, 3]; //地面点坐标
                                               //输入X,Y,Z坐标
            for (int j = 0; j < n; j++)
            {
                Console.WriteLine("======= 输入第{0}个已知点坐标 =======", j + 1);
                Console.WriteLine("<地面点坐标> X{0}(m)", j + 1);
                string userInput_X = Console.ReadLine();
                arr2[j, 0] = (double)Convert.ToSingle(userInput_X);
                Console.WriteLine("<地面点坐标> Y{0}(m)", j + 1);
                string userInput_Y = Console.ReadLine();
                arr2[j, 1] = (double)Convert.ToSingle(userInput_Y);
                Console.WriteLine("<地面点坐标> Z{0}(m)", j + 1);
                string userInput_Z = Console.ReadLine();
                arr2[j, 2] = (double)Convert.ToSingle(userInput_Z);
                Console.WriteLine();

            }
            //x,y值换算成m

            //定义初始值
            u0 = 0; w0 = 0; k0 = 0;
            //X,Y,Z和
            double SX = 0;
            double SY = 0;
            double SZ = 0;
            for (int j = 0; j < n; j++)
            {
                double h = arr2[j, 0];
                SX += h;
            }
            Xs0 = SX / n;
            for (int j = 0; j < n; j++)
            {
                double h = arr2[j, 1];
                SY += h;
            }
            Ys0 = SY / n;
            for (int j = 0; j < n; j++)
            {
                double h = arr2[j, 2];
                SZ += h;
            }
            Zs0 = SZ / n;
            Console.WriteLine("Xs0 , Ys0 , Zs0 , u0 , w0 ,k0 的值分别为:{0},{1},{2},{3},{4},{5}", Xs0, Ys0, Zs0, 0, 0, 0);
            //利用三个角元素的初始值计算各方向余弦值
            double[,] arr3 = new double[3, 3];
            for (int i = 0; i < 3; i++)
            {
                arr3[i, i] = 1;
            }

            a1 = arr3[0, 0]; a2 = arr3[0, 1]; a3 = arr3[0, 2];
            b1 = arr3[1, 0]; b2 = arr3[1, 1]; b3 = arr3[1, 2];
            c1 = arr3[2, 0]; c2 = arr3[2, 1]; c3 = arr3[2, 2];
            //定义存放像点近似值的数组
            double[] arr4 = new double[2 * n];
            //计算像点x坐标近似值
            for (int i = 0; i < 2 * n; i += 2)
            {
                for (int j = 0; j < n; j++)
                {
                    arr4[i] = -f * (a1 * (arr2[j, 0] - Xs0) + b1 * (arr2[j, 1] - Ys0) + c1 * (arr2[j, 2]
                  - Zs0)) / (a3 * (arr2[j, 0] - Xs0) + b3 * (arr2[j, 1] - Ys0) + c3 * (arr2[j, 2] - Zs0));
                }
            }
            //计算像点y坐标近似值
            for (int i = 0; i < 2 * n; i += 2)
            {
                for (int j = 0; j < n; j++)
                {
                    arr4[i] = -f * (a2 * (arr2[j, 0] - Xs0) + b2 * (arr2[j, 1] - Ys0) + c2 * (arr2[j, 2]
                 - Zs0)) / (a3 * (arr2[j, 0] - Xs0) + b3 * (arr2[j, 1] - Ys0) + c3 * (arr2[j, 2] - Zs0));
                }
            }

            double[,] arr5 = new double[2 * n, 6];
            //1.计算dXs
            for (int i = 0; i < 2 * n; i += 2)
            {
                arr5[i, 0] = -1 / m;      //f/H=-1/m
            }
            //2.计算dYs
            for (int i = 1; i < 2 * n; i += 2)
            {
                arr5[i, 1] = -1 / m;
            }
            //3.计算误差方程式Vx中的dZs的系数
            for (int i = 0; i < 2 * n; i += 2)
            {
                arr5[i, 2] = -arr1[i] / m * f;
            }
            //计算误差方程式Vy中的dZs的系数
            for (int i = 1; i < 2 * n; i += 2)
            {
                arr5[i, 2] = -arr1[i] / m * f;
            }
            //4.计算误差方程式Vx中的du的系数
            for (int i = 0; i < 2 * n; i += 2)
            {
                arr5[i, 3] = -f * (1 + arr1[i] * arr1[i] / f * f);
            }
            //计算误差方程式Vy中的du的系数
            for (int i = 1; i < 2 * n; i += 2)
            {
                arr5[i, 3] = -arr1[i - 1] * arr1[i] / f;
            }
            //5.计算误差方程式Vx中的dw的系数
            for (int i = 0; i < 2 * n; i += 2)
            {
                arr5[i, 4] = -arr1[i] * arr1[i + 1] / f;
            }
            //计算误差方程式Vy中的dw的系数
            for (int i = 1; i < 2 * n; i += 2)
            {
                arr5[i, 4] = -f * (1 + arr1[i] * arr1[i] / f * f);
            }
            //6.计算误差方程式Vx中的dk的系数
            for (int i = 0; i < 2 * n; i += 2)
            {
                arr5[i, 5] = arr1[i + 1];
            }
            //计算误差方程式Vy中的dk的系数
            for (int i = 1; i < 2 * n; i += 2)
            {
                arr5[i, 5] = -arr1[i - 1];
            }
            //定义外方位元素组成的数组
            double[] arr6 = new double[6];
            //定义常数项元素组成的数组
            double[] arr7 = new double[2 * n];
            //计算lx的值
            for (int i = 0; i < 2 * n; i += 2)
            {
                arr7[i] = arr1[i] - arr4[i];
            }
            //计算ly的值
            for (int i = 1; i < 2 * (n - 1); i += 2)
            {
                arr7[i] = arr1[i] - arr4[i];
            }
            //X = (ATL)-1ATL
            //计算AT
            double[,] arr5T = new double[6, 2 * n];
            for (int i = 0; i < 6; i++)
            {
                for (int j = 0; j < 2 * n; j++)
                {
                    arr5T[i, j] = arr5[j, i];
                }
            }
            //A的转置与A的乘积,存放在arr5AA中;
            double[,] arr5AA = new double[6, 6];
            for (int i = 0; i < 6; i++)
            {
                for (int j = 0; j < 6; j++)
                {
                    arr5AA[i, j] = 0;

                    for (int vv = 0; vv < 2 * n; vv++)
                    {
                        arr5AA[i, j] += arr5T[i, vv] * arr5[vv, j];
                    }
                }
            }
            NJZ(arr5AA);//arr5AA经过求逆变为原矩阵的逆矩阵

            //arr5AA*arr5T存放在arr5AARAT
            double[,] arr5AARAT = new double[6, 2 * n];
            for (int i = 0; i < 6; i++)
            {
                for (int j = 0; j < 2 * n; j++)
                {
                    arr5AARAT[i, j] = 0;

                    for (int p = 0; p < 6; p++)
                    {
                        arr5AARAT[i, j] += arr5AA[i, p] * arr5T[p, j];
                    }
                }
            }
            //arr5AARAT x L ,存放在arrX中
            double[] arrX = new double[6];
            for (int i = 0; i < 6; i++)
            {
                for (int j = 0; j < 1; j++)
                {
                    arrX[i] = 0;

                    for (int s = 0; s < 6; s++)
                    {
                        arrX[i] += arr5AARAT[i, s] * arr7[s];
                    }
                }
            }
            Xs = Xs0 + arrX[0];
            Ys = Ys0 + arrX[1];
            Zs = Zs0 + arrX[2];
            u = u0 + arrX[3];
            w = w0 + arrX[4];
            k = k0 + arrX[5];
            for (int i = 0; i < 3; i++)
            {
                Xs += arrX[0];
                Ys += arrX[1];
                Zs += arrX[2];
                u += arrX[3];
                w += arrX[4];
                k += arrX[5];
            }
            Console.WriteLine("Xs,Ys,Zs,u,w,k的值分别为:{0},{1},{2},{3},{4},{5}", Xs, Ys, Zs, u, w, k);
            Console.Read();
        }
        public static double[,] NJZ(double[,] a)
        {
            double[,] B = new double[6, 6];
            int i, j, k;
            int r = 0;
            int c = 0;
            double max, tp;
            int[] p = new int[6];
            for (i = 0; i < 6; i++)
            {
                p[i] = i;
                B[i, i] = 1;
            }
            for (k = 0; k < 6; k++)
            {
                //找主元
                max = 0; r = c = i;
                for (i = k; i < 6; i++)
                {
                    for (j = k; j < 6; j++)
                    {
                        tp = Math.Abs(a[i, j]);
                        if (max < tp)
                        {
                            max = tp;
                            r = i;
                            c = j;

                        }
                    }
                }
                //交换行列,将主元调整到K列k行上
                if (r != k)
                {
                    for (j = 0; j < 6; j++)
                    {
                        tp  = a[r, j];  
                        a[r, j] = a[k, j];
                        a[k, j] = tp;
                        tp = B[r, j];
                        B[r, j] = B[k, j];
                        B[k, j] = tp;
                    }
                    i = p[r];
                    p[r] = p[k];
                    p[k] = i;
                }
                if (c != k)
                {
                    for (i = 0; i < 6; i++)
                    {
                        tp = a[i, c];
                        a[i, c] = a[i, k];
                        a[i, k] = tp;
                    }
                }

                //处理
                for (j = k + 1; j < 6; j++)
                {
                    a[k, j] /= a[k, k];
                }
                for (j = 0; j < 6; j++)
                {
                    B[k, j] /= a[k, k];
                    a[k, k] = 1;
                }
                for (j = k + 1; j < 6; j++)
                {
                    for (i = 0; j < k; i++)
                    {
                        a[i, j] -= a[i, k] * a[k, j];
                    }
                    for (i = k + 1; i < 6; i++)
                    {
                        a[i, j] -= a[i, k] * a[k, j];
                    }
                }
                for (j = 0; j < 6; j++)
                {
                    for (i = 0; i < k; i++)
                    {
                        B[i, j] -= a[i, k] * B[k, j];
                    }
                    for (i = k + 1; i < 6; i++)
                    {
                        B[i, j] -= a[i, k] * B[k, j];
                    }
                }
                for (i = 0; i < 6; i++)
                {
                    a[i, k] = 0;
                    a[k, k] = 1;
                }
            }
            for (j = 0; j < 6; j++)
            {
                for (i = 0; i < 6; i++)
                {
                    a[p[i], j] = B[i, j];
                }
            }
            for (i = 0; i < 6; i++)
            {
                for (j = 0; j < 6; j++)
                {
                    a[i, j] = a[i, j];
                }
            }
            return a;
        }
    }
}

在这里插入图片描述上图是测试这个代码的坐标

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值