#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]);
}
}
}
dlt-XYZ_sf.rar_dlt 3d_photogrammetry_摄影_测量_测量 摄影
版权申诉
167 浏览量
2022-09-23
06:31:52
上传
评论
收藏 2KB RAR 举报
JaniceLu
- 粉丝: 78
- 资源: 1万+