摄影测量C＋＋单像后方交会代码及其详细解析

#define _CRT_SECURE_NO_WARNINGS


#include<stdio.h>
#include<iostream>
#include<malloc.h>
#include<stdlib.h>
#include<math.h>
#include<conio.h>
#include<ctime>

using namespace std;

#define N 4 
#define M 4 



void RotationMatrix(double omega, double phi, double kappa, double* a, double* b, double* c);

void ErrorEquation(double omega, double kappa, double *A, double *l, double *a, double *b, double *c, double f, double *x, double *y, double *x0, double *y0, double *z0);

double *inv(double *a, int n);

void Multiply(double *A, double *B, double *C, int rows1, int cols1, int rows2, int cols2);

void Transpose(double *A, double AT[], int m, int n);

/*-----------------------------------------------主函数--------------------------------------------*/
int main()    
{

	//获取已知数据
	double  x[N], y[N], x0[N], y0[N], z0[N], X[N], Y[N], Z[N];
	x[0] = -86.15; y[0] = -68.99; X[0] = 36589.41; Y[0] = 25273.32; Z[0] = 2195.17;
	x[1] = -53.40; y[1] = 82.21;  X[1] = 37631.08; Y[1] = 31324.51; Z[1] = 728.69;
	x[2] = -14.78; y[2] = -76.63;  X[2] = 39100.97; Y[2] = 24934.98; Z[2] = 2386.50;
	x[3] = 10.46; y[3] = 64.43;  X[3] = 40426.54; Y[3] = 30319.81; Z[3] = 757.31;
	int i, j = 0;
	printf("控制点坐标：꣺\n");
	for (i = 0; i < N; i++)
		printf("%lf    %lf    %lf   %lf   %lf\n", x[i], y[i], X[i], Y[i], Z[i]);

	
	int m = 50000;
	double f = 153.24;
	
	printf("\n摄影机主距和摄影比例尺分母：f = %f, m = %d\n", f, m);
	f = f / 1000.0;


	double phi, omega, kappa, Xs0, Ys0, Zs0;

    //初始化参数
	double Sum1 = 0, Sum2 = 0, Sum3 = 0;
	for (i = 0; i < N; i++)
	{
		x[i] /= 1000.0;
		y[i] /= 1000.0;
		Sum1 += X[i];
		Sum2 += Y[i];
		Sum3 += Z[i];
	}
	Xs0 = Sum1 / N;
	Ys0 = Sum2 / N;                          
	Zs0 = m * f + Sum3 / N;
	phi = omega = kappa = 0;
	printf("\n外方位元素初始值Xs0 = %f,Ys0 = %f, Zs0 = %f\nphi = %f, omega = %f, kappa = %f", Xs0, Ys0, Zs0, phi, omega, kappa);

	double m0;
	//迭代循环，每一轮迭代都估计外方位元素的值，并更新这些值并逐步得到最优值      
	while (j < M)	//M=4
	{
		//计算旋转矩阵
		double a[3], b[3], c[3];
		RotationMatrix(omega, phi, kappa, a, b, c);

		//利用共线方程根据当前的外方位元素和控制点的空间坐标，计算新的像点坐标
		for (i = 0; i < N; i++)
		{
			x0[i] = -f * (a[0] * (X[i] - Xs0) + b[0] * (Y[i] - Ys0) + c[0] * (Z[i] - Zs0)) / (a[2] * (X[i] - Xs0) + b[2] * (Y[i] - Ys0) + c[2] * (Z[i] - Zs0));  
			y0[i] = -f * (a[1] * (X[i] - Xs0) + b[1] * (Y[i] - Ys0) + c[1] * (Z[i] - Zs0)) / (a[2] * (X[i] - Xs0) + b[2] * (Y[i] - Ys0) + c[2] * (Z[i] - Zs0));
			z0[i] = a[2] * (X[i] - Xs0) + b[2] * (Y[i] - Ys0) + c[2] * (Z[i] - Zs0);
		}

		//构建误差方程
		double A[2 * N * 6], l[2 * N];
		ErrorEquation(omega, kappa, A, l, a, b, c, f, x, y, x0, y0, z0);
		
		//矩阵相乘为后文所用
		double AT[6 * 2 * N], ATA[6 * 6], *InvofATA = NULL, ATl[6], V[6];
		Transpose(A, AT, 2 * N, 6);
		Multiply(AT, A, ATA, 6, 2 * N, 2 * N, 6); 

		//计算ATA的逆矩阵
		InvofATA = inv(ATA, 6);

		//解误差方程，计算AT*l得到ATl
		Multiply(AT, l, ATl, 6, 2 * N, 2 * N, 1);
		//计算未知数的向量解，可以得到外方位元素近似值的改正数
		Multiply(InvofATA, ATl, V, 6, 6, 6, 1);

		//迭代更新外方位元素
		Xs0 += V[0];
		Ys0 += V[1];
		Zs0 += V[2];
		phi += V[3];
		omega += V[4];
		kappa += V[5];
		printf("\n第%d轮迭代：外方位元素为：\n", j);
		printf("Xs=%.5lf,Ys=%.5lf,Zs=%.5lf,t=%.5lf,w=%.5lf,k=%.5lf\n", Xs0, Ys0, Zs0, phi, omega, kappa);

		//使用计算出的外方位元素计算误差向量v，并计算中误差以评估模型质量，
		double s = 0, AX[8], v[8];
		Multiply(A, V, AX, 8, 6, 6, 1); 
		for (i = 0; i < 8; i++)
		{
			v[i] = AX[i] - l[i];
			s += v[i] * v[i];
		}
		m0 = sqrt(s / 2);
		j++;
	}

	printf("\n\n\nXs=%.5lf,Ys=%.5lf,Zs=%.5lf,t=%.5lf,w=%.5lf,k=%.5lf\n", Xs0, Ys0, Zs0, phi, omega, kappa);
	printf("\n中误差为：%f\n", m0);
	system("pause");
	return 0;
}


