dlt-xyz_sf.cpp

近景摄影测量：由像片坐标和L系数反算点的物方坐标的实用程序。

#include "math.h"
#include "stdio.h"

int max_row(int row,int col,double *a,int k)
{
	int i,r;
	double max=(double)fabs(*(a+k*col+k));
	r=k;
	for(i=k+1;i<row;i++)
	{
		if(fabs(*(a+i*col+k))>max)
		{
			max=(double)fabs(*(a+i*col+k));
			r=i;
		}
	}
	return r;
}

void print_x(int num,double *x)
{
	int i;
	printf("  root:  ");
	for(i=0;i<num;i++)
		printf("%f ",*(x+i));
	printf("\n");
}

void print_er()
{
	printf("fail,the solution is not union");

}

void exchange(int row,int col,double *a,int r, int k)
{
	int j;
	double t;
	for(j=k;j<col;j++)
	{
		t=*(a+k*col+j);*(a+k*col+j)=*(a+r*col+j);*(a+r*col+j)=t;
	}
}

void Gauss(int row,int col,double *a,double *b)
{
     double t;
	 int i,k,j;
	 for(k=0;k<row-1;k++)
	 {
		 int r=max_row(row,col,a,k);
		 if(fabs(*(a+r*col+k))<1e-5)
			{
			 print_er();
			 return;
            }
		 if(k!=r)
            {
			 exchange(row,col,a,r,k);
			 t=b[k];b[k]=b[r];b[r]=t;
            }
		 for(i=k+1;i<row;i++)
		 {
			 for(j=k+1;j<col;j++)
				 *(a+i*col+j)=*(a+i*col+j)-*(a+i*col+k)*(*(a+k*col+j))/(*(a+k*col+k));

			//	 a[i][j]=a[i][j]-a[i][k]*a[k][j]/a[k][k];
			 *(b+i)=*(b+i)-(*(a+i*col+k))*(*(b+k))/(*(a+k*col+k));
       		// b[i]=b[i]-a[i][k]*b[k]/a[k][k];
		 }
	 }
	 *(b+row-1)=*(b+row-1)/(*(a+(row-1)*col+row-1));
	 //b[ROW-1]=b[ROW-1]/a[ROW-1][ROW-1];
	 for(i=row-2;i>=0;i--)
		 {
		 double t=0;
		 for(j=i+1;j<col;j++)
             t+=*(a+i*col+j)*(*(b+j));
			 //t+=a[i][j]*b[j];
         *(b+i)=(*(b+i)-t)/(*(a+i*col+i));
		 //b[i]=(b[i]-t)/a[i][i];
		 }
}

void CAL_3D_DLT_Approximation_XYZ(double lx,double ly,double rx,double ry,double *leftl,double *rightl,double *XYZ)
 {  int j;
    double a[3][3];
	double b[3];
    a[0][0]=leftl[0]+lx*leftl[8];a[0][1]=leftl[1]+lx*leftl[9];a[0][2]=leftl[2]+lx*leftl[10];b[0]=-(leftl[3]+lx);
	a[1][0]=leftl[4]+ly*leftl[8];a[1][1]=leftl[5]+ly*leftl[9];a[1][2]=leftl[6]+ly*leftl[10];b[1]=-(leftl[7]+ly);
	a[2][0]=rightl[0]+rx*rightl[8];a[2][1]=rightl[1]+rx*rightl[9];
	a[2][2]=rightl[2]+rx*rightl[10];b[2]=-(rightl[3]+rx);
	Gauss(3,3,&a[0][0],b);
	for(j=0;j<3;j++){
     XYZ[j]=b[j];
	 printf("%f,",XYZ[j]);}
	 printf("\n 误差：");
  
}
void main ( )
{  int i,j;double lx,ly,rx,ry;double wuch[4][3];
   static double lxy[4][2]={446,468,831,974,1041,468,1324,758};
  static double rxy[4][2]={33,476,491,973,658,469,897,761};
  static double leftl[11]={-494.092996,-562.396422,1.551322,-964.833259,-65.608357,-454.151696,403.280363,-676.339620,
0.016807,1.024839,0.004553};
  static double rightl[11]={-413.164003,-126.627470,8.777178,-478.178895,-8.831137,-146.052182,
399.431262,-621.214468,0.004812,0.218688,-0.004506};
   static double ys[4][3]={-1.044872,-0.03353,0.37318,-0.026393,0.776612,-1.0208,
	0.45765,0.281866,0.40256,0.957456,-0.336184,-0.311367};
   double XYZ[3];
   for(i=0;i<4;i++)
   {printf("\n 坐标：");
     lx=lxy[i][0];ly=lxy[i][1];rx=rxy[i][0];ry=rxy[i][1];
     CAL_3D_DLT_Approximation_XYZ(lx,ly,rx,ry,&leftl[0],&rightl[0],&XYZ[0]);
      for (j=0;j<3;j++)
	  {wuch[i][j]=XYZ[j]-ys[i][j];
	  printf("%f,",wuch[i][j]);
	  }
   }
  
 }