- 界面设计
- 控制点
PID,x(mm),y(mm),X(m),Y(m),Z(m)
1,-86.15,-68.99,36589.41,25273.32,2195.17
2,-53.4,82.21,37631.08,31324.51,728.69
3,-14.78,-76.63,39100.97,24934.98,2386.5
4,10.46,64.43,40426.54,30319.81,757.31
- 导入数据
OpenFileDialog dialog = new OpenFileDialog();
dialog.Filter = "文本文件.txt|*.txt";
if (dialog.ShowDialog() == DialogResult.OK)
{
path = Path.GetFullPath(dialog.FileName);
//从路径中获取文件名
}
string data;
DataTable dt = new DataTable();
StreamReader reader = new StreamReader(path);
while (true)
{
data = reader.ReadLine();
if (data == null)
break;
string[] v = data.Split(new string[] { "," }, StringSplitOptions.RemoveEmptyEntries);
if (data.Contains("PID"))
{
dt.Columns.Add(v[0]);
dt.Columns.Add(v[1]);
dt.Columns.Add(v[2]);
dt.Columns.Add(v[3]);
dt.Columns.Add(v[4]);
dt.Columns.Add(v[5]);
continue;
}
DataRow dr = dt.NewRow();
MyPoint p = new MyPoint();
p.id = Convert.ToInt32(v[0]);
p.x = (Convert.ToDouble(v[1])-x0) / 1000;//减去内方位元素
p.y = (Convert.ToDouble(v[2])-y0) / 1000;
p.X = Convert.ToDouble(v[3]);
p.Y = Convert.ToDouble(v[4]);
p.Z = Convert.ToDouble(v[5]);
dr[0] = p.id;
dr[1] = p.x;
dr[2] = p.y;
dr[3] = p.X;
dr[4] = p.Y;
dr[5] = p.Z;
dt.Rows.Add(dr);
ConPoint.Add(p);
}
n = ConPoint.Count;
dataGridView1.DataSource = dt;
4.迭代计算
double error = Convert.ToDouble(txterror.Text);
//外方位元素赋初值
double sumx = 0, sumy = 0;
for (int i = 0; i < ConPoint.Count; i++)
{
sumx += ConPoint[i].X;
sumy += ConPoint[i].Y;
}
Xs = sumx / n;
Ys = sumy / n;
Zs = m * f;
//旋转矩阵
double[,] R = new double[3, 3];
//近似(x)(y)
double[] xAppro = new double[n];
double[] yAppro = new double[n];
double[] XX = new double[n];
double[] YY = new double[n];
double[] ZZ = new double[n];
double[,] V = new double[2 * n, 1];
double[,] A = new double[2 * n, 6];
double[,] X = new double[6, 1];
double[,] L = new double[2 * n, 1];
int num = 0;
do
{
num++;
R[0, 0] = Math.Cos(phi) * Math.Cos(kappa) - Math.Sin(phi) * Math.Sin(omega) * Math.Sin(kappa);//a1
R[0, 1] = -Math.Cos(phi) * Math.Sin(kappa) - Math.Sin(phi) * Math.Sin(omega) * Math.Cos(kappa);//a2
R[0, 2] = -Math.Sin(phi) * Math.Cos(omega);//a3
R[1, 0] = Math.Cos(omega) * Math.Sin(kappa);//b1
R[1, 1] = Math.Cos(omega) * Math.Cos(kappa);//b2
R[1, 2] = -Math.Sin(omega);//b3
R[2, 0] = Math.Sin(phi) * Math.Cos(kappa) + Math.Cos(phi) * Math.Sin(omega) * Math.Sin(kappa);//c1
R[2, 1] = -Math.Sin(phi) * Math.Sin(kappa) + Math.Cos(phi) * Math.Sin(omega) * Math.Cos(kappa);//c2
R[2, 2] = Math.Cos(phi) * Math.Cos(omega);//c3
//计算(x)(y),n个控制点的
for (int i = 0; i < n; i++)
{
XX[i] = R[0, 0] * (ConPoint[i].X - Xs) + R[1, 0] * (ConPoint[i].Y - Ys) + R[2, 0] * (ConPoint[i].Z - Zs);
YY[i] = R[0, 1] * (ConPoint[i].X - Xs) + R[1, 1] * (ConPoint[i].Y - Ys) + R[2, 1] * (ConPoint[i].Z - Zs);
ZZ[i] = R[0, 2] * (ConPoint[i].X - Xs) + R[1, 2] * (ConPoint[i].Y - Ys) + R[2, 2] * (ConPoint[i].Z - Zs);
xAppro[i] = -f * XX[i] / ZZ[i];
yAppro[i] = -f * YY[i] / ZZ[i];
}
//计算A L矩阵
for (int i = 0; i < n; i++)
{
A[2 * i, 0] = (R[0, 0] * f + R[0, 2] * ConPoint[i].x) / ZZ[i];
A[2 * i, 1] = (R[1, 0] * f + R[1, 2] * ConPoint[i].x) / ZZ[i];
A[2 * i, 2] = (R[2, 0] * f + R[2, 2] * ConPoint[i].x) / ZZ[i];
A[2 * i, 3] = ConPoint[i].y * Math.Sin(omega) - (ConPoint[i].x * (ConPoint[i].x * Math.Cos(kappa) - ConPoint[i].y * Math.Sin(kappa)) / f + f * Math.Cos(kappa)) * Math.Cos(omega);
A[2 * i, 4] = -f * Math.Sin(kappa) - ConPoint[i].x * (ConPoint[i].x * Math.Sin(kappa) + ConPoint[i].y * Math.Cos(kappa)) / f;
A[2 * i, 5] = ConPoint[i].y;
A[2 * i + 1, 0] = (R[0, 1] * f + R[0, 2] * ConPoint[i].y) / ZZ[i];
A[2 * i + 1, 1] = (R[1, 1] * f + R[1, 2] * ConPoint[i].y) / ZZ[i];
A[2 * i + 1, 2] = (R[2, 1] * f + R[2, 2] * ConPoint[i].y) / ZZ[i];
A[2 * i + 1, 3] = -ConPoint[i].x * Math.Sin(omega) - (ConPoint[i].y * (ConPoint[i].x * Math.Cos(kappa) - ConPoint[i].y * Math.Sin(kappa)) / f - f * Math.Sin(kappa)) * Math.Cos(omega);
A[2 * i + 1, 4] = -f * Math.Cos(kappa) - ConPoint[i].y * (ConPoint[i].x * Math.Sin(kappa) + ConPoint[i].y * Math.Cos(kappa)) / f;
A[2 * i + 1, 5] = -ConPoint[i].x;
L[2 * i, 0] = ConPoint[i].x - xAppro[i];
L[2 * i + 1, 0] = ConPoint[i].y - yAppro[i];
}
//矩阵运算求解
MatrixO matrixw = new MatrixO();
double[,] AT = matrixw.MatrixTrans(A);
double[,] ATA = matrixw.Matrixmulti(AT, A);
double[,] Inv = matrixw.MatrixInvByCom(ATA);
double[,] InvAT = matrixw.Matrixmulti(Inv, AT);
X = matrixw.Matrixmulti(InvAT, L);
Xs += X[0, 0];
Ys += X[1, 0];
Zs += X[2, 0];
phi += X[3, 0];
omega += X[4, 0];
kappa += X[5, 0];
}
while (Math.Abs(X[0, 0]) >= error || Math.Abs(X[1, 0]) >= error || Math.Abs(X[2, 0]) >= error || Math.Abs(X[3, 0]) >= error || Math.Abs(X[4, 0]) >= error || Math.Abs(X[5, 0]) >= error);
//整理输出内容
MatrixO matrix1 = new MatrixO();
V = matrix1.MatrixSub(matrix1.Matrixmulti(A, X), L);
double[,] at = matrix1.MatrixTrans(A);
double[,] ata = matrix1.Matrixmulti(at, A);
double[,] inv = matrix1.MatrixInvByCom(ata);
double m0 = Math.Sqrt((matrix1.Matrixmulti(matrix1.MatrixTrans(V), V)[0,0]) / (2 * Convert.ToDouble(n) - 6));//中误差
double[] mi = new double[6];
for (int i = 0; i < 6; i++)
{
mi[i] = m0 * Math.Sqrt(inv[i, i]);
}
txtXs.Text = Math.Round(Xs, 4).ToString();
txtYs.Text = Math.Round(Ys, 4).ToString();
txtZs.Text = Math.Round(Zs, 4).ToString();
RadianAngle angle = new RadianAngle();
txtPhi.Text = angle.TransArcToDMS(phi);
txtOmega.Text = angle.TransArcToDMS(omega);
txtKappa.Text = angle.TransArcToDMS(kappa);
labnum.Text = "迭代次数"+num.ToString();
//写出到文件
StreamWriter writer = new StreamWriter(@"C: \Userdata\结果.txt");
writer.WriteLine("像片外方位元素为:");
writer.WriteLine("Xs={0} m={1}", Xs, mi[0]);
writer.WriteLine("Ys={0} m={1}", Ys, mi[1]);
writer.WriteLine("Zs={0} m={1}", Zs, mi[2]);
writer.WriteLine("φ={0} m={1}", phi, mi[3]);
writer.WriteLine("ω={0} m={1}", omega, mi[4]);
writer.WriteLine("κ={0} m={1}", kappa, mi[5]);
writer.WriteLine();
writer.WriteLine("旋转矩阵为:");
writer.WriteLine("{0} {1} {2}", R[0, 0], R[0, 1], R[0, 2]);
writer.WriteLine("{0} {1} {2}", R[1, 0], R[1, 1], R[1, 2]);
writer.WriteLine("{0} {1} {2}", R[2, 0], R[2, 1], R[2, 2]);
writer.Flush();
writer.Close();
5、矩阵运算类
//矩阵乘法
public double [,] Matrixmulti(double[,] A, double[,] B)
{
int m1 = A.GetLength(0);
int n1 = A.GetLength(1);
int m2 = B.GetLength(0);
int n2 = B.GetLength(1);
if (n1 != m2)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
double[,] c = new double[m1, n2];
double[,] a = A;
double[,] b = B;
for (int i = 0; i < m1; i++)
for (int j = 0; j < n2; j++)
{
c[i, j] = 0;
for (int k = 0; k < n1; k++)
c[i, j] += a[i, k] * b[k, j];
}
return c;
}
//矩阵乘法
public static double[,] MatrixMulti(double[,] A, double[,] B)
{
int m1 = A.GetLength(0);
int n1 = A.GetLength(1);
int m2 = B.GetLength(0);
int n2 = B.GetLength(1);
if (n1 != m2)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
double[,] c = new double[m1, n2];
double[,] a = A;
double[,] b = B;
for (int i = 0; i < m1; i++)
for (int j = 0; j < n2; j++)
{
c[i, j] = 0;
for (int k = 0; k < n1; k++)
c[i, j] += a[i, k] * b[k, j];
}
return c;
}
//矩阵转置
public double[,] MatrixTrans(double[,]A)
{
int m = A.GetLength(0);
int n = A.GetLength(1);
double[,] c = new double[n, m];
double[,] a = A;
for (int i = 0; i < n; i++)
for (int j = 0; j < m; j++)
c[i, j] = a[j, i];
return c;
}
//矩阵求逆
public double[,] MatrixInvByCom( double[,]A)
{
double d = MatrixO.MatrixDet(A);
if (d == 0)
{
Exception myException = new Exception("没有逆矩阵");
throw myException;
}
double[,] Ax = MatrixO.MatrixCom(A);
double[,] An = MatrixO.MatrixSimpleMulti((1.0 / d), Ax);
return An;
}
//对应行列式的代数余子式矩阵
public static double[,] MatrixSpa(double[,] A, int ai, int aj)
{
int m = A.GetLength(0);
int n = A.GetLength(1);
if (m != n)
{
Exception myException = new Exception("矩阵不是方阵");
throw myException;
}
int n2 = n - 1;
double[,] a = A;
double[,] b = new double[n2,n2];
//左上
for (int i = 0; i < ai; i++)
for (int j = 0; j < aj; j++)
{
b[i, j] = a[i, j];
}
//右下
for (int i = ai; i < n2; i++)
for (int j = aj; j < n2; j++)
{
b[i, j] = a[i + 1, j + 1];
}
//右上
for (int i = 0; i < ai; i++)
for (int j = aj; j < n2; j++)
{
b[i, j] = a[i, j + 1];
}
//左下
for (int i = ai; i < n2; i++)
for (int j = 0; j < aj; j++)
{
b[i, j] = a[i + 1, j];
}
//符号位
if ((ai + aj) % 2 != 0)
{
for (int i = 0; i < n2; i++)
b[i, 0] = -b[i, 0];
}
return b;
}
//矩阵的行列式,矩阵必须是方阵
public static double MatrixDet(double[,]A)
{
int m = A.GetLength(0);
int n = A.GetLength(1);
if (m != n)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
double[,] a = A;
if (n == 1) return a[0, 0];
double D = 0;
for (int i = 0; i < n; i++)
{
D += a[1, i] * MatrixDet(MatrixSpa(A, 1, i));
}
return D;
}
//矩阵的伴随矩阵
public static double[,] MatrixCom(double[,]A)
{
int m = A.GetLength(0);
int n = A.GetLength(1);
double[,] c = new double[m, n];
double[,] a = A;
for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++)
c[i, j] = MatrixDet(MatrixSpa(A, j, i));
return c;
}
-
结果