[Photography surveying space rear rendezvous operation] solution procedure

Source: Internet
Author: User

[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;
}

Contact Us

The content source of this page is from Internet, which doesn't represent Alibaba Cloud's opinion; products and services mentioned on that page don't have any relationship with Alibaba Cloud. If the content of the page makes you feel confusing, please write us an email, we will handle the problem within 5 days after receiving your email.

If you find any instances of plagiarism from the community, please send an email to: info-contact@alibabacloud.com and provide relevant evidence. A staff member will contact you within 5 working days.

A Free Trial That Lets You Build Big!

Start building with 50+ products and up to 12 months usage for Elastic Compute Service

  • Sales Support

    1 on 1 presale consultation

  • After-Sales Support

    24/7 Technical Support 6 Free Tickets per Quarter Faster Response

  • Alibaba Cloud offers highly flexible support services tailored to meet your exact needs.