空间前方交会（利用相机外方位元素和像点坐标进行解算）

Cheng_Star

已于 2023-05-04 23:55:20 修改

阅读量3.2k

点赞数 4

文章标签：算法 c++

于 2023-04-14 20:37:46 首次发布

本文链接：https://blog.csdn.net/cmxandsy/article/details/130159938

版权

一、前言

在摄影测量过程中，得到相机的外方位元素以及地面控制点对应的像点坐标之后，如何解算地面控制点坐标？
在研究过程中，可以利用空间误差处理中的平差知识以及计算机编程进行实现。

二、空间前方交会

1. 前方交会的概念

利用立体像对两张像片的内方位元素、同名像点坐标和像对的相对方位元素(或外方位元素)解算模型点坐标(或地面点坐标)的工作，称为空间前方交会。在摄影测量中主要有两种：
(1). 利用立体像对两张像片的相对方位元素，计算模型点的三位坐标。
(2). 利用立体像对两张像片的外方位元素，计算地面点的地面坐标。
而在本文章中，将重点介绍在得到相机的外方位元素以及地面控制点对应的像点坐标，利用平差的知识，求算出地面控制点的坐标。

2. 基本公式

由于共线方程大家都已知，就不在这里赘述。这里主要列出误差方程、误差系数以及旋转矩阵。

误差方程
$\begin{align} v_{x1} &= \frac{\partial x_1}{\partial X}\Delta X+\frac{\partial x_1}{\partial Y}\Delta Y+\frac{\partial x_1}{\partial Z}\Delta Z-l_{x1}\\ v_{y1} &= \frac{\partial y_1}{\partial X}\Delta X+\frac{\partial y_1}{\partial Y}\Delta Y+\frac{\partial y_1}{\partial Z}\Delta Z-l_{y1}\\ v_{x2} &= \frac{\partial x_2}{\partial X}\Delta X+\frac{\partial x_2}{\partial Y}\Delta Y+\frac{\partial y_2}{\partial Z}\Delta Z-l_{x2}\\ v_{y2} &= \frac{\partial y_2}{\partial X}\Delta X+\frac{\partial y_2}{\partial Y}\Delta Y+\frac{\partial y_2}{\partial Z}\Delta Z-l_{y2}\\ &\dotsb\nonumber \\ v_{xn} &= \frac{\partial x_n}{\partial X}\Delta X+\frac{\partial x_n}{\partial Y}\Delta Y+\frac{\partial x_n}{\partial Z}\Delta Z-l_{xn} \tag{2n-1}\\ v_{yn} &= \frac{\partial y_n}{\partial X}\Delta X+\frac{\partial y_n}{\partial Y}\Delta Y+\frac{\partial y_n}{\partial Z}\Delta Z-l_{yn} \tag{n} \end{align}$
误差系数
$\begin{align} \frac{\partial x}{\partial X} &=-\frac{1}{Z}(a_1f_x+a_3x)\tag{1}\\ \frac{\partial x}{\partial Y} &=-\frac{1}{Z}(b_1f_x+b_3x)\tag{2}\\ \frac{\partial x}{\partial Z} &=-\frac{1}{Z}(c_1f_x+c_3x)\tag{3}\\ \frac{\partial y}{\partial X} &=-\frac{1}{Z}(a_2f_y+a_3y)\tag{4}\\ \frac{\partial y}{\partial Y} &=-\frac{1}{Z}(b_2f_y+b_3y)\tag{5}\\ \frac{\partial y}{\partial Z} &=-\frac{1}{Z}(c_2f_y+c_3y)\tag{6} \end{align}$
旋转矩阵
$\begin{equation} \begin{bmatrix} a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \\ c_1 & c_2 & c_3 \end{bmatrix} = \begin{bmatrix} cos\phi\cos\kappa -sin\phi sin\omega cos\kappa & -cos\phi sin\kappa -sin\phi sin\omega cos\kappa & -sin\phi cos\omega \\ cos\phi sin\kappa & cos\omega cos\kappa & -sin\omega\\ sin\phi\cos\kappa +cos\phi sin\omega sin\kappa & -sin\phi sin\kappa +cos\phi sin\omega cos\kappa & cos\phi cos\omega\tag{1} \end{bmatrix} \end{equation}$