//计算旋转矩阵
//a\b\c为旋转矩阵的三个基向量, phi\omega\kappa为外方位元素
void RotationMatrix(double omega, double phi, double kappa, double* a, double* b, double* c)
{
	a[0] = cos(phi)*cos(kappa) - sin(phi)*sin(omega)*sin(kappa);
	a[1] = -cos(phi)*sin(kappa) - sin(phi)*sin(omega)*cos(kappa);
	a[2] = -sin(phi)*cos(omega);
	b[0] = cos(omega)*sin(kappa);
	b[1] = cos(omega)*cos(kappa);           
	b[2] = -sin(omega);
	c[0] = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa);
	c[1] = -sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa);
	c[2] = cos(phi)*cos(omega);
}

//计算误差差方程式
//omega、kappa为俩外方位元素值，f为摄像机主距、N为控制点的数量、a\b\c为旋转矩阵的三个基向量、x\y是控制点的坐标、x0\y0\z0为近似值
//A是误差方程的系数矩阵、l是误差方程的常数项向量
void ErrorEquation(double omega, double kappa, double *A, double *l, double *a, double *b, double *c, double f, double *x, double *y, double *x0, double *y0, double *z0)
{
	for (int i = 0; i < N; i++)
	{
		//计算近似值改正数的系数
		A[i * 12 + 0] = (a[0] * f + a[2] * x[i]) / z0[i];
		A[i * 12 + 1] = (b[0] * f + b[2] * x[i]) / z0[i];
		A[i * 12 + 2] = (c[0] * f + c[2] * x[i]) / z0[i];
		A[i * 12 + 3] = y[i] * sin(omega) - (x[i] * (x[i] * cos(kappa) - y[i] * sin(kappa)) / f + f * cos(kappa))*cos(omega);
		A[i * 12 + 4] = -f * sin(kappa) - x[i] * (x[i] * sin(kappa) + y[i] * cos(kappa)) / f;
		A[i * 12 + 5] = y[i];
		A[i * 12 + 6] = (a[1] * f + a[2] * y[i]) / z0[i];
		A[i * 12 + 7] = (b[1] * f + b[2] * y[i]) / z0[i];
		A[i * 12 + 8] = (c[1] * f + c[2] * y[i]) / z0[i];
		A[i * 12 + 9] = -x[i] * sin(omega) - (y[i] * (x[i] * cos(kappa) - y[i] * sin(kappa)) / f - f * sin(kappa))*cos(omega);
		A[i * 12 + 10] = -f * cos(kappa) - y[i] * (x[i] * sin(kappa) + y[i] * cos(kappa)) / f;
		A[i * 12 + 11] = -x[i];
		//计算常数项向量
		l[i * 2 + 0] = x[i] - x0[i];
		l[i * 2 + 1] = y[i] - y0[i];
	}
}

