相对定向--双像三维建模小软件开发实例（五）

一、基本概念

        在摄影测量学中，恢复立体像对两张像片之间的相对方位的过程叫做立体像对的相对定向。相对定向至少有两个重要作用：其一，完成立体像对的相对定向后 ,其相应光线在该面内成对相交，形成一个与实地相似的几何模型，对该几何模型进行绝对定向后即可实现对地面实际地形、地物的量测；其二，相对定向确定相对方位元素后可以生成核线影像，加快影像匹配的速度、提高匹配效率、增加匹配可靠性。

二、原理介绍

相对定向的数学模型是共面条件方程：

其中，分别为左右影像上同名像点各自的空间辅助座标。相对定向有两种方法：迭代求解法，直接求解法，和带约束的直接求解法，其中迭代求解法按照定向元素选择的不同分为连续像对的相对定向和独立像对的相对定向两种。

2.1 传统迭代求解法

连续像对的相对定向是以左影像像空间座标系为基准，确定五个相对定向元素，By,Bz,phi,omiga,kappa，然后把共面条件方程进行泰勒展开并只保留一次项，则可以得到相对定向误差方程，且该误差方程的观测值实际上就是像点量测时的“上下视差”，如果有5个以上同名像点便可以按照最小二乘法迭代求解定向元素。独立法相对定向原理和连续发相对定向相同，只不过选择的座标系基准不同，它是选用摄影基线为空间辅助座标系的X轴，其正方向与航线方向一致。（详细算法参见张祖勋等《摄影测量学》）

2.2 直接解法

迭代求解法一般用于竖直航空摄影或已知倾角近似值摄影，但当不知道倾斜摄影中的倾角近似值以及不知道影像内方位元素时，就必须采用相对定向直接解了。直接解法也是以共面条件方程为基础的，它是直接将共面条件方程展开，得到线性方程，然后利用同名像点座标直接求解线性方程的9个参数，最后由9个参数解析出相对定向元素。（详细算法参见冯文灏《近景摄影测量》）

2.3 带约束的直接解法

实际上，相对定向的独立参数只有5个，而传统相对定向却引入了9个参数（如果确定的话），因此这9个参数之间必然存在4个独立的约束条件。如果不考虑这4个约束条件的话，就会形成超参数（over parameterization）的现象，即引入了多余的参数。于是出现了带约束的直接解法，其实这4个约束根本上是通过旋转矩阵的正交性产生。（详细算法参见Yongjun Zhang. Direct Relative Orientation with Four Independent Constraints）

2.4 误差评定方法

采用间接平差进行相对定向方程解算的误差评定方法详细可参见 武汉大学测量平常组《误差理论与测量平差》。

三、实现方法

