https://blog.csdn.net/qq_37554556/article/details/88363572基于这位优秀的博主有感而发，写完后和老师的答案还是不太一样，正在找原因中。
//前方交会#include<stdio.h>#include<iostream>#include<opencv2/opencv.hpp>#include<math.h>using namespace std;using namespace cv; int main(){//定义同名像点的坐标double image[2][2] = { 0.0 };double x1 = 29.214000;double y1 = -47.640000;double x2 = 3.936000;double y2 = -47.910000;x1 = image[0][0];y1 = image[0][1];x1 = image[1][0];y1 = image[1][1];//定义内方位元素double x0 = 0.0;double y0 = 0.0;double f = 70.50;//定义外方位元素double W_ele[2][6] = { {432438.565, 3894841.049, 1274.012 , -0.005785603 , 0.0003784083 , -0.01880947 },{ 432847.537 , 3894836.718 , 1272.414 , -0.006604103 , 0.0005394704 , 0.0007619388} };double XS1 = W_ele[0][0];double YS1 = W_ele[0][1];double ZS1 = W_ele[0][2];double phi1 = W_ele[0][3];double omig1 = W_ele[0][4];double kappa1 = W_ele[0][5];double XS2 = W_ele[1][0];double YS2 = W_ele[1][1];double ZS2 = W_ele[1][2];double phi2 = W_ele[1][3];double omig2 = W_ele[1][4];double kappa2 = W_ele[1][5];//计算基线分量double Bx = XS2 - XS1;double By = YS2 - YS1;double Bz = ZS2 - ZS1;//cout << kappa1 << endl;//确定外方位角元素double a1 = cos(phi1)*cos(kappa1) - sin(phi1)*sin(omig1)*sin(kappa1);double a2= -1.0*cos(phi1)*sin(kappa1) - sin(phi1)*sin(omig1)*cos(kappa1);double a3= -1.0*sin(phi1)*cos(omig1);double b1= cos(omig1)*sin(kappa1);double b2= cos(omig1)*cos(kappa1);double b3 = -1.0*sin(omig1);double c1 = sin(phi1)*cos(kappa1) + cos(phi1)*sin(omig1)*sin(kappa1);double c2 = -1.0*sin(phi1)*sin(kappa1) + cos(phi1)*sin(omig1)*cos(kappa1);double c3 = cos(phi1)*cos(omig1);//定义旋转矩阵R1Mat R1(3, 3, CV_64F,0.0);R1.at<double>(0, 0) = a1;R1.at<double>(0, 1) = a2;R1.at<double>(0, 2) = a3;R1.at<double>(1, 0) = b1;R1.at<double>(1, 1) = b2;R1.at<double>(1, 2) = b3;R1.at<double>(2, 0) = c1;R1.at<double>(2, 1) = c2;R1.at<double>(2, 2) = c3;double aa1 = cos(phi2)*cos(kappa1) - sin(phi2)*sin(omig2)*sin(kappa2);double aa2 = -1.0*cos(phi2)*sin(kappa1) - sin(phi2)*sin(omig2)*cos(kappa2);double aa3 = -1.0*sin(phi2)*cos(omig1);double bb1 = cos(omig2)*sin(kappa1);double bb2 = cos(omig2)*cos(kappa1);double bb3 = -1.0*sin(omig2);double cc1 = sin(phi2)*cos(kappa2) + cos(phi2)*sin(omig2)*sin(kappa2);double cc2 = -1.0*sin(phi2)*sin(kappa2) + cos(phi2)*sin(omig2)*cos(kappa2);double cc3 = cos(phi2)*cos(omig2);//定义旋转矩阵R2Mat R2(3, 3, CV_64F, 0.0);R2.at<double>(0, 0) = aa1;R2.at<double>(0, 1) = aa2;R2.at<double>(0, 2) = aa3;R2.at<double>(1, 0) = bb1;R2.at<double>(1, 1) = bb2;R2.at<double>(1, 2) = bb3;R2.at<double>(2, 0) = cc1;R2.at<double>(2, 1) = cc2;R2.at<double>(2, 2) = cc3;//printf("a1=%lf", aa1);//定义同名像点的像空间辅助坐标系RR1(X1，Y1，Z1)和RR2(X2，Y2，Z)Mat RR1(3, 1, CV_64F, 0.0);Mat RR2(3, 1, CV_64F, 0.0);Mat RR11(3, 1, CV_64F, 0.0);RR11.at<double>(0, 0) = x1;RR11.at<double>(1, 0) = y1;RR11.at<double>(2, 0) = -f;Mat RR22(3, 1, CV_64F, 0.0);RR22.at<double>(0, 0) = x2;RR22.at<double>(1, 0) = y2;RR22.at<double>(2, 0) = -f;RR1 = R1*RR11;double X1 = RR1.at<double>(0, 0);double Y1 = RR1.at<double>(1, 0);double Z1 = RR1.at<double>(2, 0);RR2 = R2*RR22;double X2 = RR2.at<double>(0, 0);double Y2 = RR2.at<double>(1, 0);double Z2 = RR2.at<double>(2, 0);cout << "左像空间辅助坐标系：" << endl;cout << X1 << endl;cout << Y1 << endl;cout << Z1 << endl;cout << "右像空间辅助坐标系：" << endl;cout << X2 << endl;cout << Y2 << endl;cout << Z2 << endl;//计算点投影系数double N1 = (Bx*Z2 - Bz*X2) / (X1*Z2 - Z1*X2);double N2 = (Bx*Z1 - Bz*X1) / (X1*Z2 - Z1*X2);//计算模型点坐标double deteX = N1*X1;double deteY = 0.5*(N1*Y1 + N2*Y2 + By);double deteZ = N1*Z1;cout << "模型点坐标：" << endl;cout << deteX << endl;cout << deteY << endl;cout << deteZ << endl;//计算地面点的地面坐标（X，Y，Z）double XP = XS1 + deteX;double YP = YS1 + deteY;double ZP = ZS1 + deteZ;cout << "地面点坐标是：" << endl;cout <<"XP="<< XP << endl;cout <<"YP="<< YP<< endl;cout << "ZP="<<ZP<< endl;//std::cout << RR1 << std::endl;getchar();return 0;}//运行结果左像空间辅助坐标系：-0.4078830.0266778-70.4988右像空间辅助坐标系：2.56837-47.9375-70.5505模型点坐标：56.07533289.339692.11地面点坐标是：XP=432495YP=3.89813e+06ZP=10966.1正确结果是：