float* fInit1DPointer(int num){register int i;float* p = new float[num];for(i=0; i<num; i++){p[i]=0.0f;}return(p);}float** fInit2DPointer(int row,int col){register int i,j;float* arr = new float[row*col];float** p = new float*[row];for(i=0; i<row; i++){p[i] = arr + i*col;for(j=0; j<col; j++){p[i][j]=(float)0.0;}}return(p);}//求a矩阵的转置；//其中输入矩阵a为m×n阶，结果保存在n×m阶矩阵b里；void transpose(double a[], int m, int n, double b[]){int i,j;for (i=0; i<n; i++)for(j=0; j<m; j++){//s = a[j*n+i];//b[i*m+j] = s;b[i*m+j] = a[j*n+i];}}//矩阵相乘；//m×n阶的矩阵a和n×k阶的矩阵b相乘，得到m×k阶的矩阵保存在c中；void trmul(double a[], double b[], int m, int n, int k, double c[]){int i,j,l,u;for (i=0; i<=m-1; i++)for (j=0; j<=k-1; j++){u=i*k+j; c[u]=0;for (l=0; l<=n-1; l++)c[u]=c[u]+a[i*n+l]*b[l*k+j];}return;}//平方根法（Cholesky法）求实正定对称方程组的解；//输入n×n阶正定对称系数矩阵a，n×m阶常数矩阵；//返回分解后的U矩阵上三角部分存于a，方程的m组解向量存于d；int chlk(double a[], int n, int m, double d[]){int i,j,k,u,v;if ((a[0]+1.0==1.0)||(a[0]<0.0))//if (a<0){ printf("fail\n"); return(false);}a[0]=sqrt(a[0]);for (j=1; j<=n-1; j++) a[j]=a[j]/a[0];for (i=1; i<=n-1; i++){u=i*n+i;for (j=1; j<=i; j++){ v=(j-1)*n+i;a[u]=a[u]-a[v]*a[v];}if ((a[u]+1.0==1.0)||(a[u]<0.0)){ printf("fail\n"); return(false);}a[u]=sqrt(a[u]);if (i!=(n-1)){ for (j=i+1; j<=n-1; j++){ v=i*n+j;for (k=1; k<=i; k++)a[v]=a[v]-a[(k-1)*n+i]*a[(k-1)*n+j];a[v]=a[v]/a[u];}}}for (j=0; j<=m-1; j++){ d[j]=d[j]/a[0];for (i=1; i<=n-1; i++){ u=i*n+i; v=i*m+j;for (k=1; k<=i; k++)d[v]=d[v]-a[(k-1)*n+i]*d[(k-1)*m+j];d[v]=d[v]/a[u];}}for (j=0; j<=m-1; j++){u=(n-1)*m+j;d[u]=d[u]/a[n*n-1];for (k=n-1; k>=1; k--){ u=(k-1)*m+j;for (i=k; i<=n-1; i++){ v=(k-1)*n+i;d[u]=d[u]-a[v]*d[i*m+j];}v=(k-1)*n+k-1;d[u]=d[u]/a[v];}}return(true);}//求超定方程的最小二乘解//输入m×n阶系数矩阵r，m×1阶常数矩阵p；//返回值为n×1阶解向量；float *fun(float r[], float p[], int m, int n){int i,j;double *R = new double [m*n];double *P = new double [m*1];double *RT = new double [n*m];double *a = new double [n*n];double *b = new double [m*1];float *Q = new float [n*1];for (i=0; i<m*n; i++){R[i] = r[i];P[i] = p[i];}//for (j=0; j<m; j++)//P[j] = p[j];transpose(R, m, n, RT);trmul(RT, R, n, m, n, a);trmul(RT, P, n, m, 1, b);int flag = chlk(a, n, 1, b);if (!flag)printf("Error!");elsefor (i=0; i<n; i++)Q[i] = (float)b[i];return Q;delete []R;delete []P;delete []RT;delete []a;delete []b;delete []Q;}void get_H_matrix(matchingslist matchings,CvMat* H_Mat1,CvMat* H_Mat2){int numPoints=matchings.size();int i,j;CvPoint2D32f *points1=new CvPoint2D32f[numPoints];CvPoint2D32f *points2=new CvPoint2D32f[numPoints];matchingslist::iterator ptr = matchings.begin();for(i=0;i<numPoints;ptr++,i++){points1[i].x=cvRound(ptr->first.x);points1[i].y=cvRound(ptr->first.y);points2[i].x=cvRound(ptr->second.x);points2[i].y=cvRound(ptr->second.y);}float *a;float *b;a=fInit1DPointer(2*numPoints*8);b=fInit1DPointer(2*numPoints);for(i = 0; i <numPoints; ++i ){a[i*8+0] = a[(i+numPoints)*8+3] =points1[i].x;a[i*8+1] = a[(i+numPoints)*8+4] =points1[i].y;a[i*8+2] = a[(i+numPoints)*8+5] = 1;a[i*8+3] = a[i*8+4] = a[i*8+5] =0;a[(i+numPoints)*8+0] = a[(i+numPoints)*8+1] = a[(i+numPoints)*8+2] = 0;a[i*8+6] = -points1[i].x*points2[i].x;a[i*8+7] = -points1[i].y*points2[i].x;a[(i+numPoints)*8+6] = -points1[i].x*points2[i].y;a[(i+numPoints)*8+7] = -points1[i].y*points2[i].y;b[i]=points2[i].x;b[i+numPoints]=points2[i].y;}float *x;x=fInit1DPointer(8);x = fun(a, b, 2*numPoints,8);for(i=0;i<8;i++)cvmSet(H_Mat1,i/3,i%3,x[i]);cvmSet(H_Mat1,2,2,1.0);delete [] a;delete [] b;delete [] x;}
