tsai3d.c

这是著名的Tsai摄像机内外参数标定的源码

		b[i] = (cc->r7 * cd->xw[i] + cc->r8 * cd->yw[i] + cc->r9 * cd->zw[i]) * Yd[i];
	}
	mtxAx_b(A,cd->point_count,2,b,x);
	// update the calibration constants 
	cc->f = x[0];
	cc->Tz = x[1];
	cc->kappa1 = 0.0;		// this is the assumption that our calculation was made under 
	return 1;
}


/************************************************************************/

void tsai3D_compute_exact_f_and_Tz_error_cminpack(int  m_ptr,int n_ptr,double *params,
																					 double *err, int *erro)
{	// pointer to number of points to fit 
	// pointer to number of parameters 
	// vector of parameters 
	// vector of error from data 
	int       i;
	double    xc,yc,zc, Xu_1,Yu_1, Xu_2,Yu_2, distortion_factor,f,Tz,kappa1;
	double errAVG;

	f = params[0];
	Tz = params[1];
	kappa1 = params[2];

	errAVG = 0.0;

	for (i = 0; i < cd_temp->point_count; i++) {
		// convert from world coordinates to camera coordinates 
		xc = cc_temp->r1 * cd_temp->xw[i] + cc_temp->r2 * cd_temp->yw[i] + cc_temp->r3 * cd_temp->zw[i] + cc_temp->Tx;
		yc = cc_temp->r4 * cd_temp->xw[i] + cc_temp->r5 * cd_temp->yw[i] + cc_temp->r6 * cd_temp->zw[i] + cc_temp->Ty;
		zc = cc_temp->r7 * cd_temp->xw[i] + cc_temp->r8 * cd_temp->yw[i] + cc_temp->r9 * cd_temp->zw[i] + Tz;

		// convert from camera coordinates to undistorted sensor coordinates 
		Xu_1 = f * xc / zc;
		Yu_1 = f * yc / zc;

		// convert from distorted sensor coordinates to undistorted sensor coordinates 
		distortion_factor = 1 + kappa1 * r_squared[i];
		Xu_2 = Xd[i] * distortion_factor;
		Yu_2 = Yd[i] * distortion_factor;

		// record the error in the undistorted sensor coordinates 
		err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
		errAVG+=err[i];
	};
	if(DEBUG)	
		printf("\nErro medio= %lf",errAVG/cd_temp->point_count);
}


void tsai3D_compute_exact_f_and_Tz_cminpack ()
{
	#define NPARAMS 3

	// Parameters needed by simplified "C" version of Levenberg-Marquardt method 

	int     m = cd_temp->point_count;
	int     n = NPARAMS;
	double  *x;    //	x[NPARAMS];
	int     *msk;  // msk[NPARAMS];
	double  *fvec; // fvec[m];
	double  tol = 1.0e-10;
	int     info;
	int     nfev;

	// allocate some workspace 
	if ( (x = (double*)malloc(NPARAMS * sizeof(double))) == NULL ) 
	{
		printf("malloc: Cannot allocate workspace x\n");
		return;
	}

	if ( (msk = (int*)malloc(NPARAMS * sizeof(int))) == NULL ) 
	{
		printf("malloc: Cannot allocate workspace msk\n");
		return;
	}

	if ( (fvec = (double*)malloc(m * sizeof(double))) == NULL ) 
	{
		printf("malloc: Cannot allocate workspace fvec\n");
		return;
	}

	// use the current calibration constants as an initial guess 
	x[0] = cc_temp->f;
	x[1] = cc_temp->Tz;
	x[2] = cc_temp->kappa1;

	// Here we call a function Levenberg-Marquardt Nonlinear optimization in "C" 
	// minpack simplified version
	if(lmdif0(tsai3D_compute_exact_f_and_Tz_error_cminpack,m,n,x,msk,fvec,tol,&info,&nfev)){
			printf("\nParameters error lmdif process \n");
			
	}
	else{
		printf("\nParameters successful lmdif process \n");
		cc_temp->f = x[0];
		cc_temp->Tz = x[1];
		cc_temp->kappa1 = x[2];
	}
	
	#undef NPARAMS
	return;
}


