求帮忙解决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;
}
}
}
上图是测试这个代码的坐标