立体像对空间前方交会算法

 

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

	//定义旋转矩阵R1
	Mat 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);

	//定义旋转矩阵R2
	Mat 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.407883
0.0266778
-70.4988
右像空间辅助坐标系：
2.56837
-47.9375
-70.5505
模型点坐标：
56.0753
3289.33
9692.11
地面点坐标是：
XP=432495
YP=3.89813e+06
ZP=10966.1

正确结果是：