zhang2dmain.cpp

这是在张正友摄像机标定的基础上对其算法进行改进

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "matrix.h"
#include "matrix_3.h"
#include "cminpack\minpack.h"
#include "zhang2Dmain.h"



void solveAx0(int m, int n, double* A, double* x) 
{
	/* uses the Singular Value Decomposition of A, i.ie, A=UDVt */
	double* U   =(double*) malloc(sizeof(double)*m*n); /* [U]mxn */
	double* D   =(double*) malloc(sizeof(double)*n);   /* [D]nxn, but only the diagonal elements are stored */
	double* V   =(double*) malloc(sizeof(double)*n*n); /* [V]nxn */
	double* tmp =(double*) malloc(sizeof(double)*m);   /*  temporary area 1xm */
	
	mtxSVDAx0(A,m,n,x,U,D,V,tmp);
	
	free(tmp);
	free(V);
	free(D);
	free(U);
}


void normalise2dpts(int nPoints,double *point,double *T)
/************************************************************************/
/* normalise the matrix
/* @param[in] nPoints number of points pairs
/* @param[in][out] points model array
/* @param[in][out] T                                                                 
/************************************************************************/
{
	int i;
	double c1,c2;
	double mean;
	double scale;
	double *temp;
	if((temp = (double*)malloc(3*nPoints*sizeof(double)))==NULL)
	{
		printf("Cannot allocate workspace for temp!!");
		exit(-1);
	}

	for(i=0;i<nPoints;i++)
	{
		point[i*3+0]=point[i*3+0]/point[i*3+2];
		point[i*3+1]=point[i*3+1]/point[i*3+2];
		point[i*3+2]=1;
	}

	c1=0;                c2=0;

	for(i=0;i<nPoints;i++)
	{
		c1+=point[i*3+0];
		c2+=point[i*3+1];
	}

	c1=c1/nPoints;       c2=c2/nPoints;
	
    for(i=0;i<nPoints;i++)
	{
		temp[i*3+0]=point[i*3+0]-c1;
		temp[i*3+1]=point[i*3+1]-c2;
	}

	mean=0;

	for(i=0;i<nPoints;i++)
	{
		mean+=sqrt(temp[i*3+0]*temp[i*3+0]+temp[i*3+1]*temp[i*3+1]);
	}
    
	mean=mean/nPoints;
    scale=sqrt(2)/mean;
 
	T[0] = scale;          T[1] = 0;            T[2] = -scale*c1;
	T[3] = 0;              T[4] = scale;        T[5] = -scale*c2;
	T[6] = 0;              T[7] = 0;            T[8] = 1;
	mtxABt(T,point,3,3,nPoints,temp);
	for(i=0;i<nPoints;i++)
	{
		point[i*3+0]=temp[nPoints*0+i];
		point[i*3+1]=temp[nPoints*1+i];
		point[i*3+2]=temp[nPoints*2+i];
	}
    free(temp);
}


void fcn_lm_H(const int *m, const int *n, const double *x, double *fvec, int *iflag)
{

	int i;
	for(i=0;i<256;i++)
	{
		fvec[i*2+0]=GBimagePoints[i*2+0]-(x[0]*GBmodelPoints[i*3+0]+x[1]*GBmodelPoints[i*3+1]+x[2])/(x[6]*GBmodelPoints[i*3+0]+x[7]*GBmodelPoints[i*3+1]+x[8]);
		fvec[i*2+1]=GBimagePoints[i*2+1]-(x[3]*GBmodelPoints[i*3+0]+x[4]*GBmodelPoints[i*3+1]+x[5])/(x[6]*GBmodelPoints[i*3+0]+x[7]*GBmodelPoints[i*3+1]+x[8]);
	}
    
}


void lm_cminpack(double *matrix,int nPoints,double *lm)
{
	int i;
	int m, n, info, lwa, iwa[9], one=1;
	double tol, *x, *fvec, *wa;
	
	m=nPoints*2;
	n=9;
    lwa = m*n + 5*n + m+1;

	tol = sqrt(dpmpar_(&one));
    if((x=(double*)malloc(9*sizeof(double)))==NULL)
	{
		printf("Cannot allocate workspace for x!!");
		exit(-1);
	}

   
	if((fvec=(double*)malloc(2*nPoints*sizeof(double)))==NULL)
	{
		printf("Cannot allocate workspace for x!!");
		exit(-1);
	}
	

	if((wa=(double*)malloc(lwa*sizeof(double)))==NULL)
	{
		printf("Cannot allocate workspace for x!!");
		exit(-1);
	}

	for(i=0;i<9;i++)
		x[i]=matrix[i];
    lmdif1_(&fcn_lm_H, &m, &n, x, fvec, &tol, &info, iwa, wa, &lwa);
	for(i=0;i<9;i++)
		lm[i]=x[i];
	free(x);
	free(fvec);
	free(wa);
}

