摄影测量学空间后方交会实验报告

//确定外方位元素的初始值

//1.确定Xs的初始值： double Xs0 = 0; double sumx = 0;

for (int j = 0; j < n; j++) {

double h = arr2[j, 0]; sumx += h;

}

Xs0 = sumx / n; //2.确定Ys的初始值： double Ys0 = 0; double sumy = 0;

for (int j = 0; j < n; j++) {

double h = arr2[j, 1]; sumy += h;

}

Ys0 = sumy / n; //3.确定Zs的初始值： double Zs0 = 0; double sumz = 0;

for (int j = 0; j <= n - 1; j++) {

double h = arr2[j, 2]; sumz += h;

}

Zs0 = sumz / n; double Φ0 = 0; double Ψ0 = 0; double K0 = 0;

Console.WriteLine(\Φ0,Ψ0,K0的值分别是：{0},{1},{2},{3},{4},{5}\, Xs0, Ys0, Zs0, 0, 0, 0);

//用三个角元素的初始值按（3-4-5）计算各方向余弦值，组成旋转矩阵,此时的旋转矩阵为单位矩阵I：

double[,] arr3 = new double[3, 3]; for (int i = 0; i < 3; i++)

{

arr3[i, i] = 1; }

double a1 = arr3[0, 0]; double a2 = arr3[0, 1]; double a3 = arr3[0, 2]; double b1 = arr3[1, 0]; double b2 = arr3[1, 1]; double b3 = arr3[1, 2]; double c1 = arr3[2, 0]; double c2 = arr3[2, 1]; double c3 = arr3[2, 2];

/*利用线元素的初始值和控制点的地面坐标，代入共线方程（3-5-2）， * 逐点计算像点坐标的近似值*/ //1.定义存放像点近似值的数组

double[] arr4 = new double[2 * n];//----------近似值矩阵 //2.逐点像点坐标计算近似值 //a.计算像点的x坐标近似值（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)); } }

//b.计算像点的y坐标近似值（y） for (int i = 1; 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]; //------------系数矩阵（A） //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; //-f/H == -1/m }

//3.a.计算误差方程式Vx中dZs的系数 for (int i = 0; i < 2 * n; i += 2)

{

arr5[i, 2] = -arr1[i] / m * f; }

//3.b.计算误差方程式Vy中dZs的系数 for (int i = 1; i < 2 * n; i += 2) {

arr5[i, 2] = -arr1[i] / m * f; }

//4.a.计算误差方程式Vx中dΦ的系数 for (int i = 0; i < 2 * n; i += 2) {

arr5[i, 3] = -f * (1 + arr1[i] * arr1[i] / f * f); }

//4.a.计算误差方程式Vy中dΦ的系数 for (int i = 1; i < 2 * n; i += 2) {

arr5[i, 3] = -arr1[i - 1] * arr1[i] / f; }

//5.a.计算误差方程式Vx中dΨ的系数 for (int i = 0; i < 2 * n; i += 2) {

arr5[i, 4] = -arr1[i] * arr1[i + 1] / f; }

//5.b.计算误差方程式Vy中dΨ的系数 for (int i = 1; i < 2 * n; i += 2) {

arr5[i, 4] = -f * (1 + arr1[i] * arr1[i] / f * f); }

//6.a.计算误差方程式Vx中dk的系数 for (int i = 0; i < 2 * n; i += 2) {

arr5[i, 5] = arr1[i + 1]; }

//6.b.计算误差方程式Vy中dk的系数 for (int i = 1; i < 2 * n; i += 2) {

arr5[i, 5] = -arr1[i - 1]; }

//定义外方位元素组成的数组

double[] arr6 = new double[6];//--------------------外方位元素改正数矩阵（X） //定义常数项元素组成的数组

double[] arr7 = new double[2 * n];//-----------------常数矩阵（L） //计算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]; //将近似值矩阵的元素代入 }

/* 对于所有像点的坐标观测值，一般认为是等精度量测，所以权阵P为单位阵.

所以X=(ATA)-1ATL */ //1.计算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 l = 0; l < 2 * n; l++) {

arr5AA[i, j] += arr5T[i, l] * arr5[l, j]; } } }

nijuzhen(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++)