單像空間後方交會解算c#
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace 單像空間後方交會解算
{
class Program
{
static void Main(string[] args)
{
#region 變數定義
交會解算 jh = new 交會解算();
double f, x0, y0; //內方位元素
int No; //控制點計數
int lineNo; //線條的數目
double sum = 0;
int k = 0; //用來需要對進行迴圈的作為陣列下標
double scale = 0; //平均比例尺
double H = 0; //平均航高
double[] L; //用存放地面距離
double[] GeoX = null, GeoY = null, GeoZ = null; //地面點座標
double[] Phox = null, Phoy = null; //像點觀測值
double[,] R = new double[3, 3]; //宣告旋轉矩陣
double a1, a2, a3, b1, b2, b3, c1, c2, c3; //旋轉元素
double Xs, Ys, Zs, Phi, Omega, Kappa; //定義外方位元素
double[] x, y; //像點近似值
double[,] A ,AT,RevA,Const; //定義誤差方程係數矩陣和轉置矩陣,A的逆矩陣和AT*constx_y
double XBar, YBar, ZBar; //將共線條件方程簡化
double[,] constx_y; //用於存放誤差方程常數項
double[,] X; //最後的結果矩陣
double dX, dY, dZ, dPhi, dOmega, dKappa; //最後的結果
string yesno="Y"; //如果是Y則繼續迭代
int times = 1; //迭代次數
#endregion
Console.WriteLine("歡迎使用這個程式計算單像空間後方交會");
#region 初始化內方位元素
Console.Write("攝影主距f:");//用來存內方位元素
f = 153.24;
Console.WriteLine(f+"mm");
Console.Write("內方位元素x0:");
x0 = 0;
Console.WriteLine(x0+"mm");
Console.Write("內方位元素y0:");
y0 = 0;
Console.WriteLine(y0+"mm");
#endregion
#region 輸入地面攝影測量座標
Console.WriteLine("輸入的控制點的個數：4");
No = 4;
GeoX = new double[No]; //存入地面座標
GeoY = new double[No];
GeoZ = new double[No];
GeoX[0] = 36589410;
GeoX[1] = 37631080;
GeoX[2] = 39100970;
GeoX[3] = 40426540;
GeoY[0] = 25273320;
GeoY[1] = 31324510;
GeoY[2] = 24934980;
GeoY[3] = 30319810;
GeoZ[0] = 2195170;
GeoZ[1] = 728690;
GeoZ[2] = 2386500;
GeoZ[3] = 757310;
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("單位：mm");
for (int i = 0; i < No; i++)//遍歷攝影測量座標
{
Console.WriteLine("第{0}個點的地面攝影測量座標X/Y/Z：\t{1}，\t{2}，\t{3}", i + 1, GeoX[i], GeoY[i], GeoZ[i]);
}
#endregion
#region 輸入像點觀測值
Phox =new double [No ];
Phoy = new double[No];
Phox [0]=-86.15;
Phox [1]=-53.4;
Phox [2]=-14.78;
Phox[3] = 10.46;
Phoy [0]=-68.99;
Phoy [1]=82.21;
Phoy [2]=-76.63 ;
Phoy[3] = 64.43;
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("單位：mm");
for (int i = 0; i < No; i++)
{
Console.WriteLine("第{0}個控制點的對應像點座標x/y：\t{1}， \t{2}", i + 1, Phox[i], Phoy[i]);
}//遍歷像點座標
#endregion
#region 計算比例尺和航高
lineNo = jh.LineNo(No);
L = new double[lineNo]; //用來存放地面距離
for (int i = 0; i < No; i++)//計算地面點的距離
{
for (int j = i + 1; j < No; j++)
{
L[k] = jh.GetLength(GeoX, GeoY, i, j);
k++;
}
}
k = 0; //歸零
double[] l = new double[lineNo];
for (int i = 0; i < No; i++)//計算像點的距離
{
for (int j = i + 1; j < No; j++)
{
l[k] = jh.GetLength(Phox, Phoy, i, j);
k++;
}
}
k = 0; //歸零
for (int i = 0; i < lineNo; i++)//比例尺求和
{
sum += l[i] / L[i];
}
scale = sum / lineNo;//記錄平均比例尺
sum = 0; //歸零
Console.WriteLine("比例尺：{0}", scale);
H = f / scale; //平均航高
Console.WriteLine("平均航高：{0}", H);
#endregion
#region 初始化外方位元素並計算旋轉矩陣
for (int i = 0; i < No; i++)//計算Xs
{
sum += GeoX[i];
}
Xs = sum / No;
sum = 0; //歸零
for (int i = 0; i < No; i++)//Ys計算
{
sum += GeoY[i];
}
Ys = sum / No;
sum = 0; //歸零
Zs = H;//Zs計算
Phi = 0;
Omega = 0;
Kappa = 0;
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("單位：mm");
Console.WriteLine(" \tXs\tYs\tZs\tPhi\tOmega\tKappa");
Console.WriteLine("外方位元素近似值：\t{0}\t{1}\t{2}\t{3}\t{4}\t{5}",Xs ,Ys ,Zs ,Phi ,Omega ,Kappa );
while (yesno == "y" || yesno == "Y")
{ //計算旋轉矩陣
jh.CalRotMat(R, Phi, Omega, Kappa);
a1 = R[0, 0]; a2 = R[0, 1]; a3 = R[0, 2];
b1 = R[1, 0]; b2 = R[1, 1]; b3 = R[1, 2];
c1 = R[2, 0]; c2 = R[2, 1]; c3 = R[2, 2];//為方便後續計算用abc來代替具體的元素索引
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("旋轉矩陣是:");
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
Console.Write("\t{0}", R[i, j]);
}
Console.WriteLine();
} //輸出旋轉矩陣
#endregion
#region 計算像點近似值
x = new double[No];
y = new double[No];
for (int i = 0; i < No; i++)//計算像點座標近似值
{
XBar = a1 * (GeoX[i] - Xs) + b1 * (GeoY[i] - Ys) + c1 * (GeoZ[i] - Zs);
YBar = a2 * (GeoX[i] - Xs) + b2 * (GeoY[i] - Ys) + c2 * (GeoZ[i] - Zs);
ZBar = a3 * (GeoX[i] - Xs) + b3 * (GeoY[i] - Ys) + c3 * (GeoZ[i] - Zs);
x[i] = -f * XBar / ZBar;
y[i] = -f * YBar / ZBar;
}
Console.WriteLine("像點近似值是: x\\y mm");
for (int i = 0; i < No; i++)
{
Console.WriteLine(" \t{0} \t{1}", x[i], y[i]);
}//近似值輸出
#endregion
#region 計算誤差方程係數與常數項
A = new double[2 * No, 6];
for (int i = 0; i < 2 * No; i++)
{
XBar = a1 * (GeoX[k] - Xs) + b1 * (GeoY[k] - Ys) + c1 * (GeoZ[k] - Zs);
YBar = a2 * (GeoX[k] - Xs) + b2 * (GeoY[k] - Ys) + c2 * (GeoZ[k] - Zs);
ZBar = a3 * (GeoX[k] - Xs) + b3 * (GeoY[k] - Ys) + c3 * (GeoZ[k] - Zs);
if (i % 2 == 0)//陣列第奇數行a11,a31等
{
A[i, 0] = (a1 * f + a3 * Phox[k]) / ZBar;//a11
A[i, 1] = (b1 * f + b3 * Phox[k]) / ZBar;//a12
A[i, 2] = (c1 * f + c3 * Phox[k]) / ZBar;//a13
A[i, 3] = Phoy[k] * Math.Sin(Omega) - (Phox[k] * (Phox[k] * Math.Cos(Kappa) - Phoy[k] * Math.Sin(Kappa)) / f + f * Math.Cos(Kappa)) * Math.Cos(Omega);//a14
A[i, 4] = -f * Math.Sin(Kappa) - Phox[k] * (Phox[k] * Math.Sin(Kappa) + Phoy[k] * Math.Cos(Kappa)) / f;//a15
A[i, 5] = Phoy[k]; //a16
}
else //偶數行
{
A[i, 0] = (a2 * f + a3 * Phoy[k]) / ZBar;//a21
A[i, 1] = (b2 * f + b3 * Phoy[k]) / ZBar;//a22
A[i, 2] = (c2 * f + c3 * Phoy[k]) / ZBar;//a23
A[i, 3] = -Phox[k] * Math.Sin(Omega) - (Phoy[k] * (Phox[k] * Math.Cos(Kappa) - Phoy[k] * Math.Sin(Kappa) / f) - f * Math.Sin(Kappa)) * Math.Cos(Omega);//a24
A[i, 4] = -f * Math.Cos(Kappa) - (Phoy[k] * (Phoy[k] - Math.Sin(Kappa) + Phoy[k] * Math.Cos(Kappa))) / f;//a25
A[i, 5] = -Phox[k];//a26
k++;
}
}
k = 0;//歸零
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("誤差方程係數矩陣A是:");
for (int i = 0; i < 2 * No; i++)//行
{
for (int j = 0; j < 6; j++)
{
Console.Write("{0 ,2}\t", A[i, j]);
}
Console.WriteLine();
}//輸出係數矩陣
constx_y = new double[2 * No, 1];
for (int i = 0; i < 2 * No; i++)
{
if (i % 2 == 0) //奇數行即關於x的
constx_y[i, 0] = Phox[k] - x[k];
else //偶數行
{
constx_y[i, 0] = Phoy[k] - y[k];
k++;
}
}
k = 0;
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("像點觀測值減近似值矩陣是: mm");
for (int i = 0; i < 2 * No; i++)
{
Console.WriteLine(constx_y[i, 0]);
}
#endregion
#region 計算A的轉置乘以A和A的轉置和AT*Const
AT = new double[6, 2 * No];
AT = jh.MatTrans(A); //計算a的轉置矩陣
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("誤差方程係數矩陣轉置矩陣AT是:");
for (int i = 0; i < 6; i++)//行
{
for (int j = 0; j < 2 * No; j++)
{
Console.Write("{0 ,2}\t", AT[i, j]);
}
Console.WriteLine();
}
//計算AT*A
double[,] MultiAT_A = new double[6, 6];
MultiAT_A = jh.MultiMat(AT, A);
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("AT*A是:");
for (int i = 0; i < 6; i++)//行
{
for (int j = 0; j < 6; j++)
{
Console.Write("{0 ,2}\t", MultiAT_A[i, j]);
}
Console.WriteLine();
}
RevA = jh.ReverseMatrix(MultiAT_A); //計算AT*A的逆矩陣
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("AT*A的逆矩陣是:");
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
Console.Write("{0}\t", RevA[i, j]);
}
Console.WriteLine();
}
Const = new double[6, 1];
Const = jh.MultiMat(AT, constx_y);
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("AT*Const矩陣是：");
for (int i = 0; i < 6; i++)
{
Console.WriteLine(Const[i, 0]);
}
#endregion
#region 得到最後結果
X = new double[6, 1];
X = jh.MultiMat(RevA, Const);
dX = X[0, 0];
dY = X[1, 0];
dZ = X[2, 0];
dPhi = X[3, 0];
dOmega = X[4, 0];
dKappa = X[5, 0];
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("dx\tdy\tdz\tdphi\tdomega\tdkappa\t");
Console.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t", dX, dY, dZ, dPhi, dOmega, dKappa);
Xs += dX;
Ys += dY;
Zs += dZ;
Phi += dPhi;
Omega += dOmega;
Kappa += dKappa;
Console.WriteLine("------------------------------------------------------------------------------------------------------------------------");
Console.WriteLine("Xs\tYs\tZs\tPhi\tOmega\tKappa\t");
Console.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t", Xs, Ys, Zs, Phi, Omega, Kappa);
#endregion
times++;
//手動判斷是否需要進行迭代
//Console.WriteLine("是否需要下一次迭代？Y\\N 當前已迭代{0}次",times );
//yesno = Console.ReadLine();
if (Math .Abs ( dX) < 500)
yesno = "N";
}
Console.WriteLine("當前迭代{0}次",times );
}
}
}
新建一個類
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace 單像空間後方交會解算
{
public class 交會解算
{
public int LineNo(int pointno)//根據控制點數量用來遞迴計算需要多少條線來計算比例尺
{
if (pointno == 1)
return 0;
else
return pointno - 1 + LineNo(pointno - 1);
}
public double GetLength(double [] X,double []Y,int i,int j)//計算兩點的距離
{
return Math.Sqrt((X[i] - X[j]) * (X[i]-X[j]) + (Y[i] - Y[j]) * (Y[i] - Y[j]));
}
public void CalRotMat(double[,] R, double Phi, double Omega, double Kappa)
{
R[0, 0] = Math.Cos(Phi) * Math.Cos(Kappa) - Math.Sin(Phi) * Math.Sin(Omega) * Math.Sin(Kappa);
R[0, 1] = -Math.Cos(Phi) * Math.Sin(Kappa) - Math.Sin(Phi) * Math.Sin(Omega) * Math.Cos(Kappa);
R[0, 2] =-Math.Sin(Phi) * Math.Cos(Omega);
R[1, 0] =Math.Cos(Omega) * Math.Sin(Kappa);
R[1, 1] = Math.Cos(Omega) * Math.Cos(Kappa);
R[1, 2] =-Math.Sin(Omega);
R[2, 0] =Math.Sin(Phi) * Math.Cos(Kappa) + Math.Cos(Phi) * Math.Sin(Omega) * Math.Sin(Kappa);
R[2, 1] =-Math.Sin(Phi) * Math.Sin(Kappa) + Math.Cos(Phi) * Math.Sin(Omega) * Math.Cos(Kappa);
R[2, 2] = Math.Cos(Phi) * Math.Cos(Omega);
}
public double[,] MatTrans(double[,] B) //實現矩陣轉置
{
int m, n;
m = B.GetLength(0);
n = B.GetLength(1);
double[,] C = new double[n, m];
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
C[j, i] = B[i, j];
}
}
return C;
}
public double[,] ReverseMatrix(double[,] Array)//矩陣求逆
{
int m = 0;
int n = 0;
m = Array.GetLength(0);
n = Array.GetLength(1);
double[,] array = new double[2 * m + 1, 2 * n + 1];
for (int k = 0; k < 2 * m + 1; k++) //初始化陣列
{
for (int t = 0; t < 2 * n + 1; t++)
{
array[k, t] = 0.00000000;
}
}
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
array[i, j] = Array[i, j];
}
}
for (int k = 0; k < m; k++)
{
for (int t = n; t <= 2 * n; t++)
{
if ((t - k) == m)
{
array[k, t] = 1.0;
}
else
{
array[k, t] = 0;
}
}
}
//得到逆矩陣
for (int k = 0; k < m; k++)
{
if (array[k, k] != 1)
{
double bs = array[k, k];
array[k, k] = 1;
for (int p = k + 1; p < 2 * n; p++)
{
array[k, p] /= bs;
}
}
for (int q = 0; q < m; q++)
{
if (q != k)
{
double bs = array[q, k];
for (int p = 0; p < 2 * n; p++)
{
array[q, p] -= bs * array[k, p];
}
}
else
{
continue;
}
}
}
double[,] NI = new double[m, n];
for (int x = 0; x < m; x++)
{
for (int y = n; y < 2 * n; y++)
{
NI[x, y - n] = array[x, y];
}
}
return NI;
}
public double[,] MultiMat(double[,] A,double[,] B)//矩陣乘法A*B
{
int m, n,a,b;
n = A.GetLength(0); //A行數
a = A.GetLength(1); //A和B的列數
m = B.GetLength(1); //B列數
//a==b則可以相乘
double[,] C = new double[n, m]; //最終相乘的結果
for (int i = 0; i < n; i++) //n是第一個矩陣的行數
{
double sum = 0;
for (int j = 0; j < m; j++) //m是第二個矩陣的列數
{
for (int k = 0; k < a; k++) //n是第二個矩陣的行數
{
sum +=A[i,k]*B[k,j];
}
C[i, j] = sum;
}
}
return C;
}
}
}
例題是武漢大學出版社攝影測量學95頁第三題。
相關文章
- 單像空間後方交會計算旋轉矩陣矩陣
- 單像空間後方交會計算誤差方程係數矩陣矩陣
- Linux交換空間swap講解Linux
- 檢查交換空間: 可用的交換空間為 0 MB, 所需的交換空間為 150 MB。 未通過
- 智慧空間亮相高交會，解析園區的創新與實踐
- redhat6.9增加swap交換空間大小Redhat
- SAP公司間STO流程裡外向交貨單PGI後自動觸發內向交貨單的實現
- 瞭解下C# 名稱空間（Namespace）C#namespace
- 【scipy 基礎】--空間計算
- SAP MM 公司間STO裡交貨單PGI之後自動觸發內向交貨單功能的實現
- Linux 中交換空間 （swap）應該分多大才好？Linux
- SAP 公司間STO場景中外向交貨單過賬後自動觸發內向交貨單功能的實現
- 空間或平面判斷兩線段相交(求交點)
- Postgresql表空間詳解SQL
- C++中struct的空間計算C++Struct
- 白糖 近期等待空單機會
- 【財富空間】像先知一樣思考，如拳手般戰鬥
- ifccolumn在空間中的座標計算
- 簡單介紹C#呼叫USB攝像頭的方法C#
- 通過IDoc DESADV來實現公司間STO場景中外向交貨單過賬後自動觸發內向交貨單的功能
- 《2021網路空間測繪年報》解讀｜公有云資產畫像與風險度量
- C#學習 [型別系統] 名稱空間（12）C#型別
- c#實現簡單的俄羅斯方塊C#
- 李飛飛「空間智慧」之後，上交、智源、北大等提出空間大模型SpatialBot大模型
- Java9後String的空間優化Java優化
- Oracle的表空間quota詳解Oracle
- 一次交換空間設定不合理引發的故障
- 為什麼問題空間與解決方案空間如此重要？ - Nikhil Gupta
- 16、表空間 建立表空間
- SAP MM 公司間STO裡外向交貨單與內向交貨單裡序列號對應關係
- Keil 工程在編譯完之後，會有相應的程式所佔用的空間提示資訊解釋編譯
- onethink上傳到空間之後，runtime資料夾將空間佔滿如何解決
- MySQL 遷移表空間，備份單表MySql
- JavaScript進階-記憶體空間詳解(雙十一過後的一更)JavaScript記憶體
- AI賦能網路空間安全 • 綠盟科技亮相2019中國計算機大會AI計算機
- SAP MM STO單據的外向交貨單建立後新加ITEM？
- 恆訊科技講解：空間不足，香港雲伺服器怎麼加空間？伺服器
- SAP ＭＭ公司間STO裡的交貨單自動建立？