c++摄影测量点投影系数法和共线方程法空间前方交会

立体像对空间前方交会程序设计

运用了eigen矩阵库

程序原理

00

【实验数据】：

左影像：x0=0; y0=0; f=152.91;
Xs=970302.448784;
Ys=-1138644.971216;
Zs=3154.584941;
phi=0.010425;
omega=-0.012437;
kapa=0.003380;
右影像：x0=0; y0=0; f=152.91;
Xs=971265.303768;
Ys=-1138634.245942;
Zs=3154.784258;
phi=0.008870;
omega=-0.005062;
kapa=-0.008703;
同名像点坐标：左：（0.153，91.798），右：（-78.672，89.122）

c++代码

写入头文件

#pragma once
#include 
#include "Eigen/Dense"
#include "Eigen/Core"
using namespace std;
using namespace Eigen;

class function1
{
public:
	double point1[8]={0};// XS YS ZS Q W K X Y 依次输入
	double point2[8]={0};
	double x0, y0, f=0;//内方位元素
	double X[3] = { 0 };
	int FUzhi(double* a1, double* a2, int n)//数组赋值函数
	{
		for (int i = 0; i < n; i++)
		{
			a1[i] = a2[i];

		}return 0;
	}
	MatrixXd XZmatrix(double Q1, double W1, double K1)//RRRR 计算转转矩阵
	{
		double a1, a2, a3, b1, b2, b3, c1, c2, c3;
		a1 = cos(Q1) * cos(K1) - sin(Q1) * sin(W1) * sin(K1);
		a2 = -cos(Q1) * sin(K1) - sin(Q1) * sin(W1) * cos(K1);
		a3 = -sin(Q1) * cos(W1);
		b1 = cos(W1) * sin(K1);
		b2 = cos(W1) * cos(K1);
		b3 = -sin(W1);
		c1 = sin(Q1) * cos(K1) + cos(Q1) * sin(W1) * sin(K1);
		c2 = -sin(Q1) * sin(K1) + cos(Q1) * sin(W1) * cos(K1);
		c3 = cos(Q1) * cos(W1);
		MatrixXd mat(3, 3);
	
		mat << a1, a2, a3,
			b1, b2, b3,
			c1, c2, c3;
		return  mat;
	}
	MatrixXd Matchange(double x, double y, double f, MatrixXd R)//mar change 坐标转换
	{
		MatrixXd P(3, 1);
		P << x,
			y,
			-f;
		MatrixXd XYZ(3, 1);
		XYZ = R * P;
		return XYZ;

	}
	double pointratio(double a1, double a2)// Bx BY/求BX BY BZ
	{
		return a2 - a1;
	}
	double* Finalcalc(double* P1, double* P2, double Bx, double By, double Bz, double N, double N2, MatrixXd P11,
		MatrixXd P22)//计算最后结果
	{
		double point[3] = { Bx,By,Bz };
		double tmp[3] = { 0 };
		double tmp2[3] = { 0 };

		double* fianl = (double*)malloc(3 * sizeof(double));
		for (int i = 0; i < 3; i++)
		{
			tmp[i] = P1[i] + N * P11(i, 0);
			tmp2[i] = P2[i] + N2 * P22(i, 0);
			*(fianl + i) = (tmp[i] + tmp2[i])/2;

		}
		return fianl;
	}
	function1(double* BS, double* Mp, double x01, double y01, double f01)
	{
		FUzhi(point1, BS, 8);
		FUzhi(point2, Mp, 8);
		x0 = x01;
		y0 = y01;
		f = f01;
	};
	void function1calc()
	{
		MatrixXd R1(3, 3);
		R1 = XZmatrix(point1[3], point1[4], point1[5]);
		MatrixXd R2(3, 3);
		R2 = XZmatrix(point2[3], point2[4], point2[5]);
		double Bx = pointratio(point1[0], point2[0]);
		double By = pointratio(point1[1], point2[1]);
		double Bz = pointratio(point1[2], point2[2]);
		MatrixXd P1(3, 1);
		MatrixXd P2(3, 1);
		P1 = Matchange(point1[6], point1[7], f, R1);
		P2 = Matchange(point2[6], point2[7], f, R2);
		double N = (Bx * P2(2, 0) - Bz * P2(0, 0)) / (P1(0, 0) * P2(2, 0) - P1(2, 0) * P2(0, 0));
		double N2 = (Bx * P1(2, 0) - Bz * P1(0, 0)) / (P1(0, 0) * P2(2, 0) - P1(2, 0) * P2(0, 0));
		FUzhi(X, Finalcalc(point1, point2, Bx, By, Bz, N, N2, P1, P2), 3);
	}
	double* result()
	{
		return X;
	}
	/*~function1()
	{
		free(&X[0]*//*);*/
	//}
};

