tsai3d.c

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

/**************************
* Calibration	TSAI3D		  *
*													*
* Marcelo Gattass					*
* Manuel E. L. Fernandez	*
* Jul06,2006							*
**************************/
#include "tsai3D.h"

#define DEBUG 0
/***********************************************************************\
* Auxiliar global pointers																							*
* used only for internal calculations																		* 
\***********************************************************************/
struct tsai_calibration_data *cd_temp; 
struct tsai_calibration_constants *cc_temp;
struct tsai_camera_parameters *cp_temp;
double *Xd,*Yd,*r_squared;
/***********************************************************************\
* Functions to allocate memory space to calibration variables						*
\***********************************************************************/
void tsai3D_init_cp(struct tsai_camera_parameters *cp) {
	cp->Ncx = 0.0;		/* [sel] Number of sensor elements in camera's x direction */
	cp->Nfx = 0.0;		/* [pix] Number of pixels in frame grabber's x direction  */
	cp->dx  = 0.0;		/* [mm/sel] X dimension of camera's sensor element (in mm) */
	cp->dy  = 0.0;		/* [mm/sel] Y dimension of camera's sensor element (in mm) */
	cp->dpx = 0.0;		/* [mm/pix] Effective X dimension of pixel in frame grabber  */
	cp->dpy = 0.0;		/* [mm/pix] Effective Y dimension of pixel in frame grabber  */
	cp->Cx  = 0.0;		/* [pix] Z axis intercept of camera coordinate system  */
	cp->Cy  = 0.0;		/* [pix] Z axis intercept of camera coordinate system  */
	cp->sx  = 0.0;		/* [] Scale factor to compensate for any error in dpx  */

	cp->is_load_paremeters = 0; /*  flag to indicate that all parameters have been loaded */
}

void tsai3D_set_cp(struct tsai_camera_parameters *cp) {
	if(cp->is_load_paremeters == 1){
		cp->dpx = cp->dx * cp->Ncx / cp->Nfx;		/* [mm/pix] Effective X dimension of pixel in frame grabber  */
		cp->dpy = cp->dy;												/* [mm/pix] Effective Y dimension of pixel in frame grabber  */
		cp->sx  = 1.0;													/* [] Scale factor to compensate for any error in dpx  */		
	};
}

void tsai3D_init_cd(struct tsai_calibration_data* cd){
	cd->point_count = 0;	/* [points] 	 */
	cd->xw = 0;	/* [mm]          */
	cd->yw = 0;	/* [mm]          */
	cd->zw = 0;	/* [mm]          */
	cd->Xf = 0;	/* [pix]         */
	cd->Yf = 0;	/* [pix]         */
}

void tsai3D_free_Memory_cd(struct tsai_calibration_data* cd){
	if(cd->point_count != 0){
		free(cd->xw);
		free(cd->yw);
		free(cd->zw);
		free(cd->Xf);
		free(cd->Yf);
		cd->point_count =0;
	};
}

int tsai3D_allocate_Memory_cd(struct tsai_calibration_data* cd , int set_nro_points)
{
	if(cd->point_count != 0){
		free(cd->xw);
		free(cd->yw);
		free(cd->zw);
		free(cd->Xf);
		free(cd->Yf);
	};
	cd->xw = (double*)malloc(sizeof(double)*set_nro_points);
	cd->yw = (double*)malloc(sizeof(double)*set_nro_points);
	cd->zw = (double*)malloc(sizeof(double)*set_nro_points);
	cd->Xf = (double*)malloc(sizeof(double)*set_nro_points);
	cd->Yf = (double*)malloc(sizeof(double)*set_nro_points);
	if(cd->xw && cd->yw && cd->zw && cd->Xf && cd->Yf){
		cd->point_count = set_nro_points;
		return 1;
	}
	else{
		return 0;
	};
}

void tsai3D_init_cc(struct tsai_calibration_constants* cc ){
	cc->f = 0.0;		/* [mm]          */
	cc->kappa1 = 0.0;		/* [1/mm^2]      */
	cc->p1 = 0.0;		/* [1/mm]        */
	cc->p2= 0.0;		/* [1/mm]        */
	cc->Tx= 0.0;		/* [mm]          */
	cc->Ty= 0.0;		/* [mm]          */
	cc->Tz= 0.0;		/* [mm]          */
	cc->Rx= 0.0;		/* [rad]	 */
	cc->Ry= 0.0;		/* [rad]	 */
	cc->Rz= 0.0;		/* [rad]	 */
	cc->r1= 0.0;		/* []            */
	cc->r2= 0.0;		/* []            */
	cc->r3= 0.0;		/* []            */
	cc->r4= 0.0;		/* []            */
	cc->r5= 0.0;		/* []            */
	cc->r6= 0.0;		/* []            */
	cc->r7= 0.0;		/* []            */
	cc->r8= 0.0;		/* []            */
	cc->r9= 0.0;		/* []            */

	cc->is_load_constants = 0; /*      flag to indicate that all parameters have been loaded  */
}

