摄影外方位元素求解，结果为非数字，跪求高手
using System;using System.Collections.Generic;
using System.Text;
namespace ConsoleApplication8
{
class Program
{
public class data
{
double Xs, Ys, Zs, o=0, w=0, k=0;
double Xs0, Ys0, Zs0, m = 3000, f = 0.150;
//控制点坐标已给
double[] X = new double[3] { 100000.00, 100000.00, 106000.00 };
double[] Y = new double[3] { 137500.00, 142500.00, 137500.00 };
double[] Z = new double[3] { 11.00, 36.00, 42.00};
//对应像点坐标
double[] x = new double[3] { -91.596, -94.230, 95.207 };
double[] y = new double[3] { -74.859, 81.446, -75.521 };
double[] _x = new double[3];
double[] _y = new double[3];
double[] _z = new double[3];
//各矩阵
double[,] A = new double[6, 12];
double[,] M_A = new double[6, 6];
double[,] M_AA = new double[6, 6];
double[,] M_AAA = new double[6, 6];
double[,] M_AAAA = new double[6, 6];
double[,] M_AT = new double[6, 6];
double[,] R = new double[6, 1];
double[,] L = new double[6, 1];
double a1, a2, a3, b1, b2, b3, c1, c2, c3;
double p, q;
int i, j;
public void input()
{
for (i = 0; i < 3; i++)
{
p += X[i];
q += Y[i];
}
//初值
Zs0 = m * f;
Xs0 = p / 3;
Ys0 = q / 3;
}
public void M()
{
a1 = Math.Cos(o) * Math.Cos(k) - Math.Sin(o) * Math.Sin(w) * Math.Sin(k);
a2 = -Math.Cos(o) * Math.Sin(k) - Math.Sin(o) * Math.Sin(w) * Math.Cos(k);
a3 = -Math.Sin(o) * Math.Cos(w);
b1 = Math.Cos(w) * Math.Sin(k);
b2 = Math.Cos(w) * Math.Cos(k);
b3 = -Math.Sin(w);
c1 = Math.Sin(o) * Math.Cos(k) + Math.Cos(o) * Math.Sin(w) * Math.Sin(k);
c2 = -Math.Sin(o) * Math.Sin(k) + Math.Cos(o) * Math.Sin(w) * Math.Cos(k);
c3 = Math.Cos(o) * Math.Cos(w);
//求系数
for (i = 0; i < 3; i++)
{
_x[i] = (-f) * a1 * (X[i] - Xs0) + b1 * (Y[i] - Ys0) + c1 * (Z[i] - Zs0) / a3 * (X[i] - Xs0) + b3 * (Y[i] - Ys0) + c3 * (Z[i] - Zs0);
_y[i] = (-f) * a2 * (X[i] - Xs0) + b2 * (Y[i] - Ys0) + c2 * (Z[i] - Zs0) / a3 * (X[i] - Xs0) + b3 * (Y[i] - Ys0) + c3 * (Z[i] - Zs0);
_z[i] = a3 * (X[i] - Xs0) + b3 * (Y[i] - Ys0) + c3 * (Z[i] - Zs0);
M_A[i * 2, 0] = (a1 * f + a3 * x[i]) / _z[i];
M_A[i * 2, 1] = (b1 * f + b3 * x[i]) / _z[i];
M_A[i * 2, 2] = (c1 * f + c3 * x[i]) / _z[i];
M_A[i * 2, 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);
M_A[i * 2, 4] = -f * Math.Sin(k) - x[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
M_A[i * 2, 5] = y[i];
M_A[i * 2 + 1, 0] = (a2 * f + a3 * x[i]) / _z[i];
M_A[i * 2 + 1, 1] = (b2 * f + b3 * x[i]) / _z[i];
M_A[i * 2 + 1, 2] = (c2 * f + c3 * x[i]) / _z[i];
M_A[i * 2 + 1, 3] = x[i] * Math.Sin(w) - x[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) + f * Math.Sin(k) * Math.Cos(w);
M_A[i * 2 + 1, 4] = -f * Math.Cos(k) - y[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
M_A[i * 2 + 1, 5] = -x[i];
L[i * 2, 0] = x[i] - _x[i];
L[i * 2 + 1, 0] = y[i] - _y[i];
}
//求A的转置
for (i = 0; i < 6; i++)
for (j = 0; j < 6; j++)
M_AT[i, j] = M_A[j, i];
//把A和AT的乘积放在M_AA中
for (i = 0; i < 6; i++)
for (j = 0; j < 6; j++)
M_AA[i, j] += M_AT[i, j] * M_A[i, j];
// 求M_AA的逆矩阵
//定义A--临时数组
for (int s = 0; s < 6; s++)
{
for (int t = 0; t < 12; t++)
{
A[s, t] = 0;
}
}
//把M_AA各值赋给A
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
A[i, j] = M_AA[i, j];
}
}
/**/
////在A加入初等方阵
for (int v = 0; v < 6; v++)
{
A[v, v + 6] = 1;
}
//初等变换
for (int s = 0; s < 6; s++)
{
if (A[s, s] != 1)
{
double bs = A[s, s];
A[s, s] = 1;
for (int p = s + 1; p < 12; p++)
{
A[s, p] /= bs;
}
}
for (int q = 0; q < 6; q++)
{
if (q != s)
{
double bs = A[q, s];
for (int p = 0; p < 12; p++)
{
A[q, p] -= bs * A[s, p];
}
}
else
{
continue;
}
}
}
//把A的值赋给M_AAA
for (i = 0; i < 6; i++)
for (j = 0; j < 6; j++)
M_AAA[i, j] = A[i, j];
for (i = 0; i < 6; i++)
for (j = 0; j <6; j++)
M_AAAA[i, j] += M_AAA[i, j] * M_AT[j, i];
for (i = 0; i < 6; i++)
for (j = 0; j < 1; j++)
{
R[i, j] = 0;
for (int v = 0; v < 6; v++)
{
R[i, j] += M_AAAA[i, v] * L[v, j];
}
}
//外方位元素如下
Xs = R[0, 0];
Ys = R[1, 0];
Zs = R[2, 0];
o = R[3, 0];
w = R[4, 0];
k = R[5, 0];
}
public void output()
{
Console.WriteLine("外方位元素如下：");
Console.WriteLine("Xs={0}", Xs);
Console.WriteLine("Ys={0}", Ys);
Console.WriteLine("Zs={0}", Zs);
Console.WriteLine("o={0}", o);
Console.WriteLine("w ={0}", w);
Console.WriteLine("k={0}", k);
Console.ReadLine();
}
}
static void Main(string[] args)
{
data d = new data();
d.input();
d.M();
d.output();
}
}
}
--------------------编程问答-------------------- 能力不夠，路過幫頂下。。
补充：.NET技术 , C#