class funtion2 :public function1
{
public:
	double po1[8]={0};//xs ys zs q w k x y  
	double po2[8]={0};
	double x0, y0, H;
	double X[3] = {0};
	funtion2(double*po4,double*po3,double x,double y,double h):function1(po1,po2,1,1,1)
	{
		FUzhi(po1, po4, 8);
		FUzhi(po2, po3, 8);
		this->x0 = x;
		this->y0 = y;
		this->H = h;
		cout << "赋值完成"<<endl;
		cout << "共线法" << endl;
	};
	MatrixXd CalcA(MatrixXd R1,double x,double y,double f,double xs,double ys,double zs,double x0,double y0)
	{
		MatrixXd A(2, 3);
		
			for (int j = 1; j <=3; j++)
			{
				A(0,j-1) = f * R1(j-1, 0) + (x-x0 )* R1(j - 1, 2);
			}
			
			for (int j = 1; j <= 3; j++)
			{
				A(1, j - 1) = f * R1(j-1, 1) + (y-y0 )* R1(j - 1, 2);
			}
		
		return A;
	}
	MatrixXd CalcL(MatrixXd R1, double x, double y, double f, double xs, double ys, double zs, double x0, double y0)
	{
		double lx = f * R1(0, 0) * xs + f * R1(1, 0) * ys + f * R1(2, 0) * zs + (x - x0) * (R1(0, 2) * xs + R1(1, 2) * ys + zs * R1(2, 2));
		double ly = f * R1(0, 1) * xs + f * R1(1, 1) * ys + f * R1(2, 1) * zs + (y - y0) * (R1(0, 2) * xs + R1(1, 2) * ys + zs * R1(2, 2));
		MatrixXd L(2, 1);
		L(0, 0) = lx;
		L(1, 0) = ly;
		return L;
		
	}
	int calc()
	{
		 MatrixXd R1(3, 3);
		MatrixXd R2(3, 3);
		R1 = XZmatrix(po1[3], po1[4], po1[5]);
		R2 = XZmatrix(po2[3], po2[4], po2[5]);
		MatrixXd A1(2, 3);
		MatrixXd A2(2, 3);
		 A1 = CalcA(R1, po1[6], po1[7], this->H, po1[0], po1[1], po1[2], this->x0, this->y0);
		 A2= CalcA(R2, po2[6], po2[7], this->H, po2[0], po2[1], po2[2], this->x0, this->y0);
		MatrixXd A(4, 3);
		A << A1, A2;
		MatrixXd L1(1, 2);
		MatrixXd L2(1, 2);
		L1= CalcL(R1, po1[6], po1[7], this->H, po1[0], po1[1], po1[2], this->x0, this->y0);
		L2= CalcL(R2, po2[6], po2[7], this->H, po2[0], po2[1], po2[2], this->x0, this->y0);
		MatrixXd L(4, 1);
		L << L1, L2;
		MatrixXd XX(1, 3);
		XX = (A.transpose() * A).inverse() * A.transpose() * L;
		double* final = NULL;
		MatrixXd V(1, 4);
		V = A * XX - L;
		MatrixXd V1(1, 1);
		V1 = V.transpose() * V;
		double q0 = sqrt(V1(0,0) /2);
		
		
		final = (double*)malloc(3 * sizeof(double));
		if(final==NULL)
		{
			cout << " 内存分配失败" << endl;
			return 0;
			
		}
		for(int i=0;i<3;i++)
		{
			*(final + i) = XX(i, 0);
		}
		FUzhi(X, final, 3);
		/*cout << "精度评定" << endl;
		cout << q0 << endl;*/
		return 0;
	}
};

调用格式

#include "Function.h"
#include 
int main()
{
	double a[] = {970302.448784,-1138644.971216,3154.584941,0.010425,-0.012437,0.003380,0.153,91.798};
	double b[] = {971265.303768,-1138634.245942,3154.784258,0.008870,-0.005062,-0.008703,-78.672,89.122};
	function1 obj(b,a,0,0,152.91);
	obj.function1calc();
	printf("点系数法\n");
	for(int i=0;i<3;i++)
	printf("%2f\n",obj.X[i]);
	funtion2 ojb(a, b, 0, 0, 152.91);
	ojb.calc();
	for (int i = 0; i < 3; i++)
		printf("%2f\n", ojb.X[i]);
	system("pause");
	return 0;
	
	
}

运行结果