/*********************************************************************/
void  tsai3D_define_camera_parameters(int CameraModel, 
																			double Ncx,	double Nfx,
																			double dx,   double dy,
																			double Cx,	double Cy, double sx,
																			struct tsai_camera_parameters *cp){

	switch(CameraModel) {
	// - cameras
	case PHOTOMETRICS_STAR_I:
		cp->Ncx = 576;		/* [sel]        */
		cp->Nfx = 576;		/* [pix]        */
		cp->dx = 0.023;		/* [mm/sel]     */
		cp->dy = 0.023;		/* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;	/* [mm/pix]     */
		cp->dpy = cp->dy;		/* [mm/pix]     */
		/* something a bit more realistic */
		cp->Cx = 258.0;
		cp->Cy = 204.0;
		cp->sx = 1.0;		/* []		 */
	  break;
	
	case GENERAL_IMAGING_MOS5300:
		cp->Ncx = 649;		/* [sel]        */
		cp->Nfx = 512;		/* [pix]        */
		cp->dx = 0.015;		/* [mm/sel]     */
		cp->dy = 0.015;		/* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;	/* [mm/pix]     */
		cp->dpy = cp->dy;		/* [mm/pix]     */
		cp->Cx = 512 / 2;		/* [pix]        */
		cp->Cy = 480 / 2;		/* [pix]        */
		cp->sx = 1.0;		/* []		 */
		break;

	case PANASONIC_GP_MF702:
		cp->Ncx = 649;		/* [sel]        */
		cp->Nfx = 512;		/* [pix]        */
		cp->dx = 0.015;		/* [mm/sel]     */
		cp->dy = 0.015;		/* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;	/* [mm/pix]     */
		cp->dpy = cp->dy;		/* [mm/pix]     */
		cp->Cx = 268;		/* [pix]        */
		cp->Cy = 248;		/* [pix]        */
		cp->sx = 1.078647;		/* []           */
		break;

	case SONY_XC75:
		cp->Ncx = 768;		/* [sel]        */
		cp->Nfx = 512;		/* [pix]        */
		cp->dx = 0.0084;		/* [mm/sel]     */
		cp->dy = 0.0098;		/* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;	/* [mm/pix]     */
		cp->dpy = cp->dy;		/* [mm/pix]     */
		cp->Cx = 512 / 2;		/* [pix]        */
		cp->Cy = 480 / 2;		/* [pix]        */
		cp->sx = 1.0;		/* []           */
		break;

	case SONY_XC77:
		cp->Ncx = 768;		/* [sel]        */
		cp->Nfx = 512;		/* [pix]        */
		cp->dx = 0.011;		/* [mm/sel]     */
		cp->dy = 0.013;		/* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;	/* [mm/pix]     */
		cp->dpy = cp->dy;		/* [mm/pix]     */
		cp->Cx = 512 / 2;		/* [pix]        */
		cp->Cy = 480 / 2;		/* [pix]        */
		cp->sx = 1.0;		/* []           */
		break;

	case SONY_XC57: 
		cp->Ncx = 510;               /* [sel]        */
		cp->Nfx = 512;               /* [pix]        */
		cp->dx = 0.017;              /* [mm/sel]     */
		cp->dy = 0.013;              /* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;   /* [mm/pix]     */
		cp->dpy = cp->dy;             /* [mm/pix]     */
		cp->Cx = 512 / 2;            /* [pix]        */
		cp->Cy = 480 / 2;            /* [pix]        */
		cp->sx = 1.107914;           /* []           */
		break;
	// - webcams
	case LOGITECH_QUICKCAM_PRO_4000: 
		cp->Ncx = 659;               /* [sel]        */
		cp->Nfx = 640;               /* [pix]        */
		cp->dx = 0.0056;              /* [mm/sel]     */
		cp->dy = 0.0056;              /* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;   /* [mm/pix]     */
		cp->dpy = cp->dy;             /* [mm/pix]     */
		cp->Cx = 640 / 2;            /* [pix]        */
		cp->Cy = 480 / 2;            /* [pix]        */
		cp->sx = 1.0;           /* []           */
		break;
	
	case CREATIVE_WEBCAM_NX_PRO: 
		cp->Ncx = 642;               /* [sel]        */
		cp->Nfx = 640;               /* [pix]        */
		cp->dx = 0.008;              /* [mm/sel]     */
		cp->dy = 0.008;              /* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;   /* [mm/pix]     */
		cp->dpy = cp->dy;             /* [mm/pix]     */
		cp->Cx = 640 / 2;            /* [pix]        */
		cp->Cy = 480 / 2;            /* [pix]        */
		cp->sx = 1.0;           /* []           */
		break;
	// - Default camera 
	case PINHOLE_CAMERA: 
		cp->Ncx = Cx*2.0;               /* [sel]        */
		cp->Nfx = Cx*2.0;               /* [pix]        */
		cp->dx = 1.0;              /* [mm/sel]     */
		cp->dy = 1.0;              /* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;   /* [mm/pix]     */
		cp->dpy = cp->dy;             /* [mm/pix]     */
		cp->Cx = Cx;            /* [pix]        */
		cp->Cy = Cy;            /* [pix]        */
		cp->sx = 1.0;           /* []           */
		break;
	case NEW_CAMERA: 
		cp->Ncx = Ncx;               /* [sel]        */
		cp->Nfx = Nfx;               /* [pix]        */
		cp->dx = dx;              /* [mm/sel]     */
		cp->dy = dy;              /* [mm/sel]     */
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;   /* [mm/pix]     */
		cp->dpy = cp->dy;             /* [mm/pix]     */
		cp->Cx = Cx;            /* [pix]        */
		cp->Cy = Cy;            /* [pix]        */
		cp->sx = 1.0;           /* []           */
		break;

	default:
		printf("\nInvalid camera selection");
	}

}

