C++/Egien 交会测量

#include <Eigen/Dense> 
#include <iostream>
#include <opencv2/opencv.hpp>
#include <math.h>
#include <Eigen/core>
#include <vector>
using namespace Eigen;
using namespace std;


//计算旋转矩阵
MatrixXd calRotationMatrix(int Ax, int Ay, int Az)
{
	MatrixXd  R(3, 3);
	R(0, 0) = cos(Ay) * cos(Az) - sin(Ay)*sin(Ax)*sin(Az);
	R(0, 1) = cos(Ax)*sin(Az);
	R(0, 2) = sin(Ay)*cos(Az) + cos(Ay)*sin(Ax)*sin(Az);
	R(1, 0) = -cos(Ay)*sin(Az) - sin(Ay)*sin(Ax)*cos(Az);
	R(1, 1) = cos(Ax)*cos(Az);
	R(1, 2) = -sin(Ay)*sin(Az) + cos(Ay)*sin(Ax)*cos(Az);
	R(2, 0) = -sin(Ay)*cos(Ax);
	R(2, 1) = -sin(Ax);
	R(2, 2) = cos(Ay)*cos(Ax);
	return R;
}

//计算投影矩阵
MatrixXd cal_project_matrix(MatrixXd & InterMatrix, MatrixXd & ExternalMatrix)
{
	MatrixXd ProjectMatrix = InterMatrix * ExternalMatrix;
	//cout << "投影矩阵" << ProjectMatrix << endl;
	return ProjectMatrix;
}

//计算理想像点
MatrixXd cal_reproject_points(MatrixXd & Points3D, MatrixXd & ProjectMatrix)
{
	MatrixXd CalPoint_2D(3, 1);
	//计算重投影坐标
	
	MatrixXd Xw(4, 1);
	Xw << Points3D(0),
		Points3D(1),
		Points3D(2),
		1;
	CalPoint_2D = ProjectMatrix * Xw;
	CalPoint_2D(0) = CalPoint_2D(0) / CalPoint_2D(2);
	CalPoint_2D(1) = CalPoint_2D(1) / CalPoint_2D(2);

	
	return CalPoint_2D;
}

int main()
{
	
   //定义空间坐标点	
	MatrixXd Points3D(3,1);
	Points3D << 0, 0, 500;
	cout << "定义的空间点坐标：" << endl << Points3D << endl;
	//定义像机内参
	double fx = 718.856;
	double fy = 718.856;
	double cx = 607.1928;
	double cy = 185.2157;
	MatrixXd intrinsicMatrix(3, 4);
	intrinsicMatrix << fx, 0, cx, 0,
						0, fy, cy, 0,
						0, 0, 1, 0;
	//cout << intrinsicMatrix << endl;
	//定义平移矩阵	
	MatrixXd Transleft(3, 1);
	Transleft << 2, 3, 15;
	MatrixXd TransRight(3, 1);
	TransRight << 3, 4, 25;
	//计算旋转矩阵
	MatrixXd RLeft = calRotationMatrix(4, 3, 25);
	MatrixXd RRight = calRotationMatrix(4, 13, 25);
	//得到左右像机外参
	MatrixXd LeftExternalMatrix(4, 4);
	LeftExternalMatrix << RLeft, Transleft,
		0, 0, 0, 1;
	//cout << LeftExternalMatrix<<endl;
	MatrixXd RightExternalMatrix(4, 4);
	RightExternalMatrix << RRight, TransRight,
		0, 0, 0, 1;
	
	//计算投影矩阵
	MatrixXd LProjectMatrix(3, 4);
	LProjectMatrix = cal_project_matrix(intrinsicMatrix, LeftExternalMatrix);
	MatrixXd RProjectMatrix(3, 4);
	RProjectMatrix = cal_project_matrix(intrinsicMatrix, RightExternalMatrix);
	
	//计算左右像点
	MatrixXd LPoint_2D(3, 1);
	LPoint_2D = cal_reproject_points(Points3D, LProjectMatrix);
	
	MatrixXd RPoint_2D(3, 1);
	RPoint_2D = cal_reproject_points(Points3D, RProjectMatrix);
	//投影矩阵法求解空间点坐标
	MatrixXd U(4, 1);
	U(0) = LProjectMatrix(0, 3) - LPoint_2D(0) * LProjectMatrix(2, 3);
	U(1) = LProjectMatrix(1, 3) - LPoint_2D(1) * LProjectMatrix(2, 3);
	U(2) = RProjectMatrix(0, 3) - RPoint_2D(0) * RProjectMatrix(2, 3);
	U(3) = RProjectMatrix(1, 3) - RPoint_2D(1) * RProjectMatrix(2, 3);
	
	MatrixXd K(4, 3);
	K(0, 0) = LPoint_2D(0)*LProjectMatrix(2, 0) - LProjectMatrix(0, 0);
	K(0, 1) = LPoint_2D(0)*LProjectMatrix(2, 1) - LProjectMatrix(0, 1);
	K(0, 2) = LPoint_2D(0)*LProjectMatrix(2, 2) - LProjectMatrix(0, 2);
	K(1, 0) = LPoint_2D(1)*LProjectMatrix(2, 0) - LProjectMatrix(1, 0);
	K(1, 1) = LPoint_2D(1)*LProjectMatrix(2, 1) - LProjectMatrix(1, 1);
	K(1, 2) = LPoint_2D(1)*LProjectMatrix(2, 2) - LProjectMatrix(1, 2);

	K(2, 0) = RPoint_2D(0)*RProjectMatrix(2, 0) - RProjectMatrix(0, 0);
	K(2, 1) = RPoint_2D(0)*RProjectMatrix(2, 1) - RProjectMatrix(0, 1);
	K(2, 2) = RPoint_2D(0)*RProjectMatrix(2, 2) - RProjectMatrix(0, 2);
	K(3, 0) = RPoint_2D(1)*RProjectMatrix(2, 0) - RProjectMatrix(1, 0);
	K(3, 1) = RPoint_2D(1)*RProjectMatrix(2, 1) - RProjectMatrix(1, 1);
	K(3, 2) = RPoint_2D(1)*RProjectMatrix(2, 2) - RProjectMatrix(1, 2);
	//最小二乘求解
	MatrixXd K_trans(3, 4);
	K_trans = K.transpose();
	MatrixXd Ktrans_k(3, 3);
	Ktrans_k = K_trans * K;
	MatrixXd Ktrans_KInverse(3, 3);
	Ktrans_KInverse = Ktrans_k.inverse();
	//cout << Ktrans_KInverse << endl;
	MatrixXd S(3, 1);
	S = Ktrans_KInverse * K_trans * U;
	cout << "交会测量得到的三维点坐标：" << endl << S << endl;
	system("pause");
	return 0;
	
}