ground.txt内容:
36589.41 25273.32 2195.17
37631.08 31324.51 728.69
39100.97 24934.98 2386.50
40426.54 30319.81 757.31
image.txt内容:
-86.15 -68.99
-53.40 82.21
-14.78 -76.63
10.46 64.43
程序如下:
#include "stdio.h"
#include "math.h"
#define PI 3.1415926
//求矩阵a的转置矩阵b,a为m行、n列
void transpose(double *a, double *b, int m, int n);
//矩阵a乘以矩阵b,结果存储在c中,a为m×n大小,b为n×l大小
void multiply(double *a, double *b, double *c, int m, int n, int l);
//求矩阵a的逆
int inMerse1(double *a, int n);
//输出m行、n列的矩阵a
void shuchu(double *a, int m, int n);
//求矩阵a与b的差值矩阵,a、b、c大小均为m×n
void subtrat(double *a, double *b, double *c, int m, int n);
void main()
{
FILE *fp = NULL;
FILE *fp1 = NULL;
if((fp=fopen("image.txt","r")) == NULL)
{
printf("Open file error!");
return;
}
if((fp1=fopen("ground.txt","r")) == NULL)
{
printf("Open file error!");
return;
}
//像点坐标和地面点坐标
double imagecontrol[4][2]={0.0};
double groundcontrol[4][3]={0.0};
//摄影比例尺分母
double scale = 40000.0;
double f=0.15324;
long i,j,k;
for(i=0; i<4; i++)
{
for(j=0; j<2; j++)
{
fscanf(fp, "%lf", &imagecontrol[i][j]);
imagecontrol[i][j] /= 1000.0;
}
for(k=0; k<3; k++)
{
fscanf(fp1, "%lf", &groundcontrol[i][k]);
}
}
fclose(fp);
fclose(fp1);
double Zs(0.0),Xs(0.0),Ys(0.0),p0(0.0),w0(0.0),k0(0.0);
Zs=scale*f;
for( i=0;i<4;i++)
{
Xs+=groundcontrol[i][0];
Ys+=groundcontrol[i][1];
}
Xs/=4.0;
Ys/=4.0;
double R[3][3]={0.0};
double L3=0.0,L1=0.0,L2=0.0;
double L[8][1]={0.0},ATA1[6][6]={0.0},x=0.0,y=0.0;
double A[8][6]={0.0},AT[6][8]={0.0},ATA[6][6]={0.0},ATL[6][1]={0.0},M[6][1]={0.0},B[6][8]={0.0};
double AM[8][1]={0.0},V[6][1]={0.0};
int n(0);
do
{
R[0][0]=cos(p0)*cos(k0)-sin(p0)*sin(w0)*sin(k0);
R[0][1]=(-1)*cos(p0)*sin(k0)-sin(p0)*sin(w0)*cos(k0);
R[0][2]=(-1)*sin(p0)*cos(w0);
R[1][0]=cos(w0)*sin(k0);
R[1][1]=cos(w0)*cos(k0);
R[1][2]=(-1)*sin(w0);
R[2][0]=sin(p0)*cos(k0)+cos(p0)*sin(w0)*sin(k0);
R[2][1]=(-1)*sin(p0)*sin(k0)+cos(p0)*sin(w0)*cos(k0);
R[2][2]=cos(p0)*cos(w0);
for(i=0,j=0;j<4;i+=2,j++)
{
L1=R[0][0]*(groundcontrol[j][0]-Xs)+R[1][0]*(groundcontrol[j][1]-Ys)+R[2][0]*(groundcontrol[j][2]-Zs);
L2=R[0][1]*(groundcontrol[j][0]-Xs)+R[1][1]*(groundcontrol[j][1]-Ys)+R[2][1]*(groundcontrol[j][2]-Zs);
L3=R[0][2]*(groundcontrol[j][0]-Xs)+R[1][2]*(groundcontrol[j][1]-Ys)+R[2][2]*(groundcontrol[j][2]-Zs);
x=(-1)*f*L1/L3;
y=(-1)*f*L2/L3;
L[2*j][0]=imagecontrol[j][0]-x;
L[2*j+1][0]=imagecontrol[j][1]-y;
A[i][0]=(-1)*f/(Zs-groundcontrol[j][2]);
A[i][1]=0.0;
A[i][2]=(-1)*x/(Zs-groundcontrol[j][2]);
A[i][3]=(-1)*f*(1+(x*x/(f*f)));
A[i][4]=(-1)*x*y/f;
A[i][5]=y;
A[i+1][0]=0;
A[i+1][1]=(-1)*f/(Zs-groundcontrol[j][2]);
A[i+1][2]=(-1)*y/(Zs-groundcontrol[j][2]);
A[i+1][3]=(-1)*x*y/f;
A[i+1][4]=(-1)*f*(1+y*y/(f*f));
A[i+1][5]=(-1)*x;
}
transpose(&A[0][0],&AT[0][0],8,6);
multiply(&AT[0][0],&A[0][0],&ATA[0][0],6,8,6);
inMerse1(*ATA,6);
multiply(&ATA[0][0],&AT[0][0],&B[0][0],6,6,8);
multiply(&B[0][0],&L[0][0],&V[0][0],6,8,1);
Xs+=V[0][0];
Ys+=V[1][0];
Zs+=V[2][0];
p0+=V[3][0];
w0+=V[4][0];
k0+=V[5][0];
n++;
}while(fabs(V[3][0])*180*60/PI>=0.1 || fabs(V[4][0])*180*60/PI>=0.1 || fabs(V[5][0])*180*60/PI>=0.1);
R[0][0]=cos(p0)*cos(k0)-sin(p0)*sin(w0)*sin(k0);
R[0][1]=(-1)*cos(p0)*sin(k0)-sin(p0)*sin(w0)*cos(k0);
R[0][2]=(-1)*sin(p0)*cos(w0);
R[1][0]=cos(w0)*sin(k0);
R[1][1]=cos(w0)*cos(k0);
R[1][2]=(-1)*sin(w0);
R[2][0]=sin(p0)*cos(k0)+cos(p0)*sin(w0)*sin(k0);
R[2][1]=(-1)*sin(p0)*sin(k0)+cos(p0)*sin(w0)*cos(k0);
R[2][2]=cos(p0)*cos(w0);
printf("旋转矩阵R为:\n");
shuchu(&R[0][0],3,3);
printf("外方为元素为:\n");
printf("Xs=%lf\n",Xs);
printf("Ys=%lf\n",Ys);
printf("Zs=%lf\n",Zs);
printf("迭代次数为:%d\n",n);
fclose(fp);
}
//求矩阵a的转置矩阵b,a为m行、n列
void transpose(double *a, double *b, int m, int n)
{
int i,j;
for(i=0;i<m;i++)
{
for(j=0;j<n;j++)
{
b[j*m+i] = a[i*n+j];
}
}
}
//矩阵a乘以矩阵b,结果存储在c中,a为m×n大小,b为n×l大小
void multiply(double *a, double *b, double *c, int m, int n, int l)
{
int i,j,k;
double t;
for(i=0;i<m;i++)
{
for(j=0;j<l;j++)
{
t=0;
for(k=0;k<n;k++)
{
t += a[i*n+k]*b[k*l+j];
}
c[i*l+j]=t;
}
}
}
//shu(m,nn);
//下面开始上三角计算!!
for (i=n3-1 ;i>0;i--)
{
if(m[i][i]==0){printf("此矩阵的值为零!!!!\n");}
for (k=i-1;k>=0;k--)
{
w=m[k][i]/m[i][i];
for (j=n3-1 ;j>=0;j--)
{
m[k][j]=m[k][j]- w * m[i][j];
n[k][j]=n[k][j]-w * n[i][j];
}
}
}
//除去对角阵上的数得出逆矩阵!!
for (i=0 ;i<n3;i++)
{ w=m[i][i];
for (j=0;j<n3;j++)
{
n[i][j]=n[i][j]/w;
}
}
}
//求矩阵a的逆
int inMerse1(double *a, int n)
{
int *is, *js, i, j, k, l, u, M;
double d,p;
is = new int[n];
js = new int[n];
for(k=0; k<n; k++)
{
d=0.0;
for(i=k; i<n; i++)
{
for(j=k; j<n; j++)
{
l=i*n+j; p=fabs(a[l]);
if (p>d) { d=p; is[k]=i; js[k]=j;}
}
}
if(d+1.0 == 1.0)
{
delete []is;
delete []js;
printf("Error, not inMerse!\n");
return 0;
}
if(is[k] != k)
{
for(j=0; j<n; j++)
{
u=k*n+j;
M=is[k]*n+j;
p=a[u];
a[u]=a[M];
a[M]=p;
}
}
if(js[k]!=k)
{
for(i=0; i<=n-1; i++)
{
u=i*n+k;
M=i*n+js[k];
p=a[u];
a[u]=a[M];
a[M]=p;
}
}
l=k*n+k;
a[l]=1.0/a[l];
for(j=0; j<n; j++)
{
if(j!=k)
{
u=k*n+j; a[u]=a[u]*a[l];
}
}
for(i=0; i<n; i++)
{
if(i!=k)
{
for(j=0; j<n; j++)
{
if(j!=k)
{
u=i*n+j;
a[u]=a[u]-a[i*n+k]*a[k*n+j];
}
}
}
}
for(i=0; i<n; i++)
{
if(i!=k)
{
u=i*n+k;
a[u]=-a[u]*a[l];
}
}
}
for(k=n-1; k>=0; k--)
{
if (js[k]!=k)
{
for(j=0; j<=n-1; j++)
{
u=k*n+j; M=js[k]*n+j;
p=a[u]; a[u]=a[M]; a[M]=p;
}
}
if(is[k]!=k)
{
for(i=0; i<=n-1; i++)
{
u=i*n+k; M=i*n+is[k];
p=a[u]; a[u]=a[M]; a[M]=p;
}
}
}
delete []is;
delete []js;
return 1;
}
//输出m行、n列的矩阵a
void shuchu(double *a, int m, int n)
{
int i,j;
for(i=0;i<m;i++)
{
for(j=0;j<n;j++)
{
printf("%9lf ",a[i*n+j]);
}
printf("\n");
}
}
//求矩阵a与b的差值矩阵,a、b、c大小均为m×n
void subtrat(double *a, double *b, double *c, int m, int n)
{
int i,j;
for(i=0;i<m;i++)
{
for(j=0;j<n;j++)
{
c[i*n+j] = a[i*n+j] - b[i*n+j];
}
}
}