/* Function to get calibration Tsai process */
int   tsai3D_calibration (	int numberPoints, double* modelPoints, double* imagePoints,
														double *A, double *K, double *distortion, 
														struct tsai_camera_parameters *cp,
														struct tsai_calibration_constants *cc)
{
	int  i;
	double *U;
	// External variables
	U = 0; Xd = 0; 	Yd = 0;	r_squared = 0;
	
	// Linked reference of tsai_object to internal pointer
	cp_temp = cp;
	cd_temp = (struct tsai_calibration_data*)malloc(sizeof(struct tsai_calibration_data)*1);
	tsai3D_init_cd(cd_temp);
	tsai3D_allocate_Memory_cd(cd_temp, numberPoints);

	cc_temp = cc;

	for(i=0;i<numberPoints;i++) {
		cd_temp->Xf[i] = imagePoints[2*i + 0];
		cd_temp->Yf[i] = imagePoints[2*i + 1];
		cd_temp->xw[i] = modelPoints[3*i + 0];
		cd_temp->yw[i] = modelPoints[3*i + 1];
		cd_temp->zw[i] = modelPoints[3*i + 2];
	};
	cd_temp->point_count = numberPoints;

	// Init process of Tsai Non-coplanar calibration if have 
	// pattern points captured for calibration camera.
	if(numberPoints > 0 )
	{
		Xd				= (double*)malloc(sizeof(double)*numberPoints); 
		Yd				= (double*)malloc(sizeof(double)*numberPoints);
		r_squared = (double*)malloc(sizeof(double)*numberPoints);
		U					= (double*)malloc(sizeof(double)*7);
		tsai3D_compute_Xd_Yd_and_r_squared(cd_temp,cp_temp,Xd,Yd,r_squared);
		if(tsai3D_compute_U(cd_temp,Xd,Yd,U))
		{
			tsai3D_compute_Tx_and_Ty(cd_temp,cc_temp,Xd,Yd,r_squared,U);
			tsai3D_compute_sx(cp_temp,cc_temp,U);
			tsai3D_compute_Xd_Yd_and_r_squared(cd_temp,cp_temp,Xd,Yd,r_squared);
			tsai3D_compute_better_R(cp_temp, cc_temp, U);
			tsai3D_compute_approximate_f_and_Tz(cd_temp,cc_temp,Yd);
			if (cc_temp->f < 0.f) 
			{
				/* try the other solution for the orthonormal matrix */
				cc_temp->r3 = -cc_temp->r3;
				cc_temp->r6 = -cc_temp->r6;
				cc_temp->r7 = -cc_temp->r7;
				cc_temp->r8 = -cc_temp->r8;
				solve_RPY_transform(cc_temp);
				tsai3D_compute_approximate_f_and_Tz(cd_temp,cc_temp,Yd);
				if (cc_temp->f < 0.f){
					printf ("\nError - possible handedness problem with data\n");
					tsai3D_free_Memory_cd(cd_temp);
					free(cd_temp);
					free(U); free(Xd); free(Yd); free(r_squared);
					return 0;
				};
			};		
			tsai3D_compute_exact_f_and_Tz_cminpack();
			printf ("\n-- Calibration Tsai Non-coplanar process successful --\n");
			tsai3D_free_Memory_cd(cd_temp);
			free(cd_temp);
			free(U); free(Xd); free(Yd); free(r_squared);
			A[0] = cc_temp->f;	A[1] = 0.0;					A[2] = cp_temp->Cx;
			A[3] = 0.0;					A[4] = cc_temp->f;	A[5] = cp_temp->Cy;
			A[6] = 0.0;					A[7] = 0.0;					A[8] = 1.0;

			K[0] = cc_temp->r1;	K[1] = cc_temp->r2;	K[2]  = cc_temp->r3; K[3]  = cc_temp->Tx; 
			K[4] = cc_temp->r4;	K[5] = cc_temp->r5;	K[6]  = cc_temp->r6; K[7]  = cc_temp->Ty; 
			K[8] = cc_temp->r7;	K[9] = cc_temp->r8;	K[10] = cc_temp->r9; K[11] = cc_temp->Tz; 
			
			*distortion = cc_temp->kappa1;	

			return 1;
		}
		else{
			tsai3D_free_Memory_cd(cd_temp);
			free(cd_temp);
			free(U); free(Xd); free(Yd); free(r_squared);
			return 0;
		};
	}
	else{
		tsai3D_free_Memory_cd(cd_temp);
		free(cd_temp);
		return 0;
	};
}