/****************************相对定向***************************************/
// 算法名：RelativeOrientation
// 说明：传统的相对定向
// 参数：
/****************************相对定向***************************************/
PHOTOGRAMMETRYALGORITHMN_API void GetRelOriEquCoe(double X1, double Y1, double Z1, double X2, double Y2, double Z2, double f1,double f2,
					 double Bx,double By,double Bz,double *A,double *l)
{
	double N1,N2;
	N1 = (Bx*Z2 - Bz*X2)/(X1*Z2 - Z1*X2);
	N2 = (Bx*Z1 - Bz*X1)/(X1*Z2 - Z1*X2);

	A[0] = -X2*Y2*N2/Z2;
	A[1] = -(Z2 + Y2*Y2/Z2)*N2;
	A[2] = X2*N2;
	A[3] = Bx;
	A[4] = -Y2*Bx/Z2;
	*l = N1*Y1 - N2*Y2 - By;
}
PHOTOGRAMMETRYALGORITHMN_API bool RelativeOrientation(double *x1, double *y1, double f1,double *x2, double *y2,double f2,int num_total,
						 double *u, double *v, double *phi, double *omiga, double *kappa,double *qErr,
						 double rmsRequest,bool *deleteItem)
{
	//设定Bx = 1；
	int i;
	double A[5],l[1],At[5],AtA[25],Atl[5],Delt[5],Res[5]={0,0,0,0,0};
	double M[25]={0},L[5]={0},M2[25]={0};
	double ZB1[3],zb2[3],ZB2[3];
	double R[9];
	double Bx = 1,By,Bz;
	int num = num_total;//实际参与平差的点数
	for(i=0;i<num_total;i++)deleteItem[i] = 0;
	if ( (*u != 0) || (*v != 0) || (*phi != 0) || (*omiga != 0) || (*kappa != 0))
	{
		Res[3] = *u;
		Res[4] = *v;
		Res[0] = *phi;
		Res[1] = *omiga;
		Res[2] = *kappa;
	}
Retry:
	do
	{
		By = Bx * Res[3];
		Bz = Bx * Res[4];
		
		for(i=0;i<25;i++) M[i] = 0;
		for(i=0;i<5;i++) L[i] = 0;

		RotationMatrix(Res[0],Res[1],Res[2],R);

		for(i =0;i<num_total;i++)
		{
			if(deleteItem[i] == 1)
				continue;
			ZB1[0] = x1[i];
			ZB1[1] = y1[i];
			ZB1[2] = -f1;

			zb2[0] = x2[i];
			zb2[1] = y2[i];
			zb2[2] = -f2;

			MultMatrix(R,zb2,ZB2,3,3,1);
			GetRelOriEquCoe(ZB1[0],ZB1[1],ZB1[2],ZB2[0],ZB2[1],ZB2[2],f1,f2,Bx,By,Bz,A,l);

			TranMatrix(A,At,1,5);
			MultMatrix(At,A,AtA,5,1,5);
			MultMatrix(At,l,Atl,5,1,1);
			
			AddMatrix(M,AtA,M,5,5);
			AddMatrix(L,Atl,L,5,1);
		}
		InverMatrix(M,5);
		MultMatrix(M,L,Delt,5,5,1);
        AddMatrix(Res,Delt,Res,5,1);
	}
	while(fabs(Delt[0]) > 1.0e-6 || fabs(Delt[1]) > 1.0e-6 || fabs(Delt[2]) > 1.0e-6);

	*u = Res[3];
	*v = Res[4];
	*phi = Res[0];
	*omiga = Res[1];
	*kappa = Res[2];
	//精度评定
	for(i=0;i<5;i++)
		qErr[i] = 0;
	if(num < 6)
		return 0;
    double lt[1],ltl[1],ltA[5],ltARes[1];
	double V_V,sigma=0;
	for(i =0;i<num_total;i++)
	{
		if(deleteItem[i] == 1)
			continue;
		ZB1[0] = x1[i];
		ZB1[1] = y1[i];
		ZB1[2] = -f1;	
		zb2[0] = x2[i];
		zb2[1] = y2[i];
		zb2[2] = -f2;
		MultMatrix(R,zb2,ZB2,3,3,1);

		GetRelOriEquCoe(ZB1[0],ZB1[1],ZB1[2],ZB2[0],ZB2[1],ZB2[2],f1,f2,Bx,By,Bz,A,l);
		TranMatrix(l,lt,1,1);
     	MultMatrix(lt,l,ltl,1,1,1);
	    MultMatrix(lt,A,ltA,1,1,5);
	    MultMatrix(ltA,Res,ltARes,1,5,1);
        V_V =ltl[0] - ltARes[0];
    	sigma += V_V;

		if(fabs(V_V) > rmsRequest)
		{
			deleteItem[i] = 1;
            num--;
			goto Retry;
		}
	}
	sigma = sqrt(fabs(sigma/(num - 5)));
	qErr[0] = sigma * sqrt(M[18]);
	qErr[1] = sigma * sqrt(M[24]);
	qErr[2] = sigma * sqrt(M[0]);
	qErr[3] = sigma * sqrt(M[6]);
	qErr[4] = sigma * sqrt(M[12]);

	return 1;
}
/****************************相对定向***************************************/
// 算法名：RelativeOrientationRLT
// 说明：直接线性解相对定向，带约束和不带约束的
// 参数：
/****************************相对定向***************************************/
PHOTOGRAMMETRYALGORITHMN_API bool RelativeOrientationRLT(double *x1, double *y1, double f1,double *x2, double *y2,double f2,int num,
							double *u, double *v, double *phi, double *omiga, double *kappa,double *qErr,bool constraint)
{
	//step1:传统直接解法
	double Bx = 1,By,Bz;
	int i;
	double K5,K[9];
	double A[8],At[8],l[1],AtA[64],Atl[8],Res[8];
	double _AtA[64]={0},_Atl[8]={0};

	for(i=0;i<num;i++)
	{
		A[0] = y1[i]*x2[i];
		A[1] = y1[i]*y2[i];
		A[2] = -y1[i]*f2;
		A[3] = f1*x2[i];
		A[4] = -f1*f2;
		A[5] = x1[i]*x2[i];
		A[6] = x1[i]*y2[i];
		A[7] = -x1[i]*f2;
		
		l[0] = -y2[i]*f1; //观测值为(y1[i]-y2[2])*f1

	    TranMatrix(A,At,1,8);
	    MultMatrix(At,A,AtA,8,1,8);
		AddMatrix(_AtA,AtA,_AtA,8,8);
		MultMatrix(At,l,Atl,8,1,1);
		AddMatrix(_Atl,Atl,_Atl,8,1);
	}

	InverMatrix(_AtA,8);
	MultMatrix(_AtA,_Atl,Res,8,8,1);
	
	//求各个系数
	double L[9];
	L[0] = Res[0];
	L[1] = Res[1];
	L[2] = Res[2];
	L[3] = Res[3];
	L[4] = 1;
	L[5] = Res[4];
	L[6] = Res[5];
	L[7] = Res[6];
	L[8] = Res[7];
	
	K5 = sqrt(2*Bx*Bx/(L[0]*L[0] + L[1]*L[1] + L[2]*L[2] + L[3]*L[3] + L[4]*L[4] + L[5]*L[5] - L[6]*L[6] - L[7]*L[7] - L[8]*L[8]));
	
	for(i=0;i<9;i++)
	{
		K[i] = L[i]*K5;
	}    

	if (!constraint)
	{
		goto step3;
	}

	//step2:带约束的直接解法
	double B[45],Bt[45],l2[5],BtB[81],Btl[9];
	double Delt[9];
	double max;//迭代终止判断阈值
	do 
	{
		double _BtB[81]={0},_Btl[9]={0};
		for (i=0;i<num;i++)
		{
			B[0] = y1[i]*x2[i]/f1/K[4];
			B[1] = y1[i]*y2[i]/f1/K[4];
			B[2] = -y1[i]*f2/f1/K[4];
			B[3] = f1*x2[i]/f1/K[4];
			B[4] = -y1[i]*x2[i]*K[0]/f1/(K[4]*K[4]) - y1[i]*y2[i]*K[1]/f1/(K[4]*K[4]) + y1[i]*f2*K[2]/f1/(K[4]*K[4])
				- f1*x2[i]*K[3]/f1/(K[4]*K[4]) + f1*f2*K[5]/f1/(K[4]*K[4]) - x1[i]*x2[i]*K[6]/f1/(K[4]*K[4]) - 
				x1[i]*y2[i]*K[7]/f1/(K[4]*K[4]) + x1[i]*f2*K[8]/f1/(K[4]*K[4]);
			B[5] = -f1*f2/f1/K[4];
			B[6] = x1[i]*x2[i]/f1/K[4];
			B[7] = x1[i]*y2[i]/f1/K[4];
			B[8] = -x1[i]*f2/f1/K[4];
			
			l2[0] = y2[i] + y1[i]*x2[i]*K[0]/f1/K[4] + y1[i]*y2[i]*K[1]/f1/K[4] - y1[i]*f2*K[2]/f1/K[4]
				+ f1*x2[i]*K[3]/f1/K[4] - f1*f2*K[5]/f1/K[4] + x1[i]*x2[i]*K[6]/f1/K[4] + 
				x1[i]*y2[i]*K[7]/f1/K[4] - x1[i]*f2*K[8]/f1/K[4];
			l2[0] = -l2[0];
			
			B[9*1+0] = 2*K[0];
			B[9*1+1] = 2*K[1];
			B[9*1+2] = 2*K[2];
			B[9*1+3] = -2*K[6]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*1+4] = -2*K[7]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*1+5] = -2*K[8]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*1+6] = -2*K[3]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*1+7] = -2*K[4]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*1+8] = -2*K[5]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			
			l2[1] = pow(Bx,2)+pow((K[3]*K[6]+K[4]*K[7]+K[5]*K[8]),2)/pow(Bx,2)-pow(K[0],2)-pow(K[1],2)-pow(K[2],2);
			
			B[9*2+0] = -2*K[6]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*2+1] = -2*K[7]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*2+2] = -2*K[8]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*2+3] = 2*K[3];
			B[9*2+4] = 2*K[4];
			B[9*2+5] = 2*K[5];
			B[9*2+6] = -2*K[0]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*2+7] = -2*K[1]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*2+8] = -2*K[2]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			
			l2[2] = pow(Bx,2)+pow((K[0]*K[6]+K[1]*K[7]+K[2]*K[8]),2)/pow(Bx,2)-pow(K[3],2)-pow(K[4],2)-pow(K[5],2);
			
			B[9*3+0] = -2*K[6]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*3+1] = -2*K[7]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*3+2] = -2*K[8]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*3+3] = -2*K[6]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*3+4] = -2*K[7]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*3+5] = -2*K[8]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*3+6] = 2*K[6]-2*K[0]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2)-2*K[3]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*3+7] = 2*K[7]-2*K[1]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2)-2*K[4]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*3+8] = 2*K[8]-2*K[2]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2)-2*K[5]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			
			l2[3] = pow((K[0]*K[6]+K[1]*K[7]+K[2]*K[8]),2)/pow(Bx,2)+pow((K[3]*K[6]+K[4]*K[7]+K[5]*K[8]),2)/pow(Bx,2)
				-pow(K[6],2)-pow(K[7],2)-pow(K[8],2);	
			
			B[9*4+0] = K[3]+K[6]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*4+1] = K[4]+K[7]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*4+2] = K[5]+K[8]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			B[9*4+3] = K[0]+K[6]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*4+4] = K[1]+K[7]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*4+5] = K[2]+K[8]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])/pow(Bx,2);
			B[9*4+6] = (K[3]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])+K[0]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8]))/pow(Bx,2);
			B[9*4+7] = (K[4]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])+K[1]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8]))/pow(Bx,2);
			B[9*4+8] = (K[5]*(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])+K[2]*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8]))/pow(Bx,2);
			
			l2[4] = -K[0]*K[3]-K[1]*K[4]-K[2]*K[5]-(K[0]*K[6]+K[1]*K[7]+K[2]*K[8])*(K[3]*K[6]+K[4]*K[7]+K[5]*K[8])/pow(Bx,2);
			
			TranMatrix(B,Bt,5,9);
			MultMatrix(Bt,B,BtB,9,5,9);
			AddMatrix(_BtB,BtB,_BtB,9,9);
			MultMatrix(Bt,l2,Btl,9,5,1);
            AddMatrix(_Btl,Btl,_Btl,9,1);
		}		
	    InverMatrix(_BtB,9);
        MultMatrix(_BtB,_Btl,Delt,9,9,1);

		max=0;
		for (i=0;i<9;i++)
		{
			K[i] += Delt[i];
			if (max < fabs(Delt[i]))
			{
				max = fabs(Delt[i]);
			}
		}

	} while (max > 1.0e-6);

	//step3:分解计算结果