void lmHomography(double *imagePoints,double *modelPoints,double *H_intia,int nPoints,double *lm)
{
	int i;
	double *model;
	model = (double*)malloc(nPoints*3*sizeof(double));
    for(i=0;i<nPoints;i++)
	{
		model[i*3+0]=modelPoints[i*2+0];
		model[i*3+1]=modelPoints[i*2+1];
		model[i*3+2]=1;
	}
	GBmodelPoints = model;
	GBimagePoints = imagePoints;
	lm_cminpack(H_intia,nPoints,lm);
	free(model);
}



int homography(int nPoints,double *modelPoints,double *imagePoints,double *H)

/** 
* @brief Computes the homography [H]3x3 given nPoints pairs of Model \n
* and Image Points, i.e, computes [H]                         

* @param[in] nPoints number of points pairs
* @param[in] modelPoints points model array 
* @param[in] imagePoints points image array
* @param[out] H homography
* @return 0 = no-error or 1 = error                           
*/
{
	int i;
	double T1[9];
	double T2[9];
	double *A;
	double T2_contra[9];//逆矩阵
	double det;
	
	if((A = (double*)malloc(3*nPoints*9*sizeof(double)))==NULL)
	{
		printf("Cannot allocate workspace for A!!");
		exit(-1);
	}
	//齐次化
	double *model;
	if((model = (double*)malloc(3*nPoints*sizeof(double)))==NULL)
	{
		printf("Cannot allocate workspace for model!!");
		exit(-1);
	}
	double *image;
	if((image = (double*)malloc(3*nPoints*sizeof(double)))==NULL)
	{
		printf("Cannot allocate workspace for image!!");
		exit(-1);
	}
	for(i=0;i<nPoints;i++)
	{
		model[i*3+0]=modelPoints[i*2+0];
		model[i*3+1]=modelPoints[i*2+1];
		model[i*3+2]=1;
   
        image[i*3+0]=imagePoints[i*2+0];
      	image[i*3+1]=imagePoints[i*2+1];
      	image[i*3+2]=1;
   	}

	normalise2dpts(nPoints,model,T1);//标准化model

    normalise2dpts(nPoints,image,T2);//标准化image
    //对A赋值
	for(i=0;i<nPoints;i++)
	{
		A[(i*3+0)*9+0]=0;
		A[(i*3+0)*9+1]=0;
		A[(i*3+0)*9+2]=0;
		A[(i*3+0)*9+3]=-model[i*3+0]*image[i*3+2];
		A[(i*3+0)*9+4]=-model[i*3+1]*image[i*3+2];
		A[(i*3+0)*9+5]=-model[i*3+2]*image[i*3+2];
		A[(i*3+0)*9+6]=model[i*3+0]*image[i*3+1];
		A[(i*3+0)*9+7]=model[i*3+1]*image[i*3+1];
		A[(i*3+0)*9+8]=model[i*3+2]*image[i*3+1];

		A[(i*3+1)*9+0]=model[i*3+0]*image[i*3+2];
		A[(i*3+1)*9+1]=model[i*3+1]*image[i*3+2];
		A[(i*3+1)*9+2]=model[i*3+2]*image[i*3+2];
		A[(i*3+1)*9+3]=0;
		A[(i*3+1)*9+4]=0;
		A[(i*3+1)*9+5]=0;
		A[(i*3+1)*9+6]=-model[i*3+0]*image[i*3+0];
		A[(i*3+1)*9+7]=-model[i*3+1]*image[i*3+0];
		A[(i*3+1)*9+8]=-model[i*3+2]*image[i*3+0];

		A[(i*3+2)*9+0]=-model[i*3+0]*image[i*3+1];
		A[(i*3+2)*9+1]=-model[i*3+1]*image[i*3+1];
		A[(i*3+2)*9+2]=-model[i*3+2]*image[i*3+1];
		A[(i*3+2)*9+3]=model[i*3+0]*image[i*3+0];
		A[(i*3+2)*9+4]=model[i*3+1]*image[i*3+0];
		A[(i*3+2)*9+5]=model[i*3+2]*image[i*3+0];
		A[(i*3+2)*9+6]=0;
		A[(i*3+2)*9+7]=0;
		A[(i*3+2)*9+8]=0;
	}

	solveAx0(3*nPoints,9,A,H);
	det=m3Inv(T2,T2_contra);
	m3MultAB(T2_contra,H,T2);
	m3MultAB(T2,T1,H);

	for(i=0;i<9;i++)
		H[i]=H[i]/H[8];
    

	lmHomography(imagePoints,modelPoints,H,nPoints,T2);
	for(i=0;i<9;i++)
		H[i]=T2[i]/T2[8];
	free(A);
	free(model);
	free(image);
    return 0;
	
}

