请帮忙看看这个程序还要改哪些地方~目的是基于最小二乘的影像匹配，最终输出两幅图之间所有同名点坐标

Question

#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); //有解
}
接下面

Answer 1

void main()
{

int m=5;                                     //匹配窗口大小m*m=5*5
double min1=0.005;
FILE *fp;
int i,j,k=0;                                 //控制循环变量
int p,q;                                     //控制循环变量
int i1,i2,j1,j2,i3,j3,i4,j4,ii1,jj1; int Height=240;                              //图像的高
int Width =320;                              //图像的宽
long size=Height*Width;

unsigned char *Left;                         //指向左图的指针
unsigned char *Right;                        //指向右图的指针
Left= (unsigned char *)calloc(size,1);
Right=  (unsigned char *)calloc(size,1); unsigned char Limage[76800],Rimage[76800];        //左图和右图存放的数组 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 *Xr,*Yr ;                              //Xr是X 坐标改正，Yr是Y坐标改正 
Xr = (int *)calloc(m*m,1);   
Yr = (int *)calloc(m*m,1);   unsigned char *F,*G,*GO;
F =(unsigned char*)calloc(m*m,1);       //左匹配窗口          
G =(unsigned char*)calloc(4*m*m,1);       //右匹配窗口
GO = (unsigned char*)calloc(m*m,1); int Gx,Gy;                                   //差分运算代替求偏导

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;
//读左图并存放至数组Limage
fp = fopen("IMG0.raw","rb");
if (fp==NULL) 
{
printf("open file IMG0.raw error!\n");
exit(0);
}
fread(Left,size,1,fp);
fclose(fp); fp = NULL;


for(i=0;i<Height;i++)
{
for(j=0;j<Width;j++)
{
Limage[i*Width+j]=Left[i*Width+j];
}
} //读右图并存放至数组Rimage
fp = fopen("IMG1.raw","rb");
if (fp==NULL) 
{
printf("open file IMG1.raw error!\n");
exit(0);
}
fread(Right,size,1,fp);
fclose(fp); fp = NULL;


for(i=0;i<Height;i++)
{
for(j=0;j<Width;j++)
{
Rimage[i*Width+j]=Right[i*Width+j];
}
}
    i=0;j=0;
while (i<=Width/m)
{
while(j<=Height/m)               //读左窗口
{
Left = &(Limage[j*m*Width+i*m]);
for(i1=0;i1<m;i1++)
{
for(j1=0;j1<m;j1++)
{
F[i1*m+j1]= Left[i1*m+j1];   //目标图像  !!y*wid+x!!
}
}
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 //读右窗口
Right = &(Rimage[j*2*m*Width+i*2*m]);
for(i1=0;i1<2*m;i1++)
{
for(j1=0;j1<2*m;j1++)
{
G[i1*2*m+j1]= Right[i1*2*m+j1];   //目标图像  !!y*wid+x!!
}
} X02=m;        Y02=m;         //搜索中心为(5,5)
k2 =Y02-m;   k4 =X02-m;     //搜索起点(k4,k2),搜索中心(X02,Y02)  k2==i k4==j while (min1>=0.005) 
{
//定搜索窗口
//几何改正：根据几何变形改正参数a0,a1,a2,b0,b1,b2将左方影像窗口坐标变化到右方影像窗口
//xr=a0+a1*xL+a2*yL
//yr=b0+b1*xL+b2*yL
for(i1=0;i1<m;i1++)
{
for(j1=0;j1<m;j1++)
{
Xr[i1*m+j1]=(int)(a[2]+a[3]*k3+a[4]*k1);
Yr[i1*m+j1]=(int)(a[5]+a[6]*k3+a[7]*k1);
k1++;k3++;

}
}

k1 =Y01-m/2;  k3 =X01-m/2;   //匹配起点(k3,k1),匹配中心(X01,Y01)  k1==i k3==j
k2 =Y02-m;   k4 =X02-m;     //搜索起点(k4,k2),搜索中心(X02,Y02)  k2==i k4==j //右方影像窗口进行辐射畸变改正：G[]=h0+h1*G[]
for(i2=0;i2<m;i2++)
{
for(j2=0;j2<m;j2++)
{
k4 = Xr[i2*m+j2];
k2 = Yr[i2*m+j2];
G[k2*m+k4]=(unsigned char)(a[0]+a[1]*G[k2*m+k4]);          
}
}
for(i2=0;i2<8;i2++)
{
g1[i2][0]=0.;
for(j2=0;j2<8;j2++)
f1[i2][j2]=0.;
}

//判断是否要重解变形参数，即计算变形参数改正值x[]=dh0,dh1,da0,da1,da2,db0,db1,db2
//（判断条件：参数改正值的绝对值小于某个阈值min1）
//重解(min1>.005)：则进行组法方程并解法方程
//不重解(min1<0.005)：计算最佳匹配点位，给出同名点坐标，程序结束
k=0;
//******法化组成法方程******//
for(i3=0;i3<m;i3++)
for(j3=0;j3<m;j3++)
{
i1=(i3-1<0)?i3:i3-1; //防止差分越界，以后无用
i2=(i3+1>m-1)?i3:i3+1;
j1=(j3-1<0)?j3:j3-1;
j2=(j3+1>m-1)?j3:j3+1;
Gx=(G[i3*m+j2]-G[i3*m+j1])/2; //差分运算
Gy=(G[i2*m+j3]-G[i1*m+j3])/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[i3*m+j3]*1.0f;   //A[7]=-h1*g    =-h1*c
A[8]=a[0]+a[1]*G[i3*m+j3]-F[i3*m+j3]; //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(ii1=0;ii1<8;ii1++)
{
if(fabs(a[ii1])>min1) min1=fabs(a[ii1]);  //求改正参数的最大值以此作为阈值min1
}

// k=0;
// for(i=0;i<m;i++)          /*    坐标改正 （1）    */
// for(j=0;j<m;j++)
// {
// Xr[k]=a[0]+a[1]*Xr[k]+a[2]*Yr[k];
// Yr[k]=a[3]+a[4]*Xr[k]+a[5]*Yr[k];
// k=k+1;
// }
//
if(Xr[m*m/2]>= 0.5) X02=X02+(int)(Xr[m*m/2]+0.5);
if(Xr[m*m/2]<=-0.5) X02=X02+(int)(Xr[m*m/2]-0.5);
if(Yr[m*m/2]>= 0.5) Y02=Y02+(int)(Yr[m*m/2]+0.5);
if(Yr[m*m/2]<=-0.5) Y02=Y02+(int)(Yr[m*m/2]-0.5); }
//一次匹配成功，输出结果
for(ii1=0;ii1<m;ii1++)
{
for(jj1=0;jj1<m;jj1++)
{
k4 = Xr[ii1*m+jj1];
k2 = Yr[ii1*m+jj1];
Xt1=(float)(X02+(k4+jj1)*Gx*Gx);
Xt2=Xt2+Gx*Gx;
Yt1=(float)(Y02+(k2+ii1)*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(%d  %.d)\n",xL,yL,xR,yR);
fclose(fp);
fp = NULL;
//************print*****************//
timeR++;
j++; }
timeL++;
i++;
}
}