crp－call.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_Approximation_L(int row,int col,double *a,double *b,double *l)
{
  Gauss(row,col,a,b);
  for(int i=0;i<row;i++) *(l+i)=*(b+i);
}
main ( )
{   //void CNearSurLeftView::OnCalL()
   //CNearSurDoc *pDoc=GetDocument();
  // CLongPoint *pPoint;
   double a[11][11];
   double b[11];
   double m_L[11];
static double xyz[6][3]={-1.04318,-0.0335,1.08444,-1.039623,-0.032749,-1.03657,-0.49402,0.405341,0.37289,
0.461589,0.281311,-0.31302,1.301502,-0.77037,0.90576,1.302772,-0.759761,-1.0205};
static double  xy[6][2]={450,163,460,1071,673,481,1044,742,4961,197,4957,1099};

  //相片的六个控制点坐标xyz[6][3]，对应的像点坐标xy[6][2]
   int i;
 for( i=0;i<6;i++)
 {
	   a[i*2][0]=xyz[i][0];
	   a[i*2][1]=xyz[i][1];
	   a[i*2][2]=xyz[i][2];
	   a[i*2][3]=1;
	   a[i*2][4]=a[i*2][5]=a[i*2][6]=a[i*2][7]=0;
	   a[i*2][8]=xy[i][0]*xyz[i][0];
	   a[i*2][9]=xy[i][0]*xyz[i][1];
	   a[i*2][10]=xy[i][0]*xyz[i][2];
	   b[i*2]=-xy[i][0];
       if(i<5)
	   {
	   a[i*2+1][0]=a[i*2+1][1]=a[i*2+1][2]=a[i*2+1][3]=0;	   	   
	   a[i*2+1][4]=xyz[i][0];
	   a[i*2+1][5]=xyz[i][1];
	   a[i*2+1][6]=xyz[i][2];
	   a[i*2+1][7]=1;
	   a[i*2+1][8]=xy[i][1]*xyz[i][0];
	   a[i*2+1][9]=xy[i][1]*xyz[i][1];
	   a[i*2+1][10]=xy[i][1]*xyz[i][2];
	   b[i*2+1]=-xy[i][1];
	   }
 }
   CAL_Approximation_L(11,11,&a[0][0],&b[0],&m_L[0]);
    for(i=0;i<11;i++)
   {printf("m_L[%d]=%f\n",i,m_L[i]);
   }
}
  /*void CAL_3D_DLT_Approximation_XYZ(double lx,double ly,double rx,double ry,double *leftl,double *rightl,double &X,double &Y,double &Z)
{       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);
        X=b[0];Y=b[1];Z=b[2];
}*/