本文介绍了一种摄影测量中的后方交会算法实现过程，包括输入像点坐标、控制点坐标，计算外方位元素初值及迭代求解外方位元素等步骤。通过具体的C#代码示例展示了算法流程。
namespace ConsoleApplication1
{
class Program
{
static void Main(string[] args)
{
//输入比例尺，主距，参与平参点的个数
Console.WriteLine("请输入比例尺分母m：\r");
string m1 = Console.ReadLine();
double m = (double)Convert.ToSingle(m1);
Console.WriteLine("请输入主距f：\r");
string f1 = Console.ReadLine();
double f = (double)Convert.ToSingle(f1);
Console.WriteLine("请输入参与平差控制点的个数n：\r");
string n1 = Console.ReadLine();
int n = (int)Convert.ToSingle(n1);
//像点坐标的输入代码
double[] arr1 = new double[2 * n];
//1.像点x坐标的输入
for (int i = 0; i < n; i++)
{
Console.WriteLine("请输入已进行系统误差改正的像点坐标的x{0}值：\r", i + 1);
string u = Console.ReadLine();
for (int j = 0; j < n; j += 2)
{
arr1[j] = (double)Convert.ToSingle(u);
}
}
//2.像点y坐标的输入
for (int i = 0; i < n; i++)
{
Console.WriteLine("请输入已进行系统误差改正的像点坐标的y{0}值：\r", i + 1);
string v = Console.ReadLine();
for (int j = 1; j < n; j += 2)
{
arr1[j] = (double)Convert.ToSingle(v);
}
}
//控制点的坐标输入代码
double[,] arr2 = new double[n, 3];
//1.控制点X坐标的输入
for (int j = 0; j < n; j++)
{
Console.WriteLine("请输入控制点在地面摄影测量坐标系的坐标的X{0}值：\r", j + 1);
string u = Console.ReadLine();
arr2[j, 0] = (double)Convert.ToSingle(u);
}
//2.控制点Y坐标的输入
for (int k = 0; k < n; k++)
{
Console.WriteLine("请输入控制点在地面摄影测量坐标系的坐标的Y{0}值：\r", k + 1);
string v = Console.ReadLine();
arr2[k, 1] = (double)Convert.ToSingle(v);
}
//3.控制点Z坐标的输入
for (int p = 0; p < n; p++)
{
Console.WriteLine("请输入控制点在地面摄影测量坐标系的坐标的Z{0}值：\r", p + 1);
string w = Console.ReadLine();
arr2[p, 2] = (double)Convert.ToSingle(w);
}
//确定外方位元素的初始值
//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("Xs0,Ys0,Zs0,Φ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++)
{
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 vv = 0; vv < 6; vv++)
{
arrX[i] += arr5AARAT[i, vv] * arr7[vv];
}
}
}
//计算外方位元素值
double Xs, Ys, Zs, Φ, Ψ, K;
Xs = Xs0 + arrX[0];
Ys = Ys0 + arrX[1];
Zs = Zs0 + arrX[2];
Φ = Φ0 + arrX[3];
Ψ = Ψ0 + arrX[4];
K = K0 + arrX[5];
for (int i = 0; i <= 2; i++)
{
Xs += arrX[0];
Ys += arrX[1];
Zs += arrX[2];
Φ += arrX[3];
Ψ += arrX[4];
K += arrX[5];
}
Console.WriteLine("Xs,Ys,Zs,Φ,Ψ,K的值分别是：{0},{1},{2},{3},{4},{5}", Xs0, Ys0, Zs0, Φ, Ψ, K);
Console.Read();
}
//求arr5AA的逆矩
public static double[,] nijuzhen(double[,] a)
{
double[,] B = new double[6, 6];
int i, j, k;
int row = 0;
int col = 0;
double max, temp;
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; row = col = i;
for (i = k; i < 6; i++)
{
for (j = k; j < 6; j++)
{
temp = Math.Abs(a[i, j]);
if (max < temp)
{
max = temp;
row = i;
col = j;
}
}
}
//交换行列，将主元调整到k行k列上
if (row != k)
{
for (j = 0; j < 6; j++)
{
temp = a[row, j];
a[row, j] = a[k, j];
a[k, j] = temp;
temp = B[row, j];
B[row, j] = B[k, j];
B[k, j] = temp;
}
i = p[row]; p[row] = p[k]; p[k] = i;
}
if (col != k)
{
for (i = 0; i < 6; i++)
{
temp = a[i, col];
a[i, col] = a[i, k];
a[i, k] = temp;
}
}
//处理
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;
}
}
}
摄影测量学后方交会，遗憾没有人性化的的对象界面
扫一扫
2108
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