void v_compute(int numOfpic,double **H,double *v)
{
	int i,j;
	double h[9];
	double h_temp[9];
	double v11[6];
	double v12[6];
	double v22[6];
    for(i=0;i<numOfpic;i++)
	{
		for(j=0;j<9;j++) h_temp[j]=H[i][j];
		
		mtxAt(h_temp,3,3,h);
		//计算v11,v12,v22
		v12[0]=h[0]*h[3];             v12[1]=h[0]*h[4]+h[1]*h[3];   v12[2]=h[1]*h[4];
		v12[3]=h[2]*h[3]+h[0]*h[5];   v12[4]=h[2]*h[4]+h[1]*h[5];   v12[5]=h[2]*h[5];
		
		v11[0]=h[0]*h[0];             v11[1]=h[0]*h[1]+h[1]*h[0];    v11[2]=h[1]*h[1];
		v11[3]=h[2]*h[0]+h[0]*h[2];   v11[4]=h[2]*h[1]+h[1]*h[2];    v11[5]=h[2]*h[2];
		
		v22[0]=h[3]*h[3];             v22[1]=h[3]*h[4]+h[4]*h[3];    v22[2]=h[4]*h[4];
		v22[3]=h[5]*h[3]+h[3]*h[5];   v22[4]=h[5]*h[4]+h[4]*h[5];    v22[5]=h[5]*h[5];
        
		for(j=0;j<6;j++)
		{
			v[i*12+j+0]=v12[j];
			v[i*12+j+6]=v11[j]-v22[j];
		}
	}
}


void zhang_2D_extrinsic(int numOfpic,double **H)
{
	int i,j;
	double A_inv[9];//A的逆
	double r1[3],r2[3],r3[3],t[3],h[3],RL[9];
	double s;
	double H_tr[9];


	double* U   =(double*) malloc(sizeof(double)*3*3); 
	double* D   =(double*) malloc(sizeof(double)*3);   
	double* V   =(double*) malloc(sizeof(double)*3*3); 
	double* tmp =(double*) malloc(sizeof(double)*3);
	
	entrinsic = (double**)malloc(numOfpic*sizeof(double*));
    
	for(i=0;i<numOfpic;i++)
		entrinsic[i] = (double*)malloc(12*sizeof(double));
	
	m3Inv(A_inside,A_inv);
	
	for(i=0;i<numOfpic;i++)
	{
        mtxAt(H[i],3,3,H_tr);

		h[0]=H_tr[0];          h[1]=H_tr[1];           h[2]=H_tr[2];
		mtxAb(A_inv,h,3,3,r1);
		
		h[0]=H_tr[3];          h[1]=H_tr[4];           h[2]=H_tr[5];
		mtxAb(A_inv,h,3,3,r2);
        
		h[0]=H_tr[6];          h[1]=H_tr[7];           h[2]=H_tr[8];
		mtxAb(A_inv,h,3,3,t);
		
		s = (1/sqrt(r1[0]*r1[0]+r1[1]*r1[1]+r1[2]*r1[2])+1/sqrt(r2[0]*r2[0]+r2[1]*r2[1]+r2[2]*r2[2]))/2;
		
		for(j=0;j<3;j++)
		{
			r1[j]=s*r1[j];
            r2[j]=s*r2[j];
			t[j]=s*t[j];
		}
        
		m3Cross(r1,r2,r3);

        for(j=0;j<3;j++)
		{
			RL[j*3+0]=r1[j];
			RL[j*3+1]=r2[j];
			RL[j*3+2]=r3[j];
		}

		mtxSVD(RL,3,3,U,D,V,tmp);
		
		mtxABt(U,V,3,3,3,RL);
		for(j=0;j<3;j++)
		{
			entrinsic[i][j*4+0]=RL[j*3+0];
			entrinsic[i][j*4+1]=RL[j*3+1];
			entrinsic[i][j*4+2]=RL[j*3+2];
			entrinsic[i][j*4+3]=t[j];
		}		
	}

	free(U);
	free(D);
	free(V);
	free(tmp);
	
}

void zhang_2D_k(int numOfpic,int nPoints,double *modelPoints,double *imagePoints)
{
	int i,j;
	double *model;
    double *RT;
	double *XY;
	double *UV;
	double *D;
	double *Dt;
	double Dt_mutil_D[4];
	double Dt_mutil_D_inv[4];
	double *d;

	RT = (double*)malloc(3*3*sizeof(double));
	
	XY = (double*)malloc(3*nPoints*sizeof(double));

	UV = (double*)malloc(3*nPoints*sizeof(double));

	D = (double*)malloc(2*2*numOfpic*nPoints*sizeof(double));

	Dt = (double*)malloc(2*2*numOfpic*nPoints*sizeof(double));
	
	d = (double*)malloc(2*numOfpic*nPoints*sizeof(double));
	
	model = (double*)malloc(nPoints*3*sizeof(double));
   
	for(j=0;j<numOfpic;j++)
	{
		for(i=0;i<3;i++)
		{
			RT[i*3+0]=entrinsic[j][i*4+0];
		    RT[i*3+1]=entrinsic[j][i*4+1];
		    RT[i*3+2]=entrinsic[j][i*4+3];
		}
    
	    for(i=0;i<nPoints;i++)
		{
			model[i+nPoints*0]=modelPoints[i*2+0];
		    model[i+nPoints*1]=modelPoints[i*2+1];
		    model[i+nPoints*2]=1;