三、代码展示

#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include<Eigen/Dense>
#include<math.h>
#include <iomanip> 

/*
1.为了便于矩阵运算，采用了Eigen库来进行矩阵的转置求逆等运算
*/

using namespace std;
using namespace Eigen;

const double f = 105.200;

struct Img_Parameter//定义相机外方位元素
{
    int ImageID;
    double Xs;
    double Ys;
    double Zs;
    double Phi;
    double Omega;
    double Kappa;
};

struct Point3D  //定义控制点地面坐标
{
    string PointName;
    double X;
    double Y;
    double Z; 
};

struct image //影像坐标
{
    int ID;
    double x;
    double y;
};


//函数声明
void Read_phtData(string filename, vector<Img_Parameter>& imgPara_data);//读取相机外方位元素

void Read_ptsData(string filename, vector<Point3D>& pts_data, vector<image>& img_data, vector<int>& ImagePointsNumber);//读取地面控制点X、Y、Z

void FindID(const vector<Img_Parameter>& imgPara_data, const vector<image>& img_data,//将像点坐标所对应相机的外方位元素取出并整理成子集
           vector<Img_Parameter>& subImgPara_data);                                  //subImgPara_data方便进行条件平差

void condition_adjustment(const vector<Point3D>& pts_data, const vector<image>& img_data,//进行条件平差
                const vector<Img_Parameter>& imgPara_data, const vector<int>& ImagePointsNumber,vector<Point3D>& my_data);

void WriteToFile(vector<Point3D>& my_data, const string& filename);//写文件到“output.txt”



int main()
{
    string file1 = "Data.pht";
    string file2 = "Data.pts";
    string outputfile = "output.txt";
    vector<Img_Parameter> imgPara_data;//将相机的内方位元素读取进img_data
    vector<Point3D> pts_data,my_data;//将控制点的物方坐标存储进pts_data
    vector<image> img_data;//读取影像坐标
    vector<int> ImagePointsNumber;//将每个点对应的影像数存储

    Read_phtData(file1, imgPara_data);
    Read_ptsData(file2, pts_data, img_data, ImagePointsNumber);
    condition_adjustment(pts_data, img_data, imgPara_data, ImagePointsNumber, my_data);
    WriteToFile(my_data, outputfile);
  
    return 0;
}

void Read_phtData(string filename, vector<Img_Parameter>& imgPara_data)
{

    ifstream infile1(filename);

    if (!infile1)
    {
        cout << "Cannot open file: " << filename << endl;
    }

    string line;//用getline读取每一行
    int data_count = 0;


    while (getline(infile1, line)) {
        // 忽略以 '$' 开头的注释行
        istringstream count(line);
        if (line[0] == '$') {
            continue;
        }
        else
        {
            count >> data_count;
            break;
        }

    }

    for (int i = 0; i < data_count; i++)
    {
        getline(infile1, line);       //从读取到Image个数之后下一行开始读
        istringstream iss(line);
        Img_Parameter image;
        iss >> image.ImageID >> image.Xs >> image.Ys >> image.Zs
            >> image.Phi >> image.Omega >> image.Kappa;

        imgPara_data.push_back(image);//将其存入数组中

    }
}


