大三学期中的摄影测量实习
以下是C#窗体代码-以供同学们学习 (
相较于其他方法，解析空三的代码略微繁琐，一般有1000多行
数据来源原文链接：https://blog.csdn.net/sheyun1876/article/details/80581910
注意 ：像对2中703、702的顺序问题需要读者自行考虑
1、 基本数据
摄影机主距：f＝153.033mm bx＝200mm
2、 像对坐标数据（单位：微米）
像对1： 701 702
========================================================
1201 648790 735260 -230980 788550
1818 113860 595800 -771380 679190
1202 241050 586260 -644640 661950
1204 578030 223960 -327700 277590
1203 256140 187820 -648360 260160
1205 606230 -104550 -315660 -51440
1206 278340 -565020 -660020 -487200
1052 179220 -757800 -765430 -670400
410 478510 -637940 -466930 -569840
399 975930 -700850 16690 -658940
192 917380 -16160 -4600 18540
370 803150 758570 -76440 802960
1118 94870 709670 -785850 795830
194 35030 -34550 -877140 50930
-398 19790 -843710 -925660 -745370
像对2： 703 702
========================================================
400 -568240 -631500 426790 -634450
1207 -601170 642970 323920 637060
399 -980300 -630600 16690 -658940
192 -963100 51030 -4600 18540
370 -989450 836700 -76410 802880
1209 -8790 641530 921460 679700
401 -29060 -915900 981740 -884160
-190 1460 5490 965780 38900
像对3： 703 704
========================================================
1826 931930 729560 -26020 680090
402 907100 -785750 20330 -836630
1055 753660 -838360 -134160 -896670
411 397770 -725220 -500070 -791380
1211 49010 50160 -875490 -17320
188 918060 48330 -10150 11950
1225 890340 544420 -58540 499270
1210 624000 444520 -317940 392160
1209 -8690 641540 -945780 560530
401 -29020 -915940 -930070 -1004070
-190 1460 5460 -921980 -63410
3、控制点数据
点号 X坐标 Y坐标 Z坐标
==================================================
1201 24204.689 46604.652 46.251
1205 24343.683 45111.263 48.198
1206 24965.047 44309.253 49.340
1209 22167.904 46432.515 46.457
4、检查点数据
点号 X坐标 Y坐标 Z坐标
==================================================
1118 25192.533 46608.059 48.936
1202 24941.046 46375.998 46.539
1203 24945.705 45662.638 49.052
1204 24369.486 45700.421 49.020
1207 23218.292 46347.142 48.993
以下是Form1.cs程序：
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
namespace Analytic_aerial_triangulation
{
public partial class Form1 : Form
{
//基本数据
double f = 0.153033;
double bx = 0.2;
//矩阵计算模块已被我封装，名称为MatrixOperations.cs
MatrixOperations myMatrix = new MatrixOperations();
public Form1()
{
InitializeComponent();
}
void ReadData(int i)
{//数据读取函数
OpenFileDialog myOpenFileDialog = new OpenFileDialog();
myOpenFileDialog.Title = "打开txt文件对话框";
myOpenFileDialog.Filter = "txt文件|*.txt|所有文件|*.*";
if (myOpenFileDialog.ShowDialog() == DialogResult.OK)
{
switch (i)
{
case 1:
richTextBox1.LoadFile(myOpenFileDialog.FileName, RichTextBoxStreamType.PlainText);
break;
case 2:
richTextBox2.LoadFile(myOpenFileDialog.FileName, RichTextBoxStreamType.PlainText);
break;
case 3:
richTextBox3.LoadFile(myOpenFileDialog.FileName, RichTextBoxStreamType.PlainText);
break;
}
}
}
private void button1_Click(object sender, EventArgs e)
{//像对坐标数据存储
ReadData(1);
}
private void button2_Click(object sender, EventArgs e)
{//控制点数据存储
ReadData(2);
}
private void button3_Click(object sender, EventArgs e)
{//检查点数据存储
ReadData(3);
}
private void button4_Click(object sender, EventArgs e)
{
//先进行数据的初始化
//点号
double[] points1_Name = new double[15];
double[] points2_Name = new double[8];
double[] points3_Name = new double[11];
//像点坐标
double[] left1_x = new double[15]; double[] right1_x = new double[15];
double[] left1_y = new double[15]; double[] right1_y = new double[15];
double[] left2_x = new double[8]; double[] right2_x = new double[8];
double[] left2_y = new double[8]; double[] right2_y = new double[8];
double[] left3_x = new double[11]; double[] right3_x = new double[11];
double[] left3_y = new double[11]; double[] right3_y = new double[11];
//投影系数
double[] N1_1 = new double[15]; double[] N1_2 = new double[15];
double[] N2_1 = new double[8]; double[] N2_2 = new double[8];
double[] N3_1 = new double[11]; double[] N3_2 = new double[11];
//摄影基线分量
double[] bx = new double[3];
double[] by = new double[3];
double[] bz = new double[3];
//像空间辅助坐标系-像点
double[] LeftPoint1_fz_x = new double[15];
double[] LeftPoint1_fz_y = new double[15];
double[] LeftPoint1_fz_z = new double[15];
double[] LeftPoint2_fz_x = new double[8];
double[] LeftPoint2_fz_y = new double[8];
double[] LeftPoint2_fz_z = new double[8];
double[] LeftPoint3_fz_x = new double[11];
double[] LeftPoint3_fz_y = new double[11];
double[] LeftPoint3_fz_z = new double[11];
double[] RightPoint1_fz_y = new double[15];
double[] RightPoint2_fz_y = new double[8];
double[] RightPoint3_fz_y = new double[11];
//像空间辅助坐标系-模型点
double[] ModelPoint_1_x = new double[15];
double[] ModelPoint_1_y = new double[15];
double[] ModelPoint_1_z = new double[15];
double[] ModelPoint_2_x = new double[8];
double[] ModelPoint_2_y = new double[8];
double[] ModelPoint_2_z = new double[8];
double[] ModelPoint_3_x = new double[11];
double[] ModelPoint_3_y = new double[11];
double[] ModelPoint_3_z = new double[11];
//像对坐标数据读取
string[] line = richTextBox1.Text.Split('\n');
for (int i = 0; i < 15; i++)
{
string[] PointData1 = line[i].Split(' ');
points1_Name[i] = Convert.ToDouble(PointData1[0]);
left1_x[i] = Convert.ToDouble(PointData1[1]) / 1000000.0;
left1_y[i] = Convert.ToDouble(PointData1[2]) / 1000000.0;
right1_x[i] = Convert.ToDouble(PointData1[3]) / 1000000.0;
right1_y[i] = Convert.ToDouble(PointData1[4]) / 1000000.0;
}
for (int i = 15; i < 23; i++)
{
string[] PointData2 = line[i].Split(' ');
points2_Name[i - 15] = Convert.ToDouble(PointData2[0]);
left2_x[i - 15] = Convert.ToDouble(PointData2[1]) / 1000000.0;
left2_y[i - 15] = Convert.ToDouble(PointData2[2]) / 1000000.0;
right2_x[i - 15] = Convert.ToDouble(PointData2[3]) / 1000000.0;
right2_y[i - 15] = Convert.ToDouble(PointData2[4]) / 1000000.0;
}
for (int i = 23; i < 34; i++)
{
string[] PointData3 = line[i].Split(' ');
points3_Name[i - 23] = Convert.ToDouble(PointData3[0]);
left3_x[i - 23] = Convert.ToDouble(PointData3[1]) / 1000000.0;
left3_y[i - 23] = Convert.ToDouble(PointData3[2]) / 1000000.0;
right3_x[i - 23] = Convert.ToDouble(PointData3[3]) / 1000000.0;
right3_y[i - 23] = Convert.ToDouble(PointData3[4]) / 1000000.0;
}
double[,] roElements1 = new double[5, 1];
double[,] roElements2 = new double[5, 1];
double[,] roElements3 = new double[5, 1];
double[,] roElements_gaizheng = new double[15,1];
//确定相对定向元素的初始值
for (int i = 0; i < 5; i++)
{
roElements1[i, 0] = 0;
}
XDDX(15, left1_x, left1_y, right1_x, right1_y,roElements_gaizheng ,roElements1, ModelPoint_1_x, ModelPoint_1_y, ModelPoint_1_z, N1_1, N1_2, LeftPoint1_fz_x, LeftPoint1_fz_y, LeftPoint1_fz_z,RightPoint1_fz_y, ref bx[0], ref by[0], ref bz[0]);
for (int i = 0; i < 5; i++)
{
roElements2[i, 0] = roElements1[i, 0];
}
XDDX(8, left2_x, left2_y, right2_x, right2_y, roElements_gaizheng, roElements2, ModelPoint_2_x, ModelPoint_2_y, ModelPoint_2_z, N2_1, N2_2, LeftPoint2_fz_x, LeftPoint2_fz_y, LeftPoint2_fz_z, RightPoint2_fz_y, ref bx[1], ref by[1], ref bz[1]);
for (int i = 0; i < 5; i++)
{
roElements3[i, 0] = roElements2[i, 0];
}
XDDX(11, left3_x, left3_y, right3_x, right3_y, roElements_gaizheng, roElements3, ModelPoint_3_x, ModelPoint_3_y, ModelPoint_3_z, N3_1, N3_2, LeftPoint3_fz_x, LeftPoint3_fz_y, LeftPoint3_fz_z, RightPoint3_fz_y, ref bx[2], ref by[2], ref bz[2]);
//比例尺归化系数
double[] k = new double[3];
k[0] = 1;
double[] k1 = new double[3];
//找三个公共点点进行计算
k1[0] = (N1_1[9] * LeftPoint1_fz_z[9] - bz[0]) / (N2_1[2] * LeftPoint2_fz_z[2]);
k1[1] = (N1_1[10] * LeftPoint1_fz_z[10] - bz[0]) / (N2_1[3] * LeftPoint2_fz_z[3]);
k1[2] = (N1_1[11] * LeftPoint1_fz_z[11] - bz[0]) / (N2_1[4] * LeftPoint2_fz_z[4]);
k[1] = (k1[0] + k1[1] + k1[2]) / 3.0;
double[] k2 = new double[3];
k2[0] = (N2_1[5] * LeftPoint2_fz_z[5] - bz[1]) / (N3_1[8] * LeftPoint3_fz_z[8]);
k2[1] = (N2_1[6] * LeftPoint2_fz_z[6] - bz[1]) / (N3_1[9] * LeftPoint3_fz_z[9]);
k2[2] = (N2_1[7] * LeftPoint2_fz_z[7] - bz[1]) / (N3_1[10] * LeftPoint3_fz_z[10]);
k[2] = ((k2[0] + k2[1] + k2[2]) / 3.0) * k[1];
//控制点与像片点计算比例尺
//定义地面控制点
double[] XControl = new double[4];
double[] YControl = new double[4];
double[] ZControl = new double[4];
//读取控制点坐标
string[] lines = richTextBox2.Text.Split('\n');
for (int i = 0; i < lines.Length; i++)
{
string[] Data = lines[i].Split(' ');
XControl[i] = Convert.ToDouble(Data[1]);
YControl[i] = Convert.ToDouble(Data[2]);
ZControl[i] = Convert.ToDouble(Data[3]);
}
//通过三个点互相之间的距离来计算比例尺
double m1, m2, m3;
m1 = (Math.Sqrt((XControl[0] - XControl[1]) * (XControl[0] - XControl[1]) + (YControl[0] - YControl[1]) * (YControl[0] - YControl[1])))
/ (Math.Sqrt((left1_x[0] - left1_x[5]) * (left1_x[0] - left1_x[5]) + (left1_y[0] - left1_y[5]) * (left1_y[0] - left1_y[5])));
m2 = (Math.Sqrt((XControl[0] - XControl[2]) * (XControl[0] - XControl[2]) + (YControl[0] - YControl[2]) * (YControl[0] - YControl[2])))
/ (Math.Sqrt((left1_x[0] - left1_x[6]) * (left1_x[0] - left1_x[6]) + (left1_y[0] - left1_y[6]) * (left1_y[0] - left1_y[6])));
m3 = (Math.Sqrt((XControl[2] - XControl[1]) * (XControl[2] - XControl[1]) + (YControl[2] - YControl[1]) * (YControl[2] - YControl[1])))
/ (Math.Sqrt((left1_x[6] - left1_x[5]) * (left1_x[6] - left1_x[5]) + (left1_y[6] - left1_y[5]) * (left1_y[6] - left1_y[5])));
double avg_m = (m1 + m2 + m3) / 3.0;
//摄站的摄影测量坐标
//求出摄站点坐标之后用于下面求模型点摄影测量坐标系坐标
double[] Xs_sheyingceliang = new double[4];
double[] Ys_sheyingceliang = new double[4];
double[] Zs_sheyingceliang = new double[4];
for (int i = 0; i < 4; i++)
{//四个摄站
if (i == 0)
{
Xs_sheyingceliang[0] = 0.0;
Ys_sheyingceliang[0] = 0.0;
Zs_sheyingceliang[0] = avg_m * f;
}
else
{
Xs_sheyingceliang[i] = Xs_sheyingceliang[i - 1] + avg_m * k[i - 1] * bx[i - 1];
Ys_sheyingceliang[i] = Ys_sheyingceliang[i - 1] + avg_m * k[i - 1] * by[i - 1];
Zs_sheyingceliang[i] = Zs_sheyingceliang[i - 1] + avg_m * k[i - 1] * bz[i - 1];
}
}
//模型点摄影测量坐标
double[,] model_sheyingceliang_first = new double[15, 3];
double[,] model_sheyingceliang_second = new double[8, 3];
double[,] model_sheyingceliang_third = new double[11, 3];
//模型点摄测坐标计算
for (int i = 0; i < 15; i++)
{//像对1
model_sheyingceliang_first[i, 0] = Xs_sheyingceliang[0] + k[0] * avg_m * N1_1[i] * LeftPoint1_fz_x[i];
model_sheyingceliang_first[i, 1] = 0.5 * (Ys_sheyingceliang[0] + k[0] * avg_m * N1_1[i] * LeftPoint1_fz_y[i] + Ys_sheyingceliang[1] + k[0] * avg_m * N1_2[i] * RightPoint1_fz_y[i]);
model_sheyingceliang_first[i, 2] = Zs_sheyingceliang[0] + k[0] * avg_m * N1_1[i] * LeftPoint1_fz_z[i];
}
for (int i = 0; i < 8; i++)
{
model_sheyingceliang_second[i, 0] = Xs_sheyingceliang[1] + k[1] * avg_m * N2_1[i] * LeftPoint2_fz_x[i];
model_sheyingceliang_second[i, 1] = 0.5 * (Ys_sheyingceliang[1] + k[1] * avg_m * N2_1[i] * LeftPoint2_fz_y[i] + Ys_sheyingceliang[2] + k[1] * avg_m * N2_2[i] * RightPoint2_fz_y[i]);
model_sheyingceliang_second[i, 2] = Zs_sheyingceliang[1] + k[1] * avg_m * N2_1[i] * LeftPoint2_fz_z[i];
}
for (int i = 0; i < 11; i++)
{
model_sheyingceliang_third[i, 0] = Xs_sheyingceliang[2] + k[2] * avg_m * N3_1[i] * LeftPoint3_fz_x[i];
model_sheyingceliang_third[i, 1] = 0.5 * (Ys_sheyingceliang[2] + k[2] * avg_m * N3_1[i] * LeftPoint3_fz_y[i] + Ys_sheyingceliang[3] + k[2] * avg_m * N3_2[i] * RightPoint3_fz_y[i]);
model_sheyingceliang_third[i, 2] = Zs_sheyingceliang[2] + k[2] * avg_m * N3_1[i] * LeftPoint3_fz_z[i];
}
//控制点地面摄影测量坐标
double[] control_dimianshece_x = new double[4];
double[] control_dimianshece_y = new double[4];
double[] control_dimianshece_z = new double[4];
//模型点摄影测量坐标
double[] modelX = new double[4];
double[] modelY = new double[4];
double[] modelZ = new double[4];
//绝对定向元素
double delta_x, delta_y, delta_z, lamda, fai, omiga, kappa;
//绝对定向过程
//先根据公式存下坐标变换差
//大地坐标差
double delta_Xt = 0;
double delta_Yt = 0;
//摄影测量坐标差
double delta_Xp = 0;
double delta_Yp = 0;
//转换参数和缩放系数
double a, b, lamda1;
//利用求转换参数和缩放系数
delta_Xt = XControl[0] - XControl[3]; //第一个和最后一个控制点
delta_Yt = YControl[0] - YControl[3];
//从模型点的摄影测量坐标入手
delta_Xp = model_sheyingceliang_first[0, 0] - model_sheyingceliang_third[8, 0]; //第一个像对的第一个模型点和最后一个像对的最后一个模型点
delta_Yp = model_sheyingceliang_first[0, 1] - model_sheyingceliang_third[8, 1];
a = (delta_Xp * delta_Yt + delta_Yp * delta_Xt) / (delta_Xt * delta_Xt + delta_Yt * delta_Yt);
b = (delta_Xp * delta_Xt - delta_Yp * delta_Yt) / (delta_Xt * delta_Xt + delta_Yt * delta_Yt);
lamda1 = Math.Sqrt(a * a + b * b);
//四组模型点的摄影测量坐标系坐标，用来计算绝对定向元素
//根据模型点的点号去找到算出来的点的对应的摄影测量坐标
modelX[0] = model_sheyingceliang_first[0, 0];
modelY[0] = model_sheyingceliang_first[0, 1];
modelZ[0] = model_sheyingceliang_first[0, 2];
modelX[1] = model_sheyingceliang_first[5, 0];
modelY[1] = model_sheyingceliang_first[5, 1];
modelZ[1] = model_sheyingceliang_first[5, 2];
modelX[2] = model_sheyingceliang_first[6, 0];
modelY[2] = model_sheyingceliang_first[6, 1];
modelZ[2] = model_sheyingceliang_first[6, 2];
modelX[3] = model_sheyingceliang_third[8, 0];
modelY[3] = model_sheyingceliang_third[8, 1];
modelZ[3] = model_sheyingceliang_third[8, 2];
//控制点大地坐标转换为地面摄影测量坐标
for (int i = 0; i < 4; i++)
{
control_dimianshece_x[i] = b * (XControl[i] - XControl[0]) + a * (YControl[i] - YControl[0]);
control_dimianshece_y[i] = a * (XControl[i] - XControl[0]) - b * (YControl[i] - YControl[0]);
control_dimianshece_z[i] = lamda1 * (ZControl[i] - ZControl[0]);
}
//求解绝对定向元素
double[,] B = new double[12, 7];//系数阵B
double[,] R = new double[3, 3];//旋转矩阵R
double[,] L = new double[12, 1];//常数项L
double[,] dx = new double[7, 1];//改正数dx
//确定绝对定向元素的初始值
delta_x = 0;
delta_y = 0;
delta_z = 0;
lamda = 1;
fai = 0;
omiga = 0;
kappa = 0;
//计算系数矩阵B
for (int i = 0; i < 4; i++)
{
B[3 * i, 0] = 1;
B[3 * i, 1] = 0;
B[3 * i, 2] = 0;
B[3 * i, 3] = modelX[i];
B[3 * i, 4] = -modelZ[i];
B[3 * i, 5] = 0;
B[3 * i, 6] = -modelY[i];
B[3 * i + 1, 0] = 0;
B[3 * i + 1, 1] = 1;
B[3 * i + 1, 2] = 0;
B[3 * i + 1, 3] = modelY[i];
B[3 * i + 1, 4] = 0;
B[3 * i + 1, 5] = -modelZ[i];
B[3 * i + 1, 6] = modelX[i];
B[3 * i + 2, 0] = 0;
B[3 * i + 2, 1] = 0;
B[3 * i + 2, 2] = 1;
B[3 * i + 2, 3] = modelZ[i];
B[3 * i + 2, 4] = modelX[i];
B[3 * i + 2, 5] = modelY[i];
B[3 * i + 2, 6] = 0;
}
int count = 0;
for (int j = 0; ; j++)
{
//计算旋转矩阵R
R[0, 0] = Math.Cos(fai) * Math.Cos(kappa) - Math.Sin(fai) * Math.Sin(omiga) * Math.Sin(kappa);
R[0, 1] = -Math.Cos(fai) * Math.Sin(kappa) - Math.Sin(fai) * Math.Sin(omiga) * Math.Cos(kappa);
R[0, 2] = -Math.Sin(fai) * Math.Cos(omiga);
R[1, 0] = Math.Cos(omiga) * Math.Sin(kappa);
R[1, 1] = Math.Cos(omiga) * Math.Cos(kappa);
R[1, 2] = -Math.Sin(omiga);
R[2, 0] = Math.Sin(fai) * Math.Cos(kappa) + Math.Cos(fai) * Math.Sin(omiga) * Math.Sin(kappa);
R[2, 1] = -Math.Sin(fai) * Math.Sin(kappa) + Math.Cos(fai) * Math.Sin(omiga) * Math.Cos(kappa);
R[2, 2] = Math.Cos(fai) * Math.Cos(omiga);
//计算常数项L
for (int i = 0; i < 4; i++)
{
L[3 * i, 0] = control_dimianshece_x[i] - lamda * (modelX[i] - kappa * modelY[i] - fai * modelZ[i]) - delta_x;
L[3 * i + 1, 0] = control_dimianshece_y[i] - lamda * (kappa * modelX[i] + modelY[i] - omiga * modelZ[i]) - delta_y;
L[3 * i + 2, 0] = control_dimianshece_z[i] - lamda * (fai * modelX[i] + omiga * modelY[i] + modelZ[i]) - delta_z;
}
//计算改正数
double[,] BT = new double[7, 12];//系数阵B的转置
BT = myMatrix.Matrix_Transpose(B);
double[,] BTB = new double[7, 7]; //BT*B
BTB = myMatrix.Matrix_Multiply(BT, B);
double[,] BTBN = new double[7, 7]; //BT*B的逆矩阵
BTBN = myMatrix.Matrix_Reverse(BTB);
double[,] BTBNBT = new double[7, 12]; //BT*B的逆矩阵乘B的转置
BTBNBT = myMatrix.Matrix_Multiply(BTBN, BT);
dx = myMatrix.Matrix_Multiply(BTBNBT, L);
//将改正数赋给原来的初始值，得到下一次绝对定向元素的初始值
delta_x += dx[0, 0];
delta_y += dx[1, 0];
delta_z += dx[2, 0];
lamda += dx[3, 0];
fai += dx[4, 0];
omiga += dx[5, 0];
kappa += dx[6, 0];
//限制迭代100次
if (count >= 100)
{
break;
}
count = count + 1;
}
//计算出来的三个模型中的模型点地面摄影测量坐标
double[,] modelpt1 = new double[15, 3];
double[,] modelpt2 = new double[8, 3];
double[,] modelpt3 = new double[11, 3];
//计算模型点地摄测系的坐标
for (int i = 0; i < 15; i++)
{
modelpt1[i, 0] = lamda * (model_sheyingceliang_first[i, 0] - kappa * model_sheyingceliang_first[i, 1] - fai * model_sheyingceliang_first[i, 2]) + delta_x;
modelpt1[i, 1] = lamda * (kappa * model_sheyingceliang_first[i, 0] + model_sheyingceliang_first[i, 1] - omiga * model_sheyingceliang_first[i, 2]) + delta_y;
modelpt1[i, 2] = lamda * (fai * model_sheyingceliang_first[i, 0] + omiga * model_sheyingceliang_first[i, 1] + model_sheyingceliang_first[i, 2]) + delta_z;
}
for (int i = 0; i < 8; i++)
{
modelpt2[i, 0] = lamda * (model_sheyingceliang_second[i, 0] - kappa * model_sheyingceliang_second[i, 1] - fai * model_sheyingceliang_second[i, 2]) + delta_x;
modelpt2[i, 1] = lamda * (kappa * model_sheyingceliang_second[i, 0] + model_sheyingceliang_second[i, 1] - omiga * model_sheyingceliang_second[i, 2]) + delta_y;
modelpt2[i, 2] = lamda * (fai * model_sheyingceliang_second[i, 0] + omiga * model_sheyingceliang_second[i, 1] + model_sheyingceliang_second[i, 2]) + delta_z;
}
for (int i = 0; i < 11; i++)
{
modelpt3[i, 0] = lamda * (model_sheyingceliang_third[i, 0] - kappa * model_sheyingceliang_third[i, 1] - fai * model_sheyingceliang_third[i, 2]) + delta_x;
modelpt3[i, 1] = lamda * (kappa * model_sheyingceliang_third[i, 0] + model_sheyingceliang_third[i, 1] - omiga * model_sheyingceliang_third[i, 2]) + delta_y;
modelpt3[i, 2] = lamda * (fai * model_sheyingceliang_third[i, 0] + omiga * model_sheyingceliang_third[i, 1] + model_sheyingceliang_third[i, 2]) + delta_z;
}
//模型点大地坐标
double[,] model_Pt1 = new double[15, 3];
double[,] model_Pt2 = new double[8, 3];
double[,] model_Pt3 = new double[11, 3];
//将地面摄影测量坐标转换为大地坐标
richTextBox5.Text += "701——702" + "\n";
richTextBox5.Text += "点号 X坐标 Y坐标 Z坐标 " + "\n";
for (int i = 0; i < 15; i++)
{
model_Pt1[i, 0] = (b * modelpt1[i, 0] + a * modelpt1[i, 1]) / (lamda1 * lamda1) + XControl[0];
model_Pt1[i, 1] = (a * modelpt1[i, 0] - b * modelpt1[i, 1]) / (lamda1 * lamda1) + YControl[0];
model_Pt1[i, 2] = modelpt1[i, 2] / lamda1 + ZControl[0];
}
for (int i = 0; i < 15; i++)
{
richTextBox5.Text += String.Format("{0, -5}", points1_Name[i]) + " " + String.Format("{0, -19}", model_Pt1[i, 0].ToString("F4")) + " " + String.Format("{0, -19}", model_Pt1[i, 1].ToString("F4")) + " " + String.Format("{0, -19}", model_Pt1[i, 2].ToString("F4")) + "\n";
}
richTextBox5.Text += "\n";
richTextBox5.Text += "702——703" + "\n";
richTextBox5.Text += "点号 X坐标 Y坐标 Z坐标 " + "\n";
for (int i = 0; i < 8; i++)
{
model_Pt2[i, 0] = (b * modelpt2[i, 0] + a * modelpt2[i, 1]) / (lamda1 * lamda1) + XControl[0];
model_Pt2[i, 1] = (a * modelpt2[i, 0] - b * modelpt2[i, 1]) / (lamda1 * lamda1) + YControl[0];
model_Pt2[i, 2] = modelpt2[i, 2] / lamda1 + ZControl[0];
}
for (int i = 0; i < 8; i++)
{
richTextBox5.Text += String.Format("{0, -5}", points2_Name[i]) + " " + String.Format("{0, -19}", model_Pt2[i, 0].ToString("F4")) + " " + String.Format("{0, -19}", model_Pt2[i, 1].ToString("F4")) + " " + String.Format("{0, -19}", model_Pt2[i, 2].ToString("F4")) + "\n";
}
richTextBox5.Text += "\n";
richTextBox5.Text += "703——704" + "\n";
richTextBox5.Text += "点号 X坐标 Y坐标 Z坐标 " + "\n";
for (int i = 0; i < 11; i++)
{
model_Pt3[i, 0] = (b * modelpt3[i, 0] + a * modelpt3[i, 1]) / (lamda1 * lamda1) + XControl[0];
model_Pt3[i, 1] = (a * modelpt3[i, 0] - b * modelpt3[i, 1]) / (lamda1 * lamda1) + YControl[0];
model_Pt3[i, 2] = modelpt3[i, 2] / lamda1 + ZControl[0];
}
for (int i = 0; i < 11; i++)
{
richTextBox5.Text += String.Format("{0, -5}", points3_Name[i]) + " " + String.Format("{0, -19}", model_Pt3[i, 0].ToString("F4")) + " " + String.Format("{0, -19}", model_Pt3[i, 1].ToString("F4")) + " " + String.Format("{0, -19}", model_Pt3[i, 2].ToString("F4")) + "\n";
}
//大地坐标系下的检查点坐标
double[] checkpoint = new double[5];
double[] checkP_Xt = new double[5];
double[] checkP_Yt = new double[5];
double[] checkP_Zt = new double[5];
//检查点的误差
double[] checkP_dx = new double[5];
double[] checkP_dy = new double[5];
double[] checkP_dz = new double[5];
//读取检查点的坐标
//检查点为四个包含在像对中的地面点的已知坐标，用来检测计算结果精度
string[] l = richTextBox3.Text.Split('\n');
for (int i = 0; i < l.Length; i++)
{
string[] Data = l[i].Split(' ');
checkpoint[i] = double.Parse(Data[0]);
checkP_Xt[i] = double.Parse(Data[1]);
checkP_Yt[i] = double.Parse(Data[2]);
checkP_Zt[i] = double.Parse(Data[3]);
}
//求检查点与控制点坐标的差值
checkP_dx[0] = model_Pt1[12, 0] - checkP_Xt[0];
checkP_dy[0] = model_Pt1[12, 1] - checkP_Yt[0];
checkP_dz[0] = model_Pt1[12, 2] - checkP_Zt[0];
checkP_dx[1] = model_Pt1[2, 0] - checkP_Xt[1];
checkP_dy[1] = model_Pt1[2, 1] - checkP_Yt[1];
checkP_dz[1] = model_Pt1[2, 2] - checkP_Zt[1];
checkP_dx[2] = model_Pt1[4, 0] - checkP_Xt[2];
checkP_dy[2] = model_Pt1[4, 1] - checkP_Yt[2];
checkP_dz[2] = model_Pt1[4, 2] - checkP_Zt[2];
checkP_dx[3] = model_Pt1[3, 0] - checkP_Xt[3];
checkP_dy[3] = model_Pt1[3, 1] - checkP_Yt[3];
checkP_dz[3] = model_Pt1[3, 2] - checkP_Zt[3];
checkP_dx[4] = model_Pt2[1, 0] - checkP_Xt[4];
checkP_dy[4] = model_Pt2[1, 1] - checkP_Yt[4];
checkP_dz[4] = model_Pt2[1, 2] - checkP_Zt[4];
//将检查的结果输出
richTextBox6.Text += "点号 X坐标差 Y坐标差 Z坐标差\n";
for (int i = 0; i < l.Length; i++)
{
richTextBox6.Text += String.Format("{0, -4}", checkpoint[i]) + " " + String.Format("{0, -19}", checkP_dx[i].ToString("F4")) + " " + String.Format("{0, -19}", checkP_dy[i].ToString("F4")) + " " + String.Format("{0, -19}", checkP_dz[i].ToString("F4")) + "\n";
}
}
public void XDDX(int point_count, double[] x1, double[] y1, double[] x2, double[] y2, //同名像点的左右平面坐标
double[,] dx, //相对方位元素改正数（dx）
double[,] X, //相对方位元素
double[] Xm, double[] Ym, double[] Zm, //模型点在像空辅中的坐标
double[] N1, double[] N2, //两个点投影系数
double[] X1, double[] Y1, double[] Z1, double[] Y2, //像点的像空间辅助坐标
ref double bX, ref double bY, ref double bZ) //摄影基线分量
{//像对定向函数
double by, bz;
double[] X2 = new double[point_count];
double[] Z2 = new double[point_count];
//R矩阵的计算过程，带入公式得到9个系数
double a11 = Math.Cos(X[2, 0]) * Math.Cos(X[4, 0]) - Math.Sin(X[2, 0]) * Math.Sin(X[3, 0]) * Math.Sin(X[4, 0]);
double a12 = -Math.Cos(X[2, 0]) * Math.Sin(X[4, 0]) - Math.Sin(X[2, 0]) * Math.Sin(X[3, 0]) * Math.Cos(X[4, 0]);
double a13 = -Math.Sin(X[2, 0]) * Math.Cos(X[3, 0]);
double b11 = Math.Cos(X[3, 0]) * Math.Sin(X[4, 0]);
double b12 = Math.Cos(X[3, 0]) * Math.Cos(X[4, 0]);
double b13 = -Math.Sin(X[3, 0]);
double c11 = Math.Sin(X[2, 0]) * Math.Cos(X[4, 0]) + Math.Cos(X[2, 0]) * Math.Sin(X[3, 0]) * Math.Sin(X[4, 0]);
double c12 = -Math.Sin(X[2, 0]) * Math.Sin(X[4, 0]) + Math.Cos(X[2, 0]) * Math.Sin(X[3, 0]) * Math.Cos(X[4, 0]);
double c13 = Math.Cos(X[2, 0]) * Math.Cos(X[3, 0]);
//通过旋转矩阵，将像空间直角坐标系转换为像空间辅助坐标系
for (int i = 0; i < point_count; i++)
{
X1[i] = (a11 * x1[i] + a12 * y1[i] - a13 * f);
Y1[i] = (b11 * x1[i] + b12 * y1[i] - b13 * f);
Z1[i] = (c11 * x1[i] + c12 * y1[i] - c13 * f);
}
//定义观测值Q
double[,] Q = new double[point_count, 1];
int cirnum = 0;
for (int k = 0; ; k++)
{
//右片的旋转矩阵R2
double a1 = Math.Cos(X[2, 0]) * Math.Cos(X[4, 0]) - Math.Sin(X[2, 0]) * Math.Sin(X[3, 0]) * Math.Sin(X[4, 0]);
double a2 = -Math.Cos(X[2, 0]) * Math.Sin(X[4, 0]) - Math.Sin(X[2, 0]) * Math.Sin(X[3, 0]) * Math.Cos(X[4, 0]);
double a3 = -Math.Sin(X[2, 0]) * Math.Cos(X[3, 0]);
double b1 = Math.Cos(X[3, 0]) * Math.Sin(X[4, 0]);
double b2 = Math.Cos(X[3, 0]) * Math.Cos(X[4, 0]);
double b3 = -Math.Sin(X[3, 0]);
double c1 = Math.Sin(X[2, 0]) * Math.Cos(X[4, 0]) + Math.Cos(X[2, 0]) * Math.Sin(X[3, 0]) * Math.Sin(X[4, 0]);
double c2 = -Math.Sin(X[2, 0]) * Math.Sin(X[4, 0]) + Math.Cos(X[2, 0]) * Math.Sin(X[3, 0]) * Math.Cos(X[4, 0]);
double c3 = Math.Cos(X[2, 0]) * Math.Cos(X[3, 0]);
//同理，通过旋转矩阵R2得到右边相片像空间辅助坐标系
for (int i = 0; i < point_count; i++)
{
//MessageBox.Show(Convert.ToString(i));
X2[i] = a1 * x2[i] + a2 * y2[i] + a3 * (-f);
Y2[i] = b1 * x2[i] + b2 * y2[i] + b3 * (-f);
Z2[i] = c1 * x2[i] + c2 * y2[i] + c3 * (-f);
}
//bx乘以相对方位元素得到by和bz
by = bx * X[0, 0]; bz = bx * X[1, 0];
//通过得到的各数计算点投影系数N1 N2以及观测值Q
for (int i = 0; i < point_count; i++)
{
N1[i] = (bx * Z2[i] - bz * X2[i]) / (X1[i] * Z2[i] - X2[i] * Z1[i]);
N2[i] = (bx * Z1[i] - bz * X1[i]) / (X1[i] * Z2[i] - X2[i] * Z1[i]);
Q[i, 0] = N1[i] * Y1[i] - N2[i] * Y2[i] - by;
}
//组成误差方程式
//先对系数矩阵B进行计算
double[,] B = new double[point_count, 5];
for (int i = 0; i < point_count; i++)
{
B[i, 0] = bx;
B[i, 1] = -Y2[i] / Z2[i] * bx;
B[i, 2] = -X2[i] * Y2[i] * N2[i] / Z2[i];
B[i, 3] = -(Z2[i] + Y2[i] * Y2[i] / Z2[i]) * N2[i];
B[i, 4] = X2[i] * N2[i];
}
//解算法方程
double[,] BT = new double[7, 12];//系数阵B的转置
BT = myMatrix.Matrix_Transpose(B);
double[,] BTB = new double[7, 7]; //BT*B
BTB = myMatrix.Matrix_Multiply(BT, B);
double[,] BTBN = new double[7, 7]; //BT*B的逆矩阵
BTBN = myMatrix.Matrix_Reverse(BTB);
double[,] BTBNBT = new double[7, 12]; //BT*B的逆矩阵乘B的转置
BTBNBT = myMatrix.Matrix_Multiply(BTBN, BT);
//得到改正数
dx = myMatrix.Matrix_Multiply(BTBNBT, Q);
//限制迭代次数
if (cirnum >= 100)
{
for (int i = 0; i < 5; i++)
{
X[i, 0] = X[i, 0] + dx[i, 0];
}
break;
}
//加上改正数 得到方位元素新值
else
{
for (int i = 0; i < 5; i++)
{
X[i, 0] = X[i, 0] + dx[i, 0];
}
}
cirnum = cirnum + 1;
}
//计算地面点的像空间辅助坐标
for (int j = 0; j < point_count; j++)
{
Xm[j] = N1[j] * X1[j];
Ym[j] = 0.5 * (N1[j] * Y1[j] + N2[j] * Y2[j] + by);
Zm[j] = N1[j] * Z1[j];
}
bZ = bz;
bY = by;
bX = bx;
}
}
}
以下是矩阵计算模块：
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Analytic_aerial_triangulation
{
class MatrixOperations
{
//1、矩阵减法
public double[,] Matr_Sub(double[,] Matr1, double[,] Matr2)
{
int row = Matr1.GetLength(0);
int column = Matr1.GetLength(1);
double[,] Matr_Output = new double[row, column];
for (int i = 0; i < row; i++)
{
for (int j = 0; j < column; j++)
{
Matr_Output[i, j] = Matr1[i, j] - Matr2[i, j]; //对应位置相减
}
}
return Matr_Output;
}
//2、矩阵求转置，误差方程中的B系数矩阵需要用到转置
public double[,] Matrix_Transpose(double[,] Matr1)
{
int row = Matr1.GetLength(0);
int column = Matr1.GetLength(1);
double[,] Matr_Output = new double[column, row]; //新定义一个矩阵，与原矩阵的行数和列数相反
for (int i = 0; i < column; i++)
{
for (int j = 0; j < row; j++)
{
Matr_Output[i, j] = Matr1[j, i]; //新矩阵和原矩阵的行列对应位置进行新值的赋值
}
}
return Matr_Output;
}
//3、矩阵乘法
public double[,] Matrix_Multiply(double[,] Matr1, double[,] Matr2) //矩阵乘法
{
//在误差方程中，计算两矩阵乘法时保证了前提是矩阵1的列等于矩阵2的行，故在此不再进行矩阵的行列是否相等判断
//此处定义的row_2既为矩阵1的列也为矩阵2的行
int row_1 = Matr1.GetLength(0);
int column_2 = Matr2.GetLength(1);
int row_2 = Matr2.GetLength(0);
double[,] Matr_Output = new double[row_1, column_2]; //定义结果矩阵
for (int i = 0; i < row_1; i++)
{
for (int j = 0; j < column_2; j++)
{
for (int k = 0; k < row_2; k++)
{
Matr_Output[i, j] += Matr1[i, k] * Matr2[k, j]; //k为新矩阵的列定位，通过此项定位确定结果位置
}
}
}
return Matr_Output;
}
//4、矩阵行列式计算
public double Matrix_Value(double[,] Matr1, int level)
{
double[,] Matr_MidResult = new double[level, level]; //确定一个新的固定行列数的矩阵
for (int i = 0; i < level; i++)
{
for (int j = 0; j < level; j++)
{
Matr_MidResult[i, j] = Matr1[i, j]; //将原有矩阵赋给新矩阵进行后续计算
}
}
double MidValue, MidResultValue;
int PositiveNumberJudge = 1;
for (int i = 0, j = 0; i < level && j < level; i++, j++)
{
if (Matr_MidResult[i, j] == 0)
{
int point_count = i;
for (; Matr_MidResult[point_count, j] == 0; point_count++) ;
if (point_count == level)
{
return 0; //找到最后一行时停止计算
}
else
{
for (int n = j; n < level; n++)
{
MidValue = Matr_MidResult[i, n];
Matr_MidResult[i, n] = Matr_MidResult[point_count, n];
Matr_MidResult[point_count, n] = MidValue;
}
PositiveNumberJudge *= (-1);
}
}
for (int s = level - 1; s > i; s--)
{
MidResultValue = Matr_MidResult[s, j];
for (int t = j; t < level; t++)
{
Matr_MidResult[s, t] -= Matr_MidResult[i, t] * (MidResultValue / Matr_MidResult[i, j]);
}
}
}
double ChangelessValue = 1;
for (int i = 0; i < level; i++)
{
if (Matr_MidResult[i, i] != 0)
{
ChangelessValue *= Matr_MidResult[i, i];
}
else
{
return (0);
}
}
return PositiveNumberJudge * ChangelessValue;
}
//5、矩阵求逆
public double[,] Matrix_Reverse(double[,] dMatrix)
{
int Level = dMatrix.GetLength(0);
double ReverseValue, CalculationValue;
double SubMatrix = Matrix_Value(dMatrix, Level);
if (SubMatrix == 0)
{
return null;
}
double[,] MatrixReturn = new double[Level, 2 * Level];
for (int i = 0; i < Level; i++)
{
for (int j = 0; j < 2 * Level; j++)
{
if (j < Level)
{
MatrixReturn[i, j] = dMatrix[i, j];
}
else
{
MatrixReturn[i, j] = 0;
}
}
MatrixReturn[i, Level + i] = 1;
}
for (int i = 0, j = 0; i < Level && j < Level; i++, j++)
{
if (MatrixReturn[i, j] == 0)
{
int point_count = i;
for (; dMatrix[point_count, j] == 0; point_count++) ;
if (point_count == Level)
{
return null;
}
else
{
for (int n = j; n < 2 * Level; n++)
{
MatrixReturn[i, n] += MatrixReturn[point_count, n];
}
}
}
ReverseValue = MatrixReturn[i, j];
if (ReverseValue != 1)
{
for (int n = j; n < 2 * Level; n++)
{
if (MatrixReturn[i, n] != 0)
{
MatrixReturn[i, n] /= ReverseValue;
}
}
}
for (int s = Level - 1; s > i; s--)
{
ReverseValue = MatrixReturn[s, j];
for (int t = j; t < 2 * Level; t++)
{
MatrixReturn[s, t] -= (MatrixReturn[i, t] * ReverseValue);
}
}
}
for (int i = Level - 2; i >= 0; i--)
{
for (int j = i + 1; j < Level; j++)
{
if (MatrixReturn[i, j] != 0)
{
CalculationValue = MatrixReturn[i, j];
for (int n = j; n < 2 * Level; n++)
{
MatrixReturn[i, n] -= (CalculationValue * MatrixReturn[j, n]);
}
}
}
}
double[,] Matrix_Return = new double[Level, Level];
for (int i = 0; i < Level; i++)
{
for (int j = 0; j < Level; j++)
{
Matrix_Return[i, j] = MatrixReturn[i, j + Level];
}
}
return Matrix_Return;
}
}
}
以下是窗体界面以供参考：
主要用到richTextBox和button
欢迎大家在评论区留言，后续也会根据大家的建议进一步优化代码。
2134
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