//计算矩阵的转置
//A为要转置矩阵的指针、AT是存放转置结果的矩阵的指针、m是原始矩阵的行数、n是原始矩阵的列数
void Transpose(double *A, double AT[], int m, int n)    
{
	int i, j;
	for (i = 0; i < m; i++)
		for (j = 0; j < n; j++)
		    //将原始矩阵中第i行第j列的元素赋给转置矩阵中的第j行第i列
			AT[j*m + i] = A[i*n + j];
}

//计算两矩阵相乘
//A为第一个矩阵的指针，B为第二个矩阵的指针，C是存放结果的矩阵的指针，rows1、cols1、rows2、cols2分别是第一个、第二个矩阵的行列数
void Multiply(double *A, double *B, double *C, int rows1, int cols1, int rows2, int cols2)  
{
	int i, j, l, u;
	//检查两个矩阵的纬度是否满足相乘要求
	if (cols1 != rows2)
	{
		printf("error!cannot do the multiplication.\n");
		return;
	}
	//矩阵相乘
	for (i = 0; i < rows1; i++)
		for (j = 0; j < cols2; j++)
		{
			u = i * cols2 + j;
			//初始化结果矩阵
			C[u] = 0.0;
			//l用于遍历两个矩阵中参与乘法的对应元素，
			for (l = 0; l < cols1; l++)
			    //通过 A[i*cols1 + l] 和 B[l*cols2 + j] 分别取得要相乘的两个元素，进行乘法运算并累加到矩阵 C 的当前位置。
				C[u] += A[i*cols1 + l] * B[l*cols2 + j];
		}
	return;
}

//矩阵逆运算函数
double *inv(double *a, int n)       
{                                 
	//分配了两个整数型数组is和js，分别用于记录主元素所在的行列索引
	int *is, *js, i, j, k, l, u, v;
	double d, p;
	is = (int*)malloc(n * sizeof(int));
	js = (int*)malloc(n * sizeof(int));
	//主元素提取：在每次迭代中，通过循环遍历矩阵的行和列，找到当前迭代中绝对值最大的元素，该元素为主元素
	//并记录主元素所在的行列索引，以备后续使用
	for (k = 0; k <= n - 1; k++)
	{
		d = 0.0;
		//获取主元素所在位置的索引，然后将主元素归一化
		for (i = k; i < n; i++)
			for (j = k; j < n; j++)
			{
				l = i * n + j;
				p = fabs(a[l]);
				if (p > d)
				{
					d = p;
					is[k] = i;
					js[k] = j;
				}
			}
		if (d + 1.0 == 1.0)
		{
			free(is);
			free(js);
			printf("error not inv\n");
			return NULL;
		}
		if (is[k] != k)
			for (j = 0; j < n; j++)
			{
				u = k * n + j;
				v = is[k] * n + j;
				p = a[u];
				a[u] = a[v];
				a[v] = p;
			}
		if (js[k] != k)
			for (i = 0; i < n; i++)
			{
				u = i * n + k;
				v = i * n + js[k];
				p = a[u];
				a[u] = a[v];
				a[v] = p;
			}
		//归一化计算
		l = k * n + k;
		a[l] = 1.0 / a[l];
		//对当前行进行消元
		for (j = 0; j < n; j++)
			if (j != k)
			{
				u = k * n + j;
				a[u] = a[u] * a[l];
			}
		//以下两个嵌套循环对其他行进行消元
		for (i = 0; i < n; i++)
			if (i != k)
				for (j = 0; j < n; j++)
					if (j != k)
					{
						u = i * n + j;
						a[u] = a[u] - a[i*n + k] * a[k*n + j];
					}
		for (i = 0; i < n; i++)
			if (i != k)
			{
				u = i * n + k;
				a[u] = -a[u] * a[l];
			}
	}
	//将其他行中主元素所在列的元素消元为0，更新其他行中的元素
	//并进行回代求逆得到单位矩阵、逆序恢复：对消元过程中所做的行列交换进行逆序操作
	for (k = n - 1; k >= 0; k--)
	{
		if (js[k] != k)
			for (j = 0; j <= n - 1; j++)
			{
				u = k * n + j;
				v = js[k] * n + j;
				p = a[u];
				a[u] = a[v];
				a[v] = p;
			}
		if (is[k] != k)
			for (i = 0; i < n; i++)
			{
				u = i * n + k;
				v = i * n + is[k];
				p = a[u];
				a[u] = a[v];
				a[v] = p;
			}
	}
	//释放内存
	free(is);
	free(js);
	return a;
}

具体的项目文件可以私聊我。