void Read_ptsData(string filename, vector<Point3D>& pts_data, vector<image>& img_data, vector<int>& ImagePointsNumber)
{
    ifstream file(filename);

    if (!file)
    {
        cout << "Cannot open file." << endl;
    }

    string line;//用来读取每一行设置的字符串
    int Point_Number = 0;

    while (getline(file, line))
    {
        if (line[0] == '$')
            continue;
        else
        {
            istringstream iss(line);
            iss >> Point_Number;
            break;
        }

    }

    for (int i = 0; i < Point_Number; i++)
    {
        getline(file, line);
        istringstream is(line);
        Point3D pts;
        is >> pts.PointName
            >> pts.X >> pts.Y >> pts.Z;
        pts_data.push_back(pts);

        int img_pts_Num;//设置照片点数

        getline(file, line);
        istringstream img_pts_num(line);
        img_pts_num >> img_pts_Num;

        ImagePointsNumber.push_back(img_pts_Num);//将每个点对应的影像数存储

        for (int j = 0; j < img_pts_Num; j++)
        {
            image img;
            getline(file, line);
            istringstream is_image(line);

            is_image >> img.ID >> img.x >> img.y;
            img_data.push_back(img);

        }
    }

}


void condition_adjustment( const vector<Point3D>& pts_data, const vector<image>& img_data,
               const vector<Img_Parameter>& imgPara_data, const vector<int>& ImagePointsNumber, vector<Point3D>& my_data)
{

    int i = 0;
    int start = 0;
    

    for (auto it1 = pts_data.begin(); it1 != pts_data.end(); ++it1,i++)//遍历循环X, Y, Z
    {
        int o = 1;
        vector<Img_Parameter> subImgPara_data;//用来取出对应image的内方位元素
        Point3D mydata;
        MatrixXd X(3,1);
        mydata.PointName = it1->PointName;
        mydata.X = 720000;
        mydata.Y = 4340000;
        mydata.Z = 1000;
                                          
        
            int size = ImagePointsNumber[i];
            vector<image> subimage(img_data.begin() + start, img_data.begin() + start + size);

            // cout << "subimage " << i+1 << ":" << start<<" " << size<<endl;
            start += size;
            FindID(imgPara_data, subimage, subImgPara_data);//找到对应的外方位元素,这里的即为图片所对应的外方位元素

            do {
            double* _X = new double[size];//设置动态
            double* _Y = new double[size];
            double* _Z = new double[size];
            double* col_equa_x = new double[size];
            double* col_equa_y = new double[size];
            MatrixXd A(size * 2, 3);
            MatrixXd l(size * 2, 1);



            int j = 0;
            for (auto it2 = subImgPara_data.begin(); it2 != subImgPara_data.end(); ++it2, j++)
            {

                double R[3][3] = { 0.0 };	//旋转矩阵R
                R[0][0] = cos(it2->Phi) * cos(it2->Kappa) - sin(it2->Phi) * sin(it2->Omega) * sin(it2->Kappa);
                R[0][1] = -cos(it2->Phi) * sin(it2->Kappa) - sin(it2->Phi) * sin(it2->Omega) * cos(it2->Kappa);
                R[0][2] = -sin(it2->Phi) * cos(it2->Omega);
                R[1][0] = cos(it2->Omega) * sin(it2->Kappa);
                R[1][1] = cos(it2->Omega) * cos(it2->Kappa);
                R[1][2] = -sin(it2->Omega);
                R[2][0] = sin(it2->Phi) * cos(it2->Kappa) + cos(it2->Phi) * sin(it2->Omega) * sin(it2->Kappa);
                R[2][1] = -sin(it2->Phi) * sin(it2->Kappa) + cos(it2->Phi) * sin(it2->Omega) * cos(it2->Kappa);
                R[2][2] = cos(it2->Phi) * cos(it2->Omega);

                _X[j] = R[0][0] * (mydata.X - it2->Xs) + R[1][0] * (mydata.Y - it2->Ys)
                    + R[2][0] * (mydata.Z - it2->Zs);
                _Y[j] = R[0][1] * (mydata.X - it2->Xs) + R[1][1] * (mydata.Y - it2->Ys)
                    + R[2][1] * (mydata.Z - it2->Zs);
                _Z[j] = R[0][2] * (mydata.X - it2->Xs) + R[1][2] * (mydata.Y - it2->Ys)
                    + R[2][2] * (mydata.Z - it2->Zs);

                col_equa_x[j] = -f * _X[j] / _Z[j];
                col_equa_y[j] = -f * _Y[j] / _Z[j];

                A(j * 2, 0) = -(1 / _Z[j]) * (R[0][0] * f + R[0][2] * col_equa_x[j]);
                A(j * 2, 1) = -(1 / _Z[j]) * (R[1][0] * f + R[1][2] * col_equa_x[j]);
                A(j * 2, 2) = -(1 / _Z[j]) * (R[2][0] * f + R[2][2] * col_equa_x[j]);
                A(j * 2 + 1, 0) = -(1 / _Z[j]) * (R[0][1] * f + R[0][2] * col_equa_y[j]);
                A(j * 2 + 1, 1) = -(1 / _Z[j]) * (R[1][1] * f + R[1][2] * col_equa_y[j]);
                A(j * 2 + 1, 2) = -(1 / _Z[j]) * (R[2][1] * f + R[2][2] * col_equa_y[j]);


                l(j * 2, 0) = subimage[j].x + f * _X[j] / _Z[j];
                l(j * 2 + 1, 0) = subimage[j].y + f * _Y[j] / _Z[j];
            }

            X = (A.transpose() * A).inverse() * A.transpose() * l;

            mydata.X += X(0,0);
            mydata.Y += X(1,0);
            mydata.Z += X(2,0);

           // cout << "间接平差系数B:" << A.format(Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ",\n", "", "")) << endl;

            //cout << "第" << i + 1 << "组控制点坐标:" << endl;
          //  cout << fixed << setprecision(6) << X(0,0) <<" "<<X(1,0)<<" "<<X(2,0)<< endl;
            
            //cout << o++ << endl;

            delete[]_X;
            delete[]_Y;
            delete[]_Z;
            delete[]col_equa_x;
            delete[]col_equa_y;

        } while (abs(mydata.X - it1->X) > 0.01 || abs(mydata.Y - it1->Y) > 0.01 || abs(mydata.Z - it1->Z) > 0.01);

       /* cout << "控制点" << it1->PointName << "坐标:" << endl;
        cout <<"X: " << setprecision(9) << mydata.X << endl;
        cout <<"Y: " << setprecision(10) << mydata.Y << endl;
        cout <<"Z: " << setprecision(7) << mydata.Z << endl;
        cout << endl;*/

        my_data.push_back(mydata);

        subImgPara_data.clear();
    }
}


