相對定向--雙像三維建模小軟件開發實例（五）

一、基本概念

        在攝影測量學中，恢復立體像對兩張像片之間的相對方位的過程叫做立體像對的相對定向。相對定向至少有兩個重要作用：其一，完成立體像對的相對定向後 ,其相應光線在該面內成對相交，形成一個與實地相似的幾何模型，對該幾何模型進行絕對定向後即可實現對地面實際地形、地物的量測；其二，相對定向確定相對方位元素後可以生成核線影像，加快影像匹配的速度、提高匹配效率、增加匹配可靠性。

二、原理介紹

相對定向的數學模型是共麪條件方程：

其中，分別爲左右影像上同名像點各自的空間輔助座標。相對定向有兩種方法：迭代求解法，直接求解法，和帶約束的直接求解法，其中迭代求解法按照定向元素選擇的不同分爲連續像對的相對定向和獨立像對的相對定向兩種。

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的方向的位移相比之下非常小。這個條件在早期的航空攝影測量中是容易滿足的，但是如今的無人機影像飛行質量非常隨意，甚至無序，這時常常不能滿足連續法相對定向的前提假設，那麼本文的方法也就不能直接拿來使用了。