前方交会之错误改正

错误终于找到了，一定要仔细耐心检查代码，不能掉以轻心

下一步，一定要弄会单像空间后方交会

#include<stdio.h>
#include<iostream>
#include<opencv2/opencv.hpp>
#include<iomanip>


using namespace std;
using namespace cv;
 
int main()
{
	//定义同名像点的坐标
	

	double x1 = 29.214000/1000.0;
	double y1 = -47.640000/1000.0;
	double x2 = 3.936000/1000.0;
	double y2 = -47.910000/1000.0;

	//定义内方位元素
	double x0 = 0.0;
	double y0 = 0.0;
	double f = 70.50/1000.0;

	//定义外方位元素
	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 << "Bx=" << Bx << endl;
	
	//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;

	cout << "R1=" << R1 << endl;

	double aa1 = cos(phi2)*cos(kappa2) - sin(phi2)*sin(omig2)*sin(kappa2);
	double aa2 = -1.0*cos(phi2)*sin(kappa2) - sin(phi2)*sin(omig2)*cos(kappa2);
	double aa3 = -1.0*sin(phi2)*cos(omig2);
	double bb1 = cos(omig2)*sin(kappa2);
	double bb2 = cos(omig2)*cos(kappa2);
	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);
	cout << "N1="<<N1  <<"N2="<<  N2 << endl;

	//计算模型点坐标
	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;
}



//正确结果
Bx=408.972
R1=[0.9998063323635187, 0.01881023503340721, 0.005785570308718148;
 -0.01880835955329377, 0.9998230355508315, -0.0003784082909691068;
 -0.005791664417347059, 0.0002695179189425382, 0.9999831918504272]
左像空间辅助坐标系：
0.0279043
-0.0481544
-0.0706809
右像空间辅助坐标系：
0.00350666
-0.0478689
-0.0705505
N1=16763.9N2=16772.3
模型点坐标：
467.787
-807.229
-1184.89
地面点坐标是：
XP=432906
YP=3.89403e+06
ZP=89.1226