step3:
	By = -(K[0]*K[6] + K[1]*K[7] +K[2]*K[8])/Bx;
	Bz = (K[3]*K[6] + K[4]*K[7] +K[5]*K[8])/Bx;
	
	*u = By/Bx; 
	*v = Bz/Bx;
	
	double R[9];
	R[0] = (K[2]*K[4] - K[5]*K[1] - Bz*K[0] - By*K[3])/(Bx*Bx + By*By + Bz*Bz);
	R[3] = (K[0]*K[5] - K[2]*K[3] - Bz*K[1] - By*K[4])/(Bx*Bx + By*By + Bz*Bz);
	R[6] = (K[1]*K[3] - K[0]*K[4] - Bz*K[2] - By*K[5])/(Bx*Bx + By*By + Bz*Bz);
    R[1] = (By*R[0] + K[3])/Bx;
    R[4] = (By*R[3] + K[4])/Bx;
    R[7] = (By*R[6] + K[5])/Bx;
	R[2] = (Bz*R[0] + K[0])/Bx;
	R[5] = (Bz*R[3] + K[1])/Bx;
	R[8] = (Bz*R[6] + K[2])/Bx;
	
	double R1[9];
	Bx = Bx/sqrt(Bx*Bx + By*By + Bz*Bz);
	By = By/sqrt(Bx*Bx + By*By + Bz*Bz);
	Bz = Bz/sqrt(Bx*Bx + By*By + Bz*Bz);
	R1[0] = 2*Bx*Bx - 1;
	R1[4] = 2*By*By - 1;
	R1[8] = 2*Bz*Bz - 1;
	R1[1] = R1[3] = 2*Bx*By;
	R1[2] = R1[6] = 2*Bx*Bz;
	R1[5] = R1[7] = 2*By*Bz;
	  
	*phi = atan2(-R[6], R[8]);
	*omiga = asin(-R[7]);
	*kappa = atan2(R[1],R[4]);
	
	if(*phi < -PI/2 || *omiga < -PI/2)
	{
		double R1R[9];
		MultMatrix(R1,R,R1R,3,3,3);
		*phi = atan2(-R1R[6],R1R[8]);
		*omiga = asin(-R1R[7]);
		*kappa = atan2(R1R[1],R1R[4]);
	}

	return 1;
}
/****************************计算相对定向残差***************************************/
// 算法名：CalqErr
// 说明：计算相对定向残差
// 参数：
/****************************计算相对定向残差***************************************/
PHOTOGRAMMETRYALGORITHMN_API void CalqErr(double num,double *x1,double *y1,double f1,double *x2,double *y2,double f2,double u,double v,
			 double phi,double omiga,double kappa,double *qErr,double *sigma)
{
	int i;
	double R[9];
	double zb0[3],zb1[3],zb2[3];
	double sum = 0;
	RotationMatrix(phi,omiga,kappa,R);
	double Bx =1, By = u, Bz = v;
	for (i=0;i<num;i++)
	{ 
		zb0[0] = x1[i];
		zb0[1] = y1[i];
		zb0[2] = -f1;
		zb1[0] = x2[i];
		zb1[1] = y2[i];
		zb1[2] = -f2;
		MultMatrix(R,zb1,zb2,3,3,1);
		//计算点头应系数
		double N1,N2;
		N2 = (Bx*zb0[2]-Bz*zb0[0])/(zb0[0]*zb2[2]-zb0[2]*zb2[0]);
		N1 = (Bx*zb2[2]-Bz*zb2[0])/(zb0[0]*zb2[2]-zb0[2]*zb2[0]);
		
		qErr[i] = N1*zb0[1] - N2*zb2[1] - By;
        sum += pow(qErr[i],2);
	}
	*sigma = sqrt(sum);
}

四、运行界面和结果展示

其中，测试数据是用上一节的人工量点程序量测的同名像点座标数据，相对定向中误差内在地表明了相对定向的正确性，而下一节中利用该相对定向结果生成的核线影像不存在上下视差则可以外在地证明了相对定向的正确性。

 五、补充说明

本帖介绍和实现的是连续法相对定向，根据武大版《摄影测量学》（第二版）第55页的说明，连续法相对定向假设By约等于Bx*u,Bz约等于Bx*v，这个假设显然是建立在u和v很小的前提之下的，这就意味着连续法相对定向认为在摄影测量座标系下，两个摄站（相机）的位移主要发生在x方向，而在y和z的方向的位移相比之下非常小。这个条件在早期的航空摄影测量中是容易满足的，但是如今的无人机影像飞行质量非常随意，甚至无序，这时常常不能满足连续法相对定向的前提假设，那么本文的方法也就不能直接拿来使用了。