立体像对空间前方交会（利用外方位元素交会出地面点三维坐标）

1. 要通过外方位元素进行前方交会首先需要的已知量有：
   

2. 然后计算左右片在地辅坐标系中旋转矩阵的方向余弦
   

3. 再计算基线分量
   

4. 计算像点的像空间辅助坐标
   

5. 计算投影系数
   

6. 计算地面点的左像辅系坐标
   

7. 计算地面点的地面坐标
   
   具体代码如下

#include "iostream"
#include "opencv2\opencv.hpp"
using namespace std;


int main()
{
	double phi1 = -0.005785603;
	double omiga1 = 0.0003784083;
	double kapa1 = -0.01880947;
	double phi2 = -0.006604103;
	double omiga2 = 0.0005394704;
	double kapa2 = 0.0007619388;

	double XS1 = 432438.565, YS1 = 3894841.049, ZS1 = 1274.012;
	double XS2 = 432847.537, YS2 = 3894836.718, ZS2 = 1272.414;

	double x1 = 0.029214000, y1 = -0.047640000;
	double x2 = 0.003936000, y2 = -0.047910000;

	double f = 0.0705;

	double rotate[2][3][3];
	rotate[0][0][0] = cos(phi1)*cos(kapa1) - sin(phi1)*sin(omiga1)*sin(kapa1);
	rotate[0][0][1] = (-1.0)*cos(phi1)*sin(kapa1) - sin(phi1)*sin(omiga1)*cos(kapa1);
	rotate[0][0][2] = (-1.0)*sin(phi1)*cos(omiga1);
	rotate[0][1][0] = cos(omiga1)*sin(kapa1);
	rotate[0][1][1] = cos(omiga1)*cos(kapa1);
	rotate[0][1][2] = (-1.0)*sin(omiga1);
	rotate[0][2][0] = sin(phi1)*cos(kapa1) + cos(phi1)*sin(omiga1)*sin(kapa1);
	rotate[0][2][1] = (-1.0)*sin(phi1)*sin(kapa1) + cos(phi1)*sin(omiga1)*cos(kapa1);
	rotate[0][2][2] = cos(phi1)*cos(omiga1);

	rotate[1][0][0] = cos(phi2)*cos(kapa2) - sin(phi2)*sin(omiga2)*sin(kapa2);
	rotate[1][0][1] = (-1.0)*cos(phi2)*sin(kapa2) - sin(phi2)*sin(omiga2)*cos(kapa2);
	rotate[1][0][2] = (-1.0)*sin(phi2)*cos(omiga2);
	rotate[1][1][0] = cos(omiga2)*sin(kapa2);
	rotate[1][1][1] = cos(omiga2)*cos(kapa2);
	rotate[1][1][2] = (-1.0)*sin(omiga2);
	rotate[1][2][0] = sin(phi2)*cos(kapa2) + cos(phi2)*sin(omiga2)*sin(kapa2);
	rotate[1][2][1] = (-1.0)*sin(phi2)*sin(kapa2) + cos(phi2)*sin(omiga2)*cos(kapa2);
	rotate[1][2][2] = cos(phi2)*cos(omiga2);

	cv::Mat rotate1(3, 3, CV_64FC1, rotate[0]);
	cv::Mat rotate2(3, 3, CV_64FC1, rotate[1]);

	double Bx = XS2 - XS1, By = YS2 - YS1, Bz = ZS2 - ZS1;

	cv::Mat xfzb1(3, 1, CV_64FC1);
	cv::Mat xfzb2(3, 1, CV_64FC1);
	cv::Mat nfw1 = (cv::Mat_<double>(3, 1) << x1, y1, -f);
	cv::Mat nfw2 = (cv::Mat_<double>(3, 1) << x2, y2, -f);

	xfzb1 = rotate1*nfw1;
	xfzb2 = rotate2*nfw2;

	double N1, N2;
	N1 = (Bx*nfw2.at<double>(2, 0) - Bz*nfw2.at<double>(0, 0)) / (nfw1.at<double>(0, 0)*nfw2.at<double>(2, 0) - nfw2.at<double>(0, 0)*nfw1.at<double>(2, 0));
	N2 = (Bx*nfw1.at<double>(2, 0) - Bz*nfw1.at<double>(0, 0)) / (nfw1.at<double>(0, 0)*nfw2.at<double>(2, 0) - nfw2.at<double>(0, 0)*nfw1.at<double>(2, 0));

	double derta_X, derta_Y, derta_Z;
	derta_X = N1*nfw1.at<double>(0, 0);
	derta_Y = (N1*nfw1.at<double>(1, 0) + N2*nfw2.at<double>(1, 0) + By);
	derta_Z = N1*nfw1.at<double>(2, 0);

	double zszb_X, zszb_Y, zszb_Z;
	zszb_X = XS1 + derta_X;
	zszb_Y = YS1 + derta_Y;
	zszb_Z = ZS1 + derta_Z;

	cout << "地面点的真实坐标为（" << zszb_X << "," << zszb_Y << "," << zszb_Z << ")" << endl;


	system("pause");
	return 0;
}