[Photography surveying space rear rendezvous operation] solution procedure

[Photography surveying space rear rendezvous operation] solution procedure




# Include <stdio. h> # include <cmath>
# Include <iostream. h>
# Include <fstream. h> int main ()
{
 
Double NJZ (double sum [100] [100], double l [10]);
Double x [10], y [10], X [10], Y [10], Z [10], d, D, m, f, T, R, S, r = 0, s = 0, t = 0;
Ifstream infile; // defines the input file class.
 
Infile. open ("test _1.txt"); // open an input file named file1.txt"
For (int I = 1; I <= 4; I ++)
{
For (int j = 1; j <= 5; j ++)
{
Switch (j)
{
Case 1: infile> x [I]; break;
Case 2: infile> y [I]; break;
Case 3: infile> X [I]; break;
Case 4: infile> Y [I]; break;
Case 5: infile> Z [I]; break;
}
// Infile> a [I] [j]; // input the ten integers in parameter file1.txt to a [I ].
}
}
// For (I = 1; I <= 4; I ++)
//{
// Cout <x [I] <"," <y [I] <"," <X [I] <", "<Y [I] <", "<Z [I] <endl;
//}
// Cout <endl;
Infile. close (); // close the input file
D = sqrt (pow (x [1]-x [2], 2) + pow (y [1]-y [2], 2 ));
// Cout <"d =" <d <endl;
D = sqrt (pow (X [1]-X [2], 2) + pow (Y [1]-Y [2], 2) + pow (Z [1]-Z [2], 2 ));
// Cout <"D =" <D <endl;
M = D/d;
// Cout <"m =" <m <endl;
// Cout <"Enter the image base distance f (in mm ):";
// Cin> f;
F = 153.24;
T = m * f;
// Cout <"T =" <T <endl;
R = 0.25 * (X [1] + X [2] + X [3] + X [4]);
// Cout <"R =" <R <endl;
S = 0.25 * (Y [1] + Y [2] + Y [3] + Y [4]);
// Cout <"S =" <S <endl;
Cout <"Step 1: Initialize the six external orientation elements:" <endl;
Cout <"r = 0" <endl;
Cout <"s = 0" <endl;
Cout <"t = 0" <endl;
Cout <"R =" <R <endl;
Cout <"S =" <S <endl;
Cout <"T =" <T <endl;
Double a1, a2, a3, b1, b2, b3, c1, c2, c3;
A1 = cos (r) * cos (t)-sin (r) * sin (s) * sin (t );
A2 =-cos (r) * sin (t)-sin (r) * sin (s) * cos (t );
A3 =-sin (r) * cos (s );
B1 = cos (s) * sin (t );
B2 = cos (s) * cos (t );
B3 =-sin (s );
C1 = sin (r) * cos (t) + cos (r) * sin (s) * sin (t );
C2 =-sin (r) * sin (t) + cos (r) * sin (s) * cos (t );
C3 = cos (r) * cos (s );
Cout <"Step 2: The rotation matrix is successfully calculated as follows:" <endl;
Cout <"a1 =" <a1 <"" <"a2 =" <a2 <"<" a3 = "<a3 <endl;
Cout <"b1 =" <b1 <"" <"b2 =" <b2 <"<" b3 = "<b3 <endl;
Cout <"c1 =" <c1 <"" <"c2 =" <c2 <"<" c3 = "<c3 <endl;
// Calculate the approximate value of the image point coordinate point by point
Double u [5], v [5], U [5], V [5], W [5];
For (I = 1; I <= 4; I ++)
{
U [I] = a1 * (X [I]-R) + b1 * (Y [I]-S) + c1 * (Z [I]-T );
V [I] = a2 * (X [I]-R) + b2 * (Y [I]-S) + c2 * (Z [I]-T );
W [I] = a3 * (X [I]-R) + b3 * (Y [I]-S) + c3 * (Z [I]-T );
U [I] =-f * U [I]/W [I];
V [I] =-f * V [I]/W [I];
}
Cout <"Step 3: the approximate value of the coordinate points is successfully calculated, as follows:" <endl;
For (I = 1; I <= 4; I ++)
{
Cout <"(" <u [I] <"," <v [I] <")" <endl;
}
// Calculate the coefficient matrix
Double a [10] [10];
For (I = 1; I <= 4; I ++)
{
A [2 * I-1] [1] = (a1 * f + a3 * x [I])/W [I];
A [2 * I-1] [2] = (b1 * f + b3 * x [I])/W [I];
A [2 * I-1] [3] = (c1 * f + c3 * x [I])/W [I];
A [2 * I-1] [4] = y [I] * sin (s)-cos (s) * (x [I] * (x [I] * cos (t)-y [I] * sin (t)/f + f * cos (t ));
A [2 * I-1] [5] =-f * sin (t)-x [I] * (x [I] * sin (t) + y [I] * cos (t)/f;
A [2 * I-1] [6] = y [I];
A [2 * I] [1] = (a2 * f + a3 * y [I])/W [I];
A [2 * I] [2] = (b2 * f + b3 * y [I])/W [I];
A [2 * I] [3] = (c2 * f + c3 * y [I])/W [I];
A [2 * I] [4] =-x [I] * sin (s)-cos (s) * (y [I] * (x [I] * cos (t)-y [I] * sin (t)/f-f * sin (t ));
A [2 * I] [5] =-f * cos (t)-y [I] * (x [I] * sin (t) + y [I] * cos (t)/f;
A [2 * I] [6] =-x [I];
}
Cout <"Step 4: the calculation of the coefficient matrix is completed as follows:" <endl;
For (I = 1; I <= 4; I ++)
{
Cout <a [2 * I-1] [1] <"<a [2 * I-1] [2] <" "<a [2 * I-1] [3] <"<a [2 * I-1] [4] <" <a [2 * I-1] [5] <"<[ 2 * I-1] [6] <endl;
Cout <a [2 * I] [1] <"" <a [2 * I] [2] <"" <a [2 * I] [3] <"<a [2 * I] [4] <" "<a [2 * I] [5] <" "<[ 2 * I] [6] <endl;
}
// Calculate the constant
Double l [10];
For (I = 1; I <= 8; I ++)
{
L [2 * I-1] = x [I]-u [I];
L [2 * I] = y [I]-v [I];
}
Cout <"Step 4: the calculation of the matrix of constant items ends, as follows:" <endl;
For (I = 1; I <= 8; I ++)
{
Cout <l [I] <endl;
}
// Transpose the coefficient matrix
Int q = 0;
Double sum [100] [100], A [100] [100];
For (I = 1; I <= 8; I ++)
{
For (int j = 1; j <= 6; j ++)
{
A [j] [I] = a [I] [j];
}
}
Cout <"Step 5: the coefficient matrix transpose result is as follows:" <endl;
For (I = 1; I <= 6; I ++)
{
For (int j = 1; j <= 8; j ++)
{
Cout <A [I] [j] <"";
}
Cout <endl;
}
// Calculate the product of the coefficient matrix and its transpose matrix
For (I = 1; I <= 6; I ++)
{
For (int k = 1; k <= 6; k ++)
{
Sum [I] [k] = 0;
For (int j = 1; j <= 8; j ++)
{
Sum [I] [k] = sum [I] [k] + A [I] [j] * a [j] [k];
}
}
}
Cout <"Step 6: the product of the coefficient matrix and its transpose matrix is as follows:" <endl;
For (I = 1; I <= 6; I ++)
{
For (int j = 1; j <= 6; j ++)
{
Cout <sum [I] [j] <"";
}
Cout <endl;
}
// Calculate the product of the coefficient matrix and the constant matrix
Double mun [10];
For (I = 1; I <= 6; I ++)
{
Mun [I] = 0;
For (int j = 1; j <= 8; j ++)
{
Mun [I] = mun [I] + A [I] [j] * l [j];
}
}
Cout <"Step 7: The product of the coefficient matrix and the constant matrix is as follows:" <endl;
For (I = 1; I <= 6; I ++)
{
Cout <mun [I] <endl;
}
NJZ (sum, l );
Return 0;
}
Double NJZ (double sum [100] [100], double l [10])
{
Int I, j, n = 6, q, k, c = 0;
Double A [100], C [100] [100], B [100] [100], G [100] [100], K [10];
For (I = 1; I <= n; I ++)
{
For (j = n + 1; j <= 2 * n; j ++)
{
// Judge
If (I = j-n)
{
Sum [I] [j] = 1;
}
Else
{
Sum [I] [j] = 0;
}
}
}
// Cout <"Step 1: configure the unit matrix for the original matrix. The result is as follows:" <endl;
// For (I = 1; I <= n; I ++)
//{
// For (j = 1; j <= 2 * n; j ++)
//{
// Cout <sum [I] [j] <"";
//}
// Cout <endl;
//}
For (j = 1; j <= n; j ++) // Column
{
For (I = 1; I <= n; I ++)
{
If (I! = J)
{
If (sum [I] [j]! = 0)
{
For (k = 1; k <= 2 * n; k ++)
{
B [I] [k] = sum [I] [k]-sum [j] [k] * sum [I] [j]/sum [j] [j];
}
For (k = 1; k <= 2 * n; k ++)
{
Sum [I] [k] = B [I] [k];
}
}
}
Else
{
If (sum [I] [j] = 0) // if the diagonal line is = 0
{
For (q = 1; q <= n; q ++) // start from row 1st and find the rows not 0 and exchange them
{
If (sum [q] [j]! = 0)
{
For (k = 1; k <= 2 * n; k ++)
{
C ++;
A [k] = sum [I] [k];
Sum [I] [k] = sum [q] [k];
Sum [q] [k] = A [k];
}
Break;
}
}
}
}
// If (j = 3 & I = 3)
//{
// Cout <a [1] [1] <"" <a [1] [2] <"" <a [1] [3] <"" <a [1] [4] <"" <a [1] [5] <"" <a [1] [6] <endl;
// Cout <a [2] [1] <"" <a [2] [2] <"" <a [2] [3] <"" <a [2] [4] <"" <a [2] [5] <"" <a [2] [6] <endl;
// Cout <a [3] [1] <"" <a [3] [2] <"" <a [3] [3] <"" <a [3] [4] <"" <a [3] [5] <"" <a [3] [6] <endl;
//}
}
}
For (j = 1; j <= n; j ++)
{
If (sum [j] [j]! = 1)
{
For (k = 1; k <= 2 * n; k ++)
{
Sum [j] [k] = sum [j] [k]/sum [j] [j];
}
}
}
Cout <"Step 8: Calculate the inverse matrix result of the product of the coefficient matrix and its inverse matrix as follows:" <endl;
For (I = 1; I <= n; I ++)
{
For (j = 1; j <= 2 * n; j ++)
{
If (sum [I] [j] <0.00001)
{
Sum [I] [j] = 0;
}
}
}
For (I = 1; I <= n; I ++)
{
For (j = 1; j <= n; j ++)
{
G [I] [j] = sum [I] [j + n];
}
}
For (I = 1; I <= n; I ++)
{
For (j = 1; j <= n; j ++)
{
Cout <G [I] [j] <"";
}
Cout <endl;
}
// Calculates the number of corrections for six out-of-range elements.
For (I = 1; I <= 6; I ++)
{
K [I] = 0;
For (j = 1; j <= 6; j ++)
{
K [I] = K [I] + G [I] [j] * l [j];
}
}
Cout <"Step 9: The number of corrections for the six out-of-range elements is as follows:" <endl;
For (I = 1; I <= 6; I ++)
{
Cout <K [I] <endl;
}
Return 0;
}