Overall process
The specific flow chart of code implementation is as follows:
Data processing and class object creation
The data provided is as follows:
The four control points and image points are as shown in the figure, the focal length f = 153.24 mm, and the scale bar m = 50000. During the programming process, we use three objects (classes) for processing, namely imagePoint, terrainPoint, and Resection. The first two classes represent image points and ground points, and the Resection class represents the rear intersection process, including variables and functions. The specific definitions are as follows:
class imagePoint // Image point, provide data in millimeters, pay attention to unit conversion { public: double x; double y; imagePoint(double _x, double _y) :x(_x), y(_y) {} }; class terrainPoint // Ground point, provided data is meters { public: double x; double y; double z; terrainPoint(double _x, double _y, double _z) :x(_x), y(_y), z(_z) {} }; class Resection // rear intersection { public: // variable part double m = 50000; // scale m=50000 double f = 153.24 / 1000; // Focal length f=153.24mm, converted into meters double x0, y0 = 0; // inner element double phi = 0; // Corresponding to the Greek letter fai, three corner elements corresponding to the outer orientation elements double w = 0; double k = 0; // In the case of vertical photography, when the external azimuth element is a small angle, each coefficient can be solved approximately using phi = w = k = 0 double Xs = 0, Ys = 0, Zs = 0; // Line elements double m0 = 0; // error in unit weight vector<imagePoint> imagePoints; // store image points vector<terrainPoint> terrainPoints; //Storage ground points int numberP = 0; // Number of control points Matrix3d R = Matrix3d::Zero(3,3); // Rotation matrix VectorXdL; MatrixXdA; VectorXdX; VectorXdM; // function part int PointRead(string path_img, string path_ter); //Read the file, determine whether the number of image points is equal to the number of control points, store it in the container, and return the number of control points void GetInitialValue(vector<terrainPoint> terrainPoints, double m, double f, int numberP); // Get the initial values of Xs, Ys, Zs, phi, w, k Matrix3d GetRotateMat(double phi, double w, double k); // Get the rotation matrix void GetAL(vector<imagePoint> imagePoints, vector<terrainPoint> terrainPoints, double f, int numberP, Matrix3d R); //Get matrix A and vector L bool GetX(MatrixXd A, VectorXd L, int numberP); // Get the correction value x and determine whether the end condition is met };
Data reading
Implemented through the Resection::Pointread() function, the coordinates of the points are stored in the txt file in the following format. Enter the image point and control point file paths, read the points into the above object, and return the number of points. The point coordinate format is as follows
Outer orientation element initialization
Referring to P63 of “Photogrammetry (Third Edition)”, we can get the method for determining the initial value of the outer orientation element of vertical aerial photography as follows:
In the code, through the Resection::GetInitialValue() function, we can get the initial value of the outer orientation element by inputting four variables: ground point coordinates, scale, focal length and number of control points, and assign them directly to the class.
Calculate rotation matrix
Referring to P44 of “Photogrammetry (Third Edition)”, we can get the matrix multiplication calculation formula.
Implemented in the code through the Resection::GetRotateMat() function, we obtain the rotation matrix by inputting the three corner elements of the outer orientation element and assign it to the class.
Calculate A and L
Refer to P62 of “Photogrammetry (Third Edition)”. For each control point, the corresponding A and L are calculated as follows:
Among them, l represents the observed value of the image point coordinate minus the approximate value calculated using the control point coordinate.
In the code, it is implemented through the function Resection::GetAL(), inputting variables such as ground points and image points, focal length, number of points, and rotation matrix, to obtain the matrix A and vector L, and assign them directly to the class.
Solve X and accuracy
Refer to P62 of “Photogrammetry (Third Edition)” to get the adjustment formula:
It is implemented in the code through the function Resection::GetX(). By inputting matrix A, matrix L and point variables, X and various precisions are obtained, and assigned directly to the class. The function returns a Boolean value, true when the end condition is met, false otherwise.
Loop solution
Select your own path for the path. The data format in the txt document you click is selected as above.
int main() { Resection hyh; hyh.numberP = hyh.PointRead("C:/Users/86151/Desktop/resection_hyh/imagePoint.txt", "C:/Users/86151/Desktop/resection_hyh/terrainPoint.txt"); // Read Get files hyh.GetInitialValue(hyh.terrainPoints, hyh.m, hyh.f, hyh.numberP); // Get the initial value int ciridx = 0; // Loop index, representing the number of loops bool judge = false; // Judge whether the loop ends while (1) { if (ciridx == 100) { cout << "The loop iteration exceeds 100 times. We think it does not converge and stop the loop" << endl; return 0; } hyh.R = hyh.GetRotateMat(hyh.phi, hyh.w, hyh.k); // Get the rotation matrix, the matrix in the class will also change hyh.GetAL(hyh.imagePoints, hyh.terrainPoints, hyh.f, hyh.numberP, hyh.R); // Get matrix A and matrix L and copy them to class judge = hyh.GetX(hyh.A, hyh.L, hyh.numberP); // Get the result, calculate the accuracy, and use the correction value as the initial value of the next cycle. The calculation is stored in class ciridx + + ; // increase the number of loops if (judge == true) { cout << "The result meets the requirements, stop the loop and output the result" << endl; break; } } cout << fixed << setprecision(5) << "The loop iterated:" << ciridx << "times, the results are as follows" << endl; cout << "\tXs=" << hyh.Xs << "\tYs=" << hyh.Ys << "\tZs=" << hyh.Zs << endl; cout << "\tphi=" << hyh.phi << "\tw=" << hyh.w << "\tk=" << hyh.k << endl; cout << endl; cout << "The precision is:" << endl; cout << "\tmXs=" << hyh.M(0) << "\tmYs=" << hyh.M(1) << "\tmZs=" << hyh .M(2) << endl; cout << "\tmphi=" << hyh.M(3) << "\tmw=" << hyh.M(4) << "\tmk=" << hyh .M(5) << endl; cout << endl; cout << "The unit error m0 is: " << hyh.m0 << endl; cout << endl; cout << "The rotation matrix R is: " << endl; cout << hyh.R << endl; cout << endl; cout << defaultfloat << "The correction values of the last adjustment are: " << endl; cout << setw(10) << left << "\tdXs=" << hyh.X(0) << setw(10) << left << "\tdYs=" << hyh .X(1) << setw(10) << left << "\tdZs=" << hyh.X(2) << endl; cout << setw(10) << left << "\tdphi=" << hyh.X(3) << setw(10) << left << "\tdw=" << hyh .X(4) << setw(10) << left << "\tdk=" << hyh.X(5) << endl; return 0; }
Total code
/* * Realization of space rear intersection * Reference: "Photogrammetry (Third Edition)" P63 */ #include#include #include #include #include using namespace Eigen; using namespace std; #defineRestrict1 1e-3 #defineRestrict2 1e-6 class imagePoint // Image point, provide data in millimeters, pay attention to unit conversion { public: double x; double y; imagePoint(double _x, double _y) :x(_x), y(_y) {} }; class terrainPoint // Ground point, provided data is meters { public: double x; double y; double z; terrainPoint(double _x, double _y, double _z) :x(_x), y(_y), z(_z) {} }; class Resection // rear intersection { public: // variable part double m = 50000; // scale m=50000 double f = 153.24 / 1000; // Focal length f=153.24mm, converted into meters double x0, y0 = 0; // inner element double phi = 0; // Corresponding to the Greek letter fai, three corner elements corresponding to the outer orientation elements double w = 0; double k = 0; // In the case of vertical photography, when the external azimuth element is a small angle, each coefficient can be solved approximately using phi = w = k = 0 double Xs = 0, Ys = 0, Zs = 0; // Line elements double m0 = 0; // error in unit weight vector<imagePoint> imagePoints; // store image points vector<terrainPoint> terrainPoints; //Storage ground points int numberP = 0; // Number of control points Matrix3d R = Matrix3d::Zero(3,3); // Rotation matrix VectorXdL; MatrixXdA; VectorXdX; VectorXdM; // function part int PointRead(string path_img, string path_ter); //Read the file, determine whether the number of image points is equal to the number of control points, store it in the container, and return the number of control points void GetInitialValue(vector<terrainPoint> terrainPoints, double m, double f, int numberP); // Get the initial values of Xs, Ys, Zs, phi, w, k Matrix3d GetRotateMat(double phi, double w, double k); // Get the rotation matrix void GetAL(vector<imagePoint> imagePoints, vector<terrainPoint> terrainPoints, double f, int numberP, Matrix3d R); //Get matrix A and vector L bool GetX(MatrixXd A, VectorXd L, int numberP); // Get the correction value x and determine whether the end condition is met }; int main() { Resection hyh; hyh.numberP = hyh.PointRead("C:/Users/86151/Desktop/resection_hyh/imagePoint.txt", "C:/Users/86151/Desktop/resection_hyh/terrainPoint.txt"); // Read Get files hyh.GetInitialValue(hyh.terrainPoints, hyh.m, hyh.f, hyh.numberP); // Get the initial value int ciridx = 0; // Loop index, representing the number of loops bool judge = false; // Judge whether the loop ends while (1) { if (ciridx == 100) { cout << "The loop iteration exceeds 100 times. We think it does not converge and stop the loop" << endl; return 0; } hyh.R = hyh.GetRotateMat(hyh.phi, hyh.w, hyh.k); // Get the rotation matrix, the matrix in the class will also change hyh.GetAL(hyh.imagePoints, hyh.terrainPoints, hyh.f, hyh.numberP, hyh.R); // Get matrix A and matrix L and copy them to class judge = hyh.GetX(hyh.A, hyh.L, hyh.numberP); // Get the result, calculate the accuracy, and use the correction value as the initial value of the next cycle. The calculation is stored in class ciridx + + ; // increase the number of loops if (judge == true) { cout << "The result meets the requirements, stop the loop and output the result" << endl; break; } } cout << fixed << setprecision(5) << "The loop iterated:" << ciridx << "times, the results are as follows" << endl; cout << "\tXs=" << hyh.Xs << "\tYs=" << hyh.Ys << "\tZs=" << hyh.Zs << endl; cout << "\tphi=" << hyh.phi << "\tw=" << hyh.w << "\tk=" << hyh.k << endl; cout << endl; cout << "The precision is:" << endl; cout << "\tmXs=" << hyh.M(0) << "\tmYs=" << hyh.M(1) << "\tmZs=" << hyh .M(2) << endl; cout << "\tmphi=" << hyh.M(3) << "\tmw=" << hyh.M(4) << "\tmk=" << hyh .M(5) << endl; cout << endl; cout << "The unit error m0 is: " << hyh.m0 << endl; cout << endl; cout << "The rotation matrix R is: " << endl; cout << hyh.R << endl; cout << endl; cout << defaultfloat << "The correction values of the last adjustment are: " << endl; cout << setw(10) << left << "\tdXs=" << hyh.X(0) << setw(10) << left << "\tdYs=" << hyh .X(1) << setw(10) << left << "\tdZs=" << hyh.X(2) << endl; cout << setw(10) << left << "\tdphi=" << hyh.X(3) << setw(10) << left << "\tdw=" << hyh .X(4) << setw(10) << left << "\tdk=" << hyh.X(5) << endl; return 0; } int Resection::PointRead(string path_img, string path_ter) { this->numberP = 0; // open a file ifstream img(path_img); ifstream ter(path_ter); assert(img.is_open() & amp; & amp; ter.is_open()); // Ensure both files can be opened double img_x, img_y; // coordinates of two points double ter_x, ter_y, ter_z; while (img >> img_x >> img_y & amp; & amp; ter >> ter_x >> ter_y >> ter_z) { img_x /= 1000; img_y /= 1000; // Unit conversion, changes according to the situation this->imagePoints.push_back(imagePoint(img_x, img_y)); this->terrainPoints.push_back(terrainPoint(ter_x, ter_y, ter_z)); this->numberP + + ; } return this->numberP; } void Resection::GetInitialValue(vector terrainPoints, double m, double f, int numberP) { this->Zs = m * f; this->Xs = 0; this->Ys = 0; for (int i = 0; i < numberP; i + + ) { this->Xs + = terrainPoints[i].x; this->Ys + = terrainPoints[i].y; } this->Xs /= numberP; this->Ys /= numberP; this->phi = 0; this->w = 0; this->k = 0; // Refer to "Photogrammetry (Third Edition)" P61 } Matrix3d Resection::GetRotateMat(double phi, double w, double k) { this->R = Matrix3d::Zero(3, 3); this->R(0, 0) = cos(phi) * cos(k) - sin(phi) * sin(w) * sin(k); this->R(0, 1) = -cos(phi) * sin(k) - sin(phi) * sin(w) * cos(k); this->R(0, 2) = -sin(phi) * cos(w); this->R(1, 0) = cos(w) * sin(k); this->R(1, 1) = cos(w) * cos(k); this->R(1, 2) = -sin(w); this->R(2, 0) = sin(phi) * cos(k) + cos(phi) * sin(w) * sin(k); this->R(2, 1) = -sin(phi) * sin(k) + cos(phi) * sin(w) * cos(k); this->R(2, 2) = cos(phi) * cos(w); return this->R; } void Resection::GetAL(vector imagePoints, vector terrainPoints, double f, int numberP, Matrix3d R) { int index = 0; this->A = MatrixXd::Zero(2 * numberP, 6); this->L = VectorXd::Zero(2 * numberP); for (int i = 0; i < numberP; i + + ) { //For each point, calculate its respective values double Xbar = R(0, 0) * (terrainPoints[i].x - this->Xs) + R(1, 0) * (terrainPoints[i].y - this->Ys) + R(2, 0 ) * (terrainPoints[i].z - this->Zs); double Ybar = R(0, 1) * (terrainPoints[i].x - this->Xs) + R(1, 1) * (terrainPoints[i].y - this->Ys) + R(2, 1 ) * (terrainPoints[i].z - this->Zs); double Zbar = R(0, 2) * (terrainPoints[i].x - this->Xs) + R(1, 2) * (terrainPoints[i].y - this->Ys) + R(2, 2 ) * (terrainPoints[i].z - this->Zs); this->L(index) = imagePoints[i].x + f * Xbar / Zbar; this->A(index, 0) = (R(0, 0) * f + R(0, 2) * (imagePoints[i].x - this->x0)) / Zbar; this->A(index, 1) = (R(1, 0) * f + R(1, 2) * (imagePoints[i].x - this->x0)) / Zbar; this->A(index, 2) = (R(2, 0) * f + R(2, 2) * (imagePoints[i].x - this->x0)) / Zbar; this->A(index, 3) = (imagePoints[i].y - this->y0) * sin(this->w) - ((imagePoints[i].x - this->x0) / f * ( (imagePoints[i].x - this->x0) * cos(this->k) - (imagePoints[i].y - this->y0) * sin(this->k)) + f * cos(this ->k)) * cos(this->w); this->A(index, 4) = -f * sin(this->k) - (imagePoints[i].x - this->x0) / f * ((imagePoints[i].x - this->x0 ) * sin(this->k) + (imagePoints[i].y - this->y0) * cos(this->k)); this->A(index, 5) = imagePoints[i].y - this->y0; index + + ; //For each point, there are two lines, this step represents entering the second line this->L(index) = imagePoints[i].y + f * Ybar / Zbar; this->A(index, 0) = (R(0, 1) * f + R(0, 2) * (imagePoints[i].y - this->y0)) / Zbar; this->A(index, 1) = (R(1, 1) * f + R(1, 2) * (imagePoints[i].y - this->y0)) / Zbar; this->A(index, 2) = (R(2, 1) * f + R(2, 2) * (imagePoints[i].y - this->y0)) / Zbar; this->A(index, 3) = -(imagePoints[i].x - this->x0) * sin(this->w) - ((imagePoints[i].y - this->y0) / f * ((imagePoints[i].x - this->x0) * cos(this->k) - (imagePoints[i].y - this->y0) * sin(this->k)) - f * sin( this->k)) * cos(this->w); this->A(index, 4) = -f * cos(this->k) - (imagePoints[i].y - this->y0) / f * ((imagePoints[i].x - this->x0 ) * sin(this->k) + (imagePoints[i].y - this->y0) * cos(this->k)); this->A(index, 5) = -(imagePoints[i].x - this->x0); index + + ; // The index continues to change and enters the next point } } bool Resection::GetX(MatrixXd A, VectorXd L, int numberP) { VectorXd V = VectorXd::Zero(2 * numberP); double m0 = 0; this->X = VectorXd::Zero(6); // Initialize the matrix this->M = VectorXd::Zero(6); MatrixXd Q = MatrixXd::Zero(6, 6); Q = (A.transpose() * A).inverse(); this->X = Q * A.transpose() * L; V = A * this->X - L; for (int i = 0; i < numberP; i + + ) { m0 + = V(i) * V(i); // add vv } m0 = sqrt(m0 / (2 * double(numberP) - 6)); // Get the error in unit weight for (int i = 0; i < 6; i + + ) { this->M(i) = sqrt(Q(i, i)) * m0; } this->m0 = m0; // Change the value to the corrected value this->Xs + = this->X(0); this->Ys + = this->X(1); this->Zs + = this->X(2); this->phi + = this->X(3); this->w + = this->X(4); this->k + = this->X(5); // Determine whether the loop ends if (X(0) < Restrict1 & amp; & amp; X(1) < Restrict1 & amp; & amp; X(2) < Restrict1 & amp; & amp; X(4) < Restrict2 & amp; & amp; X(5) < Restrict2) { return true; } else { return false; } }