Using C++ to realize rear intersection of single-chip space

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