请帮忙看看这个程序，实现最小二乘影像匹配的，结果是输出两幅图之间所有同名点的坐标

月小夏 2009-03-01 06:30:23

#include <stdlib.h>
#include <malloc.h>
#include <stdio.h>
#include <math.h>
/*==================================================*/
/* 高斯解方程子程序 */
/* a[n1][n1]*x[n1][1]=b[n1][1] AX=B */
/*==================================================*/
int gs(float *a,float *b,int n1,float *x)
{int *js,l,k,i,j,is,p,q;
float d,t;
js=(int *)malloc(n1*sizeof(int));
l=1;
for(k=0;k<=n1-2;k++)
{d=0.0;
for(i=k;i<=n1-1;i++)
for(j=k;j<=n1-1;j++)
{t=(float)fabs(*(a+i*n1+j));
if(t>d) {d=t;js[k]=j;is=i;}
}
if(d+1.0==1.0) l=0;
else
{if(js[k]!=k)
for(i=0;i<=n1-1;i++)
{p=i*n1+k;q=i*n1+js[k];
t=*(a+p);*(a+p)=*(a+q);*(a+q)=t;
}
if(is!=k)
{for(j=k;j<=n1-1;j++)
{p=k*n1+j;q=is*n1+j;
t=*(a+p);*(a+p)=*(a+q);*(a+q)=t;
}
t=*(b+k);*(b+k)=*(b+is);*(b+is)=t;
}
}
if(l==0)
return(0);
d=*(a+k*n1+k);
for(j=k+1;j<=n1-1;j++)
{p=k*n1+j;*(a+p)=*(a+p)/d;}
*(b+k)=*(b+k)/d;
for(i=k+1;i<=n1-1;i++)
{for(j=k+1;j<=n1-1;j++)
{p=i*n1+j;
*(a+p)=*(a+p)-*(a+i*n1+k)*(*(a+k*n1+j));
}
*(b+i)=*(b+i)-*(a+i*n1+k)*(*(b+k));
}
}
d=*(a+(n1-1)*n1+n1-1);
if(fabs(d)+1.0==1.0)
{free(js);
return(0);
}
*(x+n1-1)=*(b+n1-1)/d;
for(i=n1-2;i>=0;i--)
{t=0.0;
for(j=i+1;j<=n1-1;j++)
t=t+*(a+i*n1+j)*(*(x+j));
*(x+i)=*(b+i)-t;
}
js[n1-1]=n1-1;
for(k=n1-1;k>=0;k--)
if(js[k]!=k)
{t=*(x+k);*(x+k)=*(x+js[k]);*(x+js[k])=t;}
free(js);
return(1); //有解
}
void main()
{
double min1=0.005;
FILE *fp;
int i,j,k=0; //控制循环变量
int p,q; //控制循环变量
int i1,i2,j1,j2;
int Height=240; //图像的高
int Width =320; //图像的宽
unsigned char *Left; //指向左图的指针
unsigned char *Right; //指向右图的指针
Left= (unsigned char *)calloc(Height*Width,1);
Right= (unsigned char *)calloc(Height*Width,1);
int k1,k2,k3,k4; //匹配起点(k3,k1),匹配中心(X01,Y01) k1==i k3==j
int X01,Y01,X02,Y02; //搜索起点(k4,k2),搜索中心(X02,Y02) k2==i k4==j
int m=5; //匹配窗口大小m*m=5*5
float *Xr,*Yr ; //Xr是X 坐标改正，Yr是Y坐标改正
Xr = (float *)calloc(m*m,1);
Yr = (float *)calloc(m*m,1);
int x_shiftL=1; int y_shiftL=1; //目标窗口平移量
int x_shiftR=1; int y_shiftR=1; //搜索窗口平移量
int Gx,Gy; //差分运算代替求偏导
unsigned char *F,*G,*GO;
F =(unsigned char*)calloc(m*m,1);
G =(unsigned char*)calloc(m*m,1);
GO =(unsigned char*)calloc(51*51,1);
float A[9],f1[8][8],g1[8][1],x[8][1]; //f1即为C'C; g1即为C'L; x是方程的解:x=[dh0,dh1,da0,da1,da2,db0,db1,db2]
int solut; //高斯方程是否有解
float a[8]={0,1,0,1,0,0,0,1}; //h0,h1,a0,a1,a2,b0,b1,b2
float Xt=0.0,Yt=0.0,Xs=0.0,Ys=0.0; //Xt,Yt计算最佳匹配点位的中间变量，Xs,Ys是同名点坐标
float Xt1=0.0,Xt2=0.0,Yt1=0.0,Yt2=0.0;
int timeL=0,timeR=0;
//当左图未越界时目标窗口平移
while (Left!=NULL)
{ //当右图未越界时搜索窗口平移
while (Right!=NULL)
{ //读左图窗口：m*m大小
fp = fopen("IMG0.raw","rb");
if (fp==NULL)
{ printf("open file IMG0.raw error!\n"); exit(0);}
fread(Left,m*m,1,fp); fclose(fp);fp = NULL;
X01=m/2;Y01=m/2; //匹配中心为(m/2,m/2)
k1= Y01-m/2; k3= X01-m/2; //匹配起点(k3,k1),匹配中心(X01,Y01) k1==i k3==j
for(i=0; i<m; i++)
{ for(j=0; j<m; j++)
{ Xr[i*m+j]= 0.0; Yr[i*m+j]= 0.0;
F[i*m+j]= Left[(unsigned long)i*m+j]; //目标图像 }}
//读右图窗口：51*51大小
fp = fopen("IMG1.raw","rb");
if (fp==NULL) {
printf("open file IMG1.raw error!\n"); exit(0);}
fread(Right,51*51,1,fp);fclose(fp);fp = NULL;
X02=X01;Y02=Y01; //搜索中心为(51/2,51/2)
k2= Y02-51/2; k4= X02-51/2; //搜索起点(k4,k2),搜索中心(X02,Y02) k2==i k4==j
for(i=0; i<51; i++)
{for(j=0; j<51; j++){GO[i*51+j] = Right[(unsigned long)i*51+j]; }}
//迭代，进行最小二乘匹配
while(min1>0.005)
{ //几何改正：根据几何变形改正参数a0,a1,a2,b0,b1,b2将左方影像窗口坐标变化到右方影像窗口
//Xr=a0+a1*xL+a2*yL//Yr=b0+b1*xL+b2*yL
for(k1=0;k1<m;k1++){
for(k3=0;k3<m;k3++){
Xr[k1*m+k3]=a[2]+a[3]*k3+a[4]*k1;Yr[k1*m+k3]=a[5]+a[6]*k3+a[7]*k1;}}
//对右方影像窗口重采样：
if(Xr[m*m/2]>= 0.5) k4=k4+(int)(Xr[m*m/2]+0.5);
if(Xr[m*m/2]<=-0.5) k4=k4+(int)(Xr[m*m/2]-0.5);
if(Yr[m*m/2]>= 0.5) k2=k2+(int)(Yr[m*m/2]+0.5);
if(Yr[m*m/2]<=-0.5) k2=k2+(int)(Yr[m*m/2]-0.5);
for(i=0;i<51;i++){
for(j=0;j<51;j++){i1 = int( i+ Xr[i*m+j]+ 0.5); //四舍五入取整用
j1 = int( j+ Yr[i*m+j]+ 0.5);
G[i*m+j]=GO[(i1+k2)*51+j1+k4]; }}
//右方影像窗口进行辐射畸变改正：G[]=h0+h1*G[]
for(i=0;i<51;i++){
for(j=0;j<51;j++){G[i*m+j]=(unsigned long)(a[0]+a[1]*G[i*m+j]); }}
//判断是否要重解变形参数，即计算变形参数改正值x[]=dh0,dh1,da0,da1,da2,db0,db1,db2
//（判断条件：参数改正值的绝对值小于某个阈值min1）
//重解(min1>.005)：则进行组法方程并解法方程
//不重解(min1<0.005)：计算最佳匹配点位，给出同名点坐标，程序结束
//******法化组成法方程******//
for(i=0;i<m;i++)
for(j=0;j<m;j++)
{ Xr[k]= i-m/2.0f;Yr[k]= j-m/2.0f; //像素中心化
if(i-1<0)i1=i; else i1=i-1;
if(i+1>m-1)i2=i;else i2=i+1;
if(j-1<0)j1=j; else j1=j-1;
if(j+1>m-1)j2=j;else j2=j+1;
Gx=(G[i*m+j2]-G[i*m+j1])/2; //差分运算
Gy=(G[i2*m+j]-G[i1*m+j])/2;
//********计算矩阵 C'L*********//
A[0]=-a[1]*Gx; //A[0]=-h1*g'x =-h1*c
A[1]=-a[1]*Gx*Xr[k]; //A[1]=-h1*g'x*x=-h1*c
A[2]=-a[1]*Gx*Yr[k]; //A[2]=-h1*g'x*y=-h1*c
A[3]=-a[1]*Gy; //A[3]=-h1*g'y =-h1*c
A[4]=-a[1]*Gy*Xr[k]; //A[4]=-h1*g'y*x=-h1*c
A[5]=-a[1]*Gy*Yr[k]; //A[5]=-h1*g'y*y=-h1*c
A[6]=-1.0; //A[6]=-h1*1 =-h1*c
A[7]=-G[i*m+j]*1.0f; //A[7]=-h1*g =-h1*c
A[8]=a[0]+a[1]*G[i*m+j]-F[i*m+j]; //A[8]= h0+h1*g-f
k=k+1;
for(p=0;p<8;p++){
g1[p][0]=g1[p][0]+A[p]*A[8]; //C'L
for(q=p;q<8;q++) //计算矩阵C'C(step 1)
f1[p][q]=f1[p][q]+A[p]*A[q];
}
//********计算矩阵 C'C(step 2)*********//
for(p=0;p<8;p++)
for(q=p;q<8;q++)
f1[q][p]=f1[p][q];
}
solut=gs(*f1,*g1,8,*x); /* 高斯求解改正数值 */
if(solut) //有解
{
//计算变形参数a[8]
a[0]=a[0]+x[0][0]+a[0]*x[1][0]; //h0=h0+dh0+h0*dh1
a[1]=a[1]+ a[1]*x[1][0]; //h1=h1+ h1*dh1
a[2]=a[2]+x[2][0]+a[2]*x[3][0]+a[5]*x[4][0]; //a0=a0+da0+a0*da1+b0*da2
a[3]=a[3]+ a[3]*x[3][0]+a[6]*x[4][0]; //a1=a1+ a1*da1+b1*da2
a[4]=a[4]+ a[4]*x[3][0]+a[7]*x[4][0]; //a2=a2+ a2*da1+b2*da2
a[5]=a[5]+x[5][0]+a[2]*x[6][0]+a[5]*x[7][0]; //b0=b0+db0+a0*db1+b0*db2
a[6]=a[6]+ a[3]*x[6][0]+a[6]*x[7][0]; //b1=b1+ a1*db1+b1*db2
a[7]=a[7]+ a[4]*x[6][0]+a[7]*x[7][0]; //b2=b2+ a2*db1+b2*db2

}
for(i=0;i<8;i++)
{if(fabs(a[i])>min1) min1=fabs(a[i]); //求改正参数的最大值以此作为阈值min1
}}
//一次匹配成功，输出结果
for(i=0;i<m;i++){
for(j=0;j<m;j++){
Xt1=(float)(X02+(k4+j)*Gx*Gx); Xt2=Xt2+Gx*Gx;
Yt1=(float)(Y02+(k2+i)*Gy*Gy); Yt2=Yt2+Gy*Gy;
} }
Xt=Xt1/Xt2;Yt=Yt1/Yt2;
Xs=a[2]+a[3]*Xt+a[4]*Yt;Ys=a[5]+a[6]*Xt+a[7]*Yt;
//*************print****************//
if ((fp=fopen("result.dat","a"))==NULL)
{ printf("Can not open the file result.dat\n"); exit(0); }
//相对坐标转绝对坐标
int xL,yL;
float xR,yR;
xL=(timeL%m)*m+X01;yL=(timeL/m)*m+Y01;xR=(timeR%m)*m+Xs;yR=(timeR/m)*m+Ys;

fprintf(fp,"(%d %d)\t\t(%.3f %.3f)\n",xL,yL,xR,yR);
fclose(fp);
fp = NULL;
//************print*****************//

}

Right=Right+y_shiftR*m+x_shiftR;
timeR++;
}

Left=Left+y_shiftL*m+x_shiftL;
timeL++;
}

...全文