单像空间后方交会

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

int space_resection()
{
	ifstream file;

	//读取地物点坐标
	file.open("E:\\专业课作业\\摄影测量\\单向空间后方交会\\地物点坐标.txt", ios::in);
	double groundcontrol[4][3];
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 3;j++)
		{
			file >> groundcontrol[i][j];
		}
	}
	file.close();

	//读取像点坐标
	file.open("E:\\专业课作业\\摄影测量\\单向空间后方交会\\像点坐标.txt", ios::in);
	double imagecontrol[4][2];
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			file >> imagecontrol[i][j] ;
			imagecontrol[i][j] /= 1000.0;
		}
	}
	file.close();

	//定义摄影比例尺和内方位元素
	double scale = 40000.0;
	double f = 0.15324;
	double x0 = 0;
	double y0 = 0;

	//定义外方位元素
	double Xs = 0.0, Ys = 0.0, Zs = 0.0, pitch = 0.0, roll = 0.0, yaw = 0.0;

	Xs = (groundcontrol[0][0] + groundcontrol[1][0] + groundcontrol[2][0] + groundcontrol[3][0]) / 4.0;
	Ys = (groundcontrol[0][1] + groundcontrol[1][1] + groundcontrol[2][1] + groundcontrol[3][1]) / 4.0;
	Zs = scale * f;
	
	//cout << "外方位线元素分别为" << endl;
	//cout << Xs << '\t' << Ys << '\t' << Zs << endl;

	double x[4] = { 0 }, y[4] = { 0 };
	double  m0 = 0, m[6] = { 0 },  vv = 0;
	cv::Mat A(8, 6, CV_64FC1);
	cv::Mat LL(8, 1, CV_64FC1);
	cv::Mat delta(6, 1, CV_64FC1);//改正数矩阵

	//定义旋转矩阵
	double rotate[3][3];

	int t = 0;//计数器
	while (1)
	{
		//旋转矩阵赋值
		rotate[0][0] = cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw);
		rotate[0][1] = (-1.0)*cos(pitch)*sin(yaw) - sin(pitch)*sin(roll)*cos(yaw);
		rotate[0][2] = (-1.0)*sin(pitch)*cos(roll);
		rotate[1][0] = cos(roll)*sin(yaw);
		rotate[1][1] = cos(roll)*cos(yaw);
		rotate[1][2] = (-1.0)*sin(roll);
		rotate[2][0] = sin(pitch)*cos(yaw) + cos(pitch)*sin(roll)*sin(yaw);
		rotate[2][1] = (-1.0)*sin(pitch)*sin(yaw) + cos(pitch)*sin(roll)*cos(yaw);
		rotate[2][2] = cos(pitch)*cos(roll);

		for (int i = 0; i < 4; i++)
		{
			x[i] = (-1.0)*f*(rotate[0][0] * (groundcontrol[i][0] - Xs) + rotate[1][0] * (groundcontrol[i][1] - Ys)
				+ rotate[2][0] * (groundcontrol[i][2] - Zs)) / (rotate[0][2] * (groundcontrol[i][0] - Xs)
				+ rotate[1][2] * (groundcontrol[i][1] - Ys) + rotate[2][2] * (groundcontrol[i][2] - Zs));
			y[i] = (-1.0)*f*(rotate[0][1] * (groundcontrol[i][0] - Xs) + rotate[1][1] * (groundcontrol[i][1] - Ys)
				+ rotate[2][1] * (groundcontrol[i][2] - Zs)) / (rotate[0][2] * (groundcontrol[i][0] - Xs)
				+ rotate[1][2] * (groundcontrol[i][1] - Ys) + rotate[2][2] * (groundcontrol[i][2] - Zs));

			LL.at<double>(i * 2,0) = imagecontrol[i][0] - x[i];
			LL.at<double>(i * 2 + 1,0) = imagecontrol[i][1] - y[i];

			A.at<double>(i * 2,0) = (-1.0)*f / (Zs - groundcontrol[i][2]);
			A.at<double>(i * 2,1) = 0.0;
			A.at<double>(i * 2,2) = (-1.0)*x[i] / (Zs - groundcontrol[i][2]);
			A.at<double>(i * 2,3) = (-1.0)*f*(1 + (x[i] * x[i]) / (f*f));
			A.at<double>(i * 2,4 )= (-1.0)*x[i] * y[i] / f;
			A.at<double>(i * 2,5) = y[i];
			A.at<double>(i * 2 + 1,0) = 0.0;
			A.at<double>(i * 2 + 1,1) = A.at<double>(i * 2,0);
			A.at<double>(i * 2 + 1,2) = (-1.0)*y[i] / (Zs - groundcontrol[i][2]);
			A.at<double>(i * 2 + 1,3) = A.at<double>(i * 2,4);
			A.at<double>(i * 2 + 1,4) = (-1.0)*f*(1 + (y[i] * y[i]) / (f*f));
			A.at<double>(i * 2 + 1,5) = (-1.0)*x[i];
		}

		//for (int i = 0; i < 8; i++)
		//{
		//	cout << A.at<double>(i, 0) << '\t' << A.at<double>(i, 1) << '\t' << A.at<double>(i, 2) << '\t' << A.at<double>(i, 3) << '\t' << A.at<double>(i, 4) << '\t' << A.at<double>(i, 5) << endl;
		//}

	
		delta = (A.t()*A).inv()*A.t()*LL;

		//cout << delta << endl;
		//cout << LL << endl;
		Xs += delta.at<double>(0,0);
		Ys += delta.at<double>(1, 0);
		Zs += delta.at<double>(2, 0);
		pitch += delta.at<double>(3, 0);
		roll += delta.at<double>(4, 0);
		yaw += delta.at<double>(5, 0);
		t++;


		if ((fabs(delta.at<double>(3,0)) < 1e-6) && (fabs(delta.at<double>(4,0)) < 1e-6) && (fabs(delta.at<double>(5,0)) < 1e-6))
		{
			break;
		}
	}
	cout << "迭代次数为：" << t<<endl;
	rotate[0][0] = cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw);
	rotate[0][1] = (-1.0)*cos(pitch)*sin(yaw) - sin(pitch)*sin(roll)*cos(yaw);
	rotate[0][2] = (-1.0)*sin(pitch)*cos(roll);
	rotate[1][0] = cos(roll)*sin(yaw);
	rotate[1][1] = cos(roll)*cos(yaw);
	rotate[1][2] = (-1.0)*sin(roll);
	rotate[2][0] = sin(pitch)*cos(yaw) + cos(pitch)*sin(roll)*sin(yaw);
	rotate[2][1] = (-1.0)*sin(pitch)*sin(yaw) + cos(pitch)*sin(roll)*cos(yaw);
	rotate[2][2] = cos(pitch)*cos(roll);

	cv::Mat v(8, 1, CV_64FC1);
	cv::Mat ATAINV(6, 6, CV_64FC1);
	ATAINV = (A.t()*A).inv();
	v = A*delta - LL;

	for (int i = 0; i < 8; i++)
	{
		vv += v.at<double>(i,0) * v.at<double>(i,0);
	}

	m0 = sqrt(vv / 2);
	
	for (int i = 0; i < 6; i++)
	{
		m[i] = m0*sqrt(fabs(ATAINV.at<double>(i,i)));
	}

	cout << "像片的外方位元素为：" << endl;
	cout << "Xs=" << Xs << "\t\tm=" << m[0] << endl;
	cout << "Ys=" << Ys << "\t\tm=" << m[1] << endl;
	cout << "Zs=" << Zs << "\t\tm=" << m[2] << endl;
	cout << "pitch=" << pitch << "\tm=" << m[3] << endl;
	cout << "roll=" << roll  << "\t\tm=" << m[4] << endl;
	cout << "yaw=" << yaw << "\t\tm=" << m[5] << endl;
	system("pause");
	return 0;

}