void FindID(const vector<Img_Parameter>& imgPara_data, const vector<image>& img_data,
            vector<Img_Parameter>& subImgPara_data)
{
    for (int i = 0; i < img_data.size(); i++)
    {
        for (auto it1 = imgPara_data.begin(); it1 != imgPara_data.end(); ++it1)
        {
            if (img_data[i].ID == it1->ImageID)
            {
                subImgPara_data.push_back(*it1);
            }
        }
    }

}

void WriteToFile(vector<Point3D>& my_data, const string& filename)
{
    ofstream outfile;
    outfile.open(filename, ios::out || ios::trunc);

    if (!outfile.is_open()) {
        cerr << "Error: failed to open the file " << filename << endl;
        return;
    }

    for (auto it = my_data.begin(); it != my_data.end(); ++it)
    {       
        outfile << "控制点" << it->PointName << "坐标:" << endl;
        outfile << "X: " << setprecision(9) << it->X << endl;
        outfile << "Y: " << setprecision(10) << it->Y << endl;
        outfile << "Z: " << setprecision(7) << it->Z << endl;
        outfile << endl;
    }
}

四、小结

此算法很好的完成了所需任务，将与实际测量的误差控制在0.01m以内，达到了很高的精度。同时本算法也具有健壮性和完备性，用相同数据格式的数据集便可以达到相同的效果。
但是由于编程简便性，并没有将焦距等信息用函数读取，而是当作已知量直接进行计算。这也是以后值得完善的地方。
在编程的过程中，由于没有注意到各个变量的单位变化，导致循环迭代一直发散并不收敛，经过多次调试演算，才发现这一错误。同时也让我在接下来的学习过程中更加注意这些细节，从而完善自己。