/***********************************************************************\
* Math routines for Non-coplanar camera calibration	 													*
\***********************************************************************/
void SINCOSf(double x, double* s, double* c)
{
	*s = (double)sin(x); *c = (double)cos(x);
};
double SQRf(double x){ 
	return (x*x);
}

int SIGNBITf(double x){ 
	if(x > 0.f) 
		return 0; 
	else 
		return 1;
}
/***********************************************************************\
* This routine solves for the roll, pitch and yaw angles (in radians)	*
* for a given orthonormal rotation matrix (from Richard P. Paul,        *
* Robot Manipulators: Mathematics, Programming and Control, p70).       *
* Note 1, should the rotation matrix not be orthonormal these will not  *
* be the "best fit" roll, pitch and yaw angles.                         *
* Note 2, there are actually two possible solutions for the matrix.     *
* The second solution can be found by adding 180 degrees to Rz before   *
* Ry and Rx are calculated.                                             *
\***********************************************************************/
void solve_RPY_transform (struct tsai_calibration_constants *cc)
{
	double    sg,cg;
	sg = 0.0; cg = 0.0;
	cc->Rz = (double)atan2 (cc->r4, cc->r1);
	SINCOSf(cc->Rz, &sg, &cg);
	cc->Ry = (double)atan2 (-cc->r7, cc->r1 * cg + cc->r4 * sg);
	cc->Rx = (double)atan2 (cc->r3 * sg - cc->r6 * cg, cc->r5 * cg - cc->r2 * sg);
}

/***********************************************************************\
* Functions to implement Tsai Non-Coplanar calibration									*
\***********************************************************************/
void tsai3D_compute_Xd_Yd_and_r_squared (struct tsai_calibration_data *cd,
																					struct tsai_camera_parameters *cp, 
																					double *Xd, double *Yd, double *r_squared)
{
	int    i;
	double  Xd_,Yd_;
	for(i=0; i<cd->point_count; i++){
		Xd[i] = Xd_ = cp->dpx * (cd->Xf[i] - cp->Cx) / cp->sx;	// [mm] 
		Yd[i] = Yd_ = cp->dpy * (cd->Yf[i] - cp->Cy);						// [mm] 
		r_squared[i] = SQRf(Xd_) + SQRf(Yd_);										// [mm^2] 
	}
}

int  tsai3D_compute_U (struct tsai_calibration_data* cd, double *Xd, double *Yd, double *U)
{
	int      i,line;
	double   *A,*x,*b;
	
	A = (double*)malloc(sizeof(double)*(cd->point_count*7)); 
	x =  (double*)malloc(sizeof(double)*7); 
	b =  (double*)malloc(sizeof(double)*cd->point_count); 
	
	for (i = 0; i < cd->point_count; i++) {
		line = i*7;
		A[line + 0] = Yd[i] * cd->xw[i];
		A[line + 1] = Yd[i] * cd->yw[i];
		A[line + 2] = Yd[i] * cd->zw[i];
		A[line + 3] = Yd[i];
		A[line + 4] = -Xd[i] * cd->xw[i];
		A[line + 5] = -Xd[i] * cd->yw[i];
		A[line + 6] = -Xd[i] * cd->zw[i];
		b[i] = Xd[i];
	}
	mtxAx_b(A,cd->point_count,7,b,x);
	
	U[0] = x[0];
	U[1] = x[1];
	U[2] = x[2];
	U[3] = x[3];
	U[4] = x[4];
	U[5] = x[5];
	U[6] = x[6];

	return 1;
}

void tsai3D_compute_Tx_and_Ty (	struct tsai_calibration_data *cd,
																struct tsai_calibration_constants *cc,
																double *Xd, double *Yd, double *r_squared, double *U)
{
	int      i,far_point;
	double    Tx,Ty,Ty_squared,x,y,r1,r2,r3,r4,r5,r6,distance,far_distance;

	// first find the square of the magnitude of Ty 
	Ty_squared = 1.0 / (SQRf(U[4]) + SQRf(U[5]) + SQRf(U[6]));

	// find a point that is far from the image center 
	far_distance = 0;
	far_point = 0;
	for (i = 0; i < cd->point_count; i++){
		if ((distance = r_squared[i]) > far_distance) {
			far_point = i;
			far_distance = distance;
		}
	}

	// now find the sign for Ty 
	// start by assuming Ty > 0 
	Ty = (double)sqrt(Ty_squared);
	r1 = U[0] * Ty;
	r2 = U[1] * Ty;
	r3 = U[2] * Ty;
	Tx = U[3] * Ty;
	r4 = U[4] * Ty;
	r5 = U[5] * Ty;
	r6 = U[6] * Ty;
	x = r1 * cd->xw[far_point] + r2 * cd->yw[far_point] + r3 * cd->zw[far_point] + Tx;
	y = r4 * cd->xw[far_point] + r5 * cd->yw[far_point] + r6 * cd->zw[far_point] + Ty;

	// flip Ty if we guessed wrong 
	if ((SIGNBITf (x) != SIGNBITf (Xd[far_point])) ||
			(SIGNBITf (y) != SIGNBITf (Yd[far_point])))
		Ty = -Ty;

	// update the calibration constants 
	cc->Tx = U[3] * Ty;
	cc->Ty = Ty;
}

void tsai3D_compute_sx (	struct tsai_camera_parameters *cp,
												struct tsai_calibration_constants *cc,
												double *U)
{
	cp->sx = (double)sqrt(SQRf (U[0]) + SQRf (U[1]) + SQRf (U[2])) * (double)fabs (cc->Ty);	
}

void tsai3D_compute_R (	struct tsai_camera_parameters* cp,
												struct tsai_calibration_constants *cc, 
												double *U)
{
	double  r1,r2,r3,r4,r5,r6,r7,r8,r9;

	r1 = U[0] * cc->Ty / cp->sx;
	r2 = U[1] * cc->Ty / cp->sx;
	r3 = U[2] * cc->Ty / cp->sx;

	r4 = U[4] * cc->Ty;
	r5 = U[5] * cc->Ty;
	r6 = U[6] * cc->Ty;

	// use the outer product of the first two rows to get the last row 
	r7 = r2 * r6 - r3 * r5;
	r8 = r3 * r4 - r1 * r6;
	r9 = r1 * r5 - r2 * r4;

	// update the calibration constants 
	cc->r1 = r1;
	cc->r2 = r2;
	cc->r3 = r3;
	cc->r4 = r4;
	cc->r5 = r5;
	cc->r6 = r6;
	cc->r7 = r7;
	cc->r8 = r8;
	cc->r9 = r9;

	// fill in cc.Rx, cc.Ry and cc.Rz 
	solve_RPY_transform (cc);
}


void tsai3D_compute_better_R (	struct tsai_camera_parameters* cp,
															struct tsai_calibration_constants *cc, 
															double *U)
{
	double    r1,r2,r3,r4,r5,r6,r7,sa,ca,sb,cb,sg,cg;

	r1 = U[0] * cc->Ty / cp->sx;
	r2 = U[1] * cc->Ty / cp->sx;
	r3 = U[2] * cc->Ty / cp->sx;

	r4 = U[4] * cc->Ty;
	r5 = U[5] * cc->Ty;
	r6 = U[6] * cc->Ty;

	// use the outer product of the first two rows to get the last row 
	r7 = r2 * r6 - r3 * r5;

	// now find the RPY angles corresponding to the estimated rotation matrix 
	cc->Rz = (double)atan2 (r4, r1);

	SINCOSf(cc->Rz, &sg, &cg);

	cc->Ry = (double)atan2 (-r7, r1 * cg + r4 * sg);

	cc->Rx = (double)atan2 (r3 * sg - r6 * cg, r5 * cg - r2 * sg);

	SINCOSf(cc->Rx, &sa, &ca);

	SINCOSf(cc->Ry, &sb, &cb);

	// now generate a more orthonormal rotation matrix from the RPY angles 
	cc->r1 = cb * cg;
	cc->r2 = cg * sa * sb - ca * sg;
	cc->r3 = sa * sg + ca * cg * sb;
	cc->r4 = cb * sg;
	cc->r5 = sa * sb * sg + ca * cg;
	cc->r6 = ca * sb * sg - cg * sa;
	cc->r7 = -sb;
	cc->r8 = cb * sa;
	cc->r9 = ca * cb;
}

int  tsai3D_compute_approximate_f_and_Tz (	struct tsai_calibration_data *cd,
																					struct tsai_calibration_constants *cc, 
																					double *Yd)
{
	int			i,line;
	double	*A,*x,*b;
	
	A = (double*)malloc(sizeof(double)*(cd->point_count*2)); 
	x = (double*)malloc(sizeof(double)*2); 
	b = (double*)malloc(sizeof(double)*cd->point_count); 

	for (i = 0; i < cd->point_count; i++) {
		line = i*2;
		A[line + 0] = cc->r4 * cd->xw[i] + cc->r5 * cd->yw[i] + cc->r6 * cd->zw[i] + cc->Ty;
		A[line + 1] = -Yd[i];