立体像对相对定向及模型点坐标计算

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


int Relative_orientation()
{

	//读取左片六个像点坐标
	ifstream file1;
	file1.open("E:\\专业课作业\\摄影测量\\相对定向\\左片像点坐标.txt", ios::in);
	double L_imagecontrol[6][2];
	for (int i = 0; i < 6; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			file1 >> L_imagecontrol[i][j];
		}
	}
	file1.close();


	//读取右片六个像点坐标
	ifstream file2;
	file2.open("E:\\专业课作业\\摄影测量\\相对定向\\右片像点坐标.txt", ios::in);
	double R_imagecontrol[6][2];
	for (int i = 0; i < 6; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			file2 >> R_imagecontrol[i][j];
		}
	}
	file2.close();

	double f = 24;
	double Bx[6];
	double By[6];
	double Bz[6];

	double tao = 0.0, miu = 0.0, pitch = 0.0, roll = 0.0, yaw = 0.0;
	double R_X[6] = { 0 }, L_X[6] = { 0 };
	double R_Y[6] = { 0 }, L_Y[6] = { 0 };
	double R_Z[6] = { 0 }, L_Z[6] = { 0 };
	double R_N[6] = { 0 }, L_N[6] = { 0 };
	//定义旋转矩阵
	double rotate[3][3] = { 0 };

	cv::Mat LL(6, 1, CV_64FC1);
	cv::Mat A(6, 5, CV_64FC1);
	cv::Mat delta(5, 1, CV_64FC1);

	int t = 0;//计数器
	while (1)
	{
		//旋转矩阵赋值
		rotate[0][0] = 1;
		rotate[0][1] = (-1.0)*yaw;
		rotate[0][2] = (-1.0)*pitch;
		rotate[1][0] = yaw;
		rotate[1][1] = 1;
		rotate[1][2] = (-1.0)*roll;
		rotate[2][0] = pitch;
		rotate[2][1] = roll;
		rotate[2][2] = 1;
		
		for (int i = 0; i < 6; i++)
		{
			R_X[i] = R_imagecontrol[i][0];
			R_Y[i] = R_imagecontrol[i][1];
			R_Z[i] = -f;

			L_X[i] = rotate[0][0] * L_imagecontrol[i][0] + rotate[0][1] * L_imagecontrol[i][1] + rotate[0][2] * (-f);
			L_Y[i] = rotate[1][0] * L_imagecontrol[i][0] + rotate[1][1] * L_imagecontrol[i][1] + rotate[1][2] * (-f);
			L_Z[i] = rotate[2][0] * L_imagecontrol[i][0] + rotate[2][1] * L_imagecontrol[i][1] + rotate[2][2] * (-f);

			Bx[i] = L_imagecontrol[i][0] - R_imagecontrol[i][0];
			By[i] = Bx[i] * tao;
			Bz[i] = Bx[i] * miu;

			R_N[i] = (Bx[i] * L_Z[i] - Bz[i] * L_X[i]) / (R_X[i] * L_Z[i] - L_X[i] * R_Z[i]);
			L_N[i] = (Bx[i] * R_Z[i] - Bz[i] * R_X[i]) / (R_X[i] * L_Z[i] - L_X[i] * R_Z[i]);

			LL.at<double>(i, 0) = R_N[i] * R_Y[i] - L_N[i] * L_Y[i] - By[i];

			A.at<double>(i, 0) = Bx[i];
			A.at<double>(i, 1) = -L_Y[i] * Bx[i] / L_Z[i];
			A.at<double>(i, 2) = -L_Y[i] * L_X[i] * L_N[i] / L_Z[i];
			A.at<double>(i, 3) = -(L_Z[i] + L_Y[i] * L_Y[i] / L_Z[i])*L_N[i];
			A.at<double>(i, 4) = L_X[i] * L_N[i];
		}

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

		tao += delta.at<double>(0, 0);
		miu += delta.at<double>(1, 0);
		pitch += delta.at<double>(2, 0);
		roll += delta.at<double>(3, 0);
		yaw += delta.at<double>(4, 0);
		t++;

		if ((fabs(delta.at<double>(0, 0)) < 1e-6) && (fabs(delta.at<double>(1, 0)) < 1e-6) && (fabs(delta.at<double>(2, 0)) < 1e-6) 
			&& (fabs(delta.at<double>(3, 0)) < 1e-6) && (fabs(delta.at<double>(4, 0)) < 1e-6))
		{
			break;
		}
	}
	cout << "迭代次数为：" << t << endl;

	double  m0 = 0, m[5] = { 0 }, vv = 0;
	cv::Mat v(5, 1, CV_64FC1);
	cv::Mat ATAINV(5, 5, CV_64FC1);
	ATAINV = (A.t()*A).inv();
	v = A*delta - LL;

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

	m0 = sqrt(vv / 2);

	for (int i = 0; i < 5; i++)
	{
		m[i] = m0*sqrt(fabs(ATAINV.at<double>(i, i)));
	}
	cout << "tao=" << tao << "\t\tm=" << m[0] << endl;
	cout << "miu=" << miu << "\t\tm=" << m[1] << endl;
	cout << "pitch=" << pitch << "\t\tm=" << m[2] << endl;
	cout << "roll=" << roll << "\t\tm=" << m[3] << endl;
	cout << "yaw=" << yaw << "\t\tm=" << m[4] << endl;

	double modelcontrol[6][3];
	for (int i = 0; i < 6;i++)
	{
		modelcontrol[i][0] = R_N[i] * R_X[i];
		modelcontrol[i][1] = (R_N[i] * R_Y[i] + L_N[i] * L_Y[i] + By[i]) / 2;
		modelcontrol[i][2] = R_N[i] * R_Z[i];
		cout << "模型点对应的三维坐标为：（" << modelcontrol[i][0] << "," << modelcontrol[i][1] << "," << modelcontrol[i][2] << "）" << endl;
	}

	//ofstream file;
	//file.open("E:\\专业课作业\\摄影测量\\相对定向\\内方位元素.txt", ios::out);
	//file << tao << endl;
	//file << miu << endl;
	//file << pitch << endl;
	//file << roll << endl;
	//file << yaw << endl;

	system("pause");
	return 0;
}