立体像对前方交会

1.点投影系数法

一、原理

二、步骤

三、代码

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;
	}
	//1、RRRR 计算转转矩阵
	MatrixXd XZmatrix(double Q1, double W1, double K1)
	{
		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;
	}
	//2、通过坐标转换，计算像空间辅助坐标
	MatrixXd Matchange(double x, double y, double f, MatrixXd R)
	{
		MatrixXd P(3, 1);
		P << x,
			y,
			-f;
		MatrixXd XYZ(3, 1);
		XYZ = R * P;
		return XYZ;

	}
	//3、利用2张相片的外方位线元素计算摄影基线向量BX BY BZ
	double pointratio(double a1, double a2)
	{
		return a2 - a1;
	}
	/*P1是第一张相片的外方位元素和像点坐标
	P2是第二张相片的外方位元素和像点坐标
	BX BY BZ 是基线分量
	N N2是点系数
	P11 P22 是计算出来的空间辅助坐标系坐标*/
	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));
		// 利用点系数N1、N2计算地面坐标
		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);
		//1、利用第一张相片的角元素，计算R1
		R1 = XZmatrix(point1[3], point1[4], point1[5]);
		MatrixXd R2(3, 3);
		//2、利用第二张相片的角元素，计算R2
		R2 = XZmatrix(point2[3], point2[4], point2[5]);
		//3、计算基线分量
		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);
		//3、利用坐标转换R1，R2，计算辅助坐标系坐标
		P1 = Matchange(point1[6], point1[7], f, R1);
		P2 = Matchange(point2[6], point2[7], f, R2);
		//4、计算点系数 N1 N2
		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));
		//5、将计算结果 复制到 X
		FUzhi(X, Finalcalc(point1, point2, Bx, By, Bz, N, N2, P1, P2), 3);
	}
	double* result()
	{
		return X;
	}

2.共线方程严密解法

一、原理

二、步骤

a.用各自像片的角元素计算出左右像片的旋转矩阵R1和R2。

b.有同名像点列出共线方程。

c.将方程写为未知数的线性方程形式，计算线性系数。

d.写出误差方程，系数矩阵与常数项。

e.计算未知点的最小二乘解。

f.重复以上步骤完成所有点的地面坐标的计算。

三、代码

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)
    //计算A=[l1,l2,l3;l4,l5,l6]
	{
		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)
    //计算L=[lx;ly]
	{
		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;
	}
};