/*******************************************************************************\ * * * This file contains utility routines for dumping, loading and printing * * the data structures used by the routines contained in the file * * camera_calibration.c. * * * * History * * ------- * * * * 01-May-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * * Original implementation. * * * * * \*******************************************************************************/ /*******************************************************************************\ * * * * * f - effective focal length of the pin hole camera * * kappa1 - 1st order radial lens distortion coefficient * * Cx, Cy - coordinates of center of radial lens distortion * * (also used as the piercing point of the camera coordinate * * frame's Z axis with the camera's sensor plane) * * sx - uncertainty factor for scale of horizontal scanline * * * * and 6 external (also called extrinsic or exterior) camera parameters: * * * * Rx, Ry, Rz, Tx, Ty, Tz - rotational and translational components of * * the transform between the world's coordinate * * frame and the camera's coordinate frame. * * * * Data for model calibration consists of the (x,y,z) world coordinates of a * * feature point (in mm) and the corresponding coordinates (Xf,Yf) (in pixels) * * of the feature point in the image. * * * world_coord_to_image_coord () * * image_coord_to_world_coord () * * distorted_to_undistorted_image_coord () * * undistorted_to_distorted_image_coord () * * * * External routines * * ----------------- * * * * Nonlinear optimization for these camera calibration routines is performed * * by 2 external IMSL Fortran routines: * * * * dunlsf - double precision, nonlinear least squares using a finite * * difference Jacobian. * * * * dumiah - double precision, multivariate function using a modified * * newton method and a user-supplied Hessian. * * * * These may not be the best algorithms for the problem but they seem to work * * reasonably well. * * * * Matrix operations (inversions, multiplications, etc.) are also provided by * * external routines. * * * * * * Extra notes * * ----------- * * * * An explanation of the basic algorithms and description of the variables * * can be found in "An Efficient and Accurate Camera Calibration Technique for * * 3D Machine Vision", Roger Y. Tsai, Proceedings of IEEE Conference on Computer * * Vision and Pattern Recognition, 1986, pages 364-374. This work also appears * * in several other publications under Roger Tsai's name. * * * * * * Notation * * -------- * * * The camera's X axis runs along increasing column coordinates in the * * image/frame. The Y axis runs along increasing row coordinates. * * * * pix == image/frame grabber picture element * * sel == camera sensor element * * * * Internal routines starting with "cc_" are for coplanar calibration. * * * * * * History * * ------- * * * * 04-Jul-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * * Added new routines to evaluate the accuracy of camera calibration. * * * * 01-May-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * * For efficiency the non-linear optimizations are now all based on * * the minimization of squared error in undistorted image coordinates * * instead of the squared error in distorted image coordinates. * * * * New routine for inverse perspective projection. * * * * 14-Feb-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * * Bug fixes and speed ups. * * * * 07-Feb-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * * Original implementation. * * * \*******************************************************************************/ #include #include #include #include "coords.h" #define SQR(a) ((a)*(a)) /* Variables used by the subroutines for I/O (perhaps not the best way of doing this) */ struct camera_parameters cp; struct calibration_constants cc; /* Local working storage */ double Xu_global, Yu_global, Xd[MAX_POINTS], Yd[MAX_POINTS]; /************************************************************************/ void undistorted_to_distorted_sensor_coords_error (n_ptr, params, err) int *n_ptr; /* pointer to number of parameters */ double *params; /* vector of parameters */ double *err; /* vector of error from data */ { double Xd, Yd, distortion_factor; Xd = params[0]; Yd = params[1]; distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd)); *err = SQR (Xu_global - Xd * distortion_factor) + SQR (Yu_global - Yd * distortion_factor); } void undistorted_to_distorted_sensor_coords_gradient (n_ptr, params, gradient) int *n_ptr; /* pointer to number of parameters */ double *params; /* vector of parameters */ double *gradient; /* vector of generated gradients for given point */ { double Xd, Yd, kappa1; Xd = params[0]; Yd = params[1]; kappa1 = cc.kappa1; /* these expressions were generated using Macsyma. A good optimizing compiler */ /* should be able to make them execute a lot more efficiently than they are */ /* laid out here. */ gradient[0] = -4 * kappa1 * Xd * Yd * Yu_global + 6 * kappa1 * kappa1 * Xd * Yd * Yd * Yd * Yd + (-2 * kappa1 * Xu_global + 12 * kappa1 * kappa1 * Xd * Xd * Xd + 8 * kappa1 * Xd) * Yd * Yd + (-6 * kappa1 * Xd * Xd - 2) * Xu_global + 6 * kappa1 * kappa1 * Xd * Xd * Xd * Xd * Xd + 8 * kappa1 * Xd * Xd * Xd + 2 * Xd; gradient[1] = (-6 * kappa1 * Yd * Yd - 2 * kappa1 * Xd * Xd - 2) * Yu_global + 6 * kappa1 * kappa1 * Yd * Yd * Yd * Yd * Yd + (12 * kappa1 * kappa1 * Xd * Xd + 8 * kappa1) * Yd * Yd * Yd + (-4 * kappa1 * Xd * Xu_global + 6 * kappa1 * kappa1 * Xd * Xd * Xd * Xd + 8 * kappa1 * Xd * Xd + 2) * Yd; } void undistorted_to_distorted_sensor_coords_hessian (n_ptr, params, hessian, ldh_ptr) int *n_ptr; /* pointer to number of parameters */ double *params; /* vector of parameters */ double *hessian; /* generated hessian matrix */ int *ldh_ptr; /* pointer to leading dimension of hessian matrix */ { double Xd, Yd, kappa1; Xd = params[0]; Yd = params[1]; kappa1 = cc.kappa1; hessian[0] = -4 * kappa1 * Yd * Yu_global + 6 * kappa1 * kappa1 * Yd * Yd * Yd * Yd + (36 * kappa1 * kappa1 * Xd * Xd + 8 * kappa1) * Yd * Yd - 12 * kappa1 * Xd * Xu_global + 30 * kappa1 * kappa1 * Xd * Xd * Xd * Xd + 24 * kappa1 * Xd * Xd + 2; hessian[1] = hessian[2] = -4 * kappa1 * Xd * Yu_global + 24 * kappa1 * kappa1 * Xd * Yd * Yd * Yd + (-4 * kappa1 * Xu_global + 24 * kappa1 * kappa1 * Xd * Xd * Xd + 16 * kappa1 * Xd) * Yd; hessian[3] = -12 * kappa1 * Yd * Yu_global + 30 * kappa1 * kappa1 * Yd * Yd * Yd * Yd + (36 * kappa1 * kappa1 * Xd * Xd + 24 * kappa1) * Yd * Yd - 4 * kappa1 * Xd * Xu_global + 6 * kappa1 * kappa1 * Xd * Xd * Xd * Xd + 8 * kappa1 * Xd * Xd + 2; } void undistorted_to_distorted_sensor_coords (Xu, Yu, Xd, Yd) double Xu, Yu, *Xd, *Yd; { #define NPARAMS 2 int nparams; /* number of parameters */ double xguess[NPARAMS]; /* guess at parameters */ double xscale[NPARAMS]; /* parameter scaling */ double fscale; /* estimate of final function reduction */ int iparams[7]; /* integer control params */ double rparams[7]; /* real control params */ double xparams[NPARAMS]; /* final solution */ double fvalue; /* final function value */ int iersvr; /* error severity */ int ipact; /* print action */ int isact; /* stop action */ /* use the undistorted coordinates Xu and Yu as an initial guess for Xd and Yd */ xguess[0] = Xu_global = Xu; xguess[1] = Yu_global = Yu; /* set some variables for IMSL */ nparams = NPARAMS; xscale[0] = 1.0; xscale[1] = 1.0; fscale = 1.0; /* use the default running parameters for this routine */ iparams[0] = 0; /* stop IMSL from stopping or printing when it wants to warn us */ iersvr = 3; /* severity = warnings */ ipact = 0; /* printing = disabled */ isact = 0; /* stopping = disabled */ /* erset_ (&iersvr, &ipact, &isact); */ /* find the optimized values for the distorted sensor coordinates Xd and Yd */ dumiah_ (undistorted_to_distorted_sensor_coords_error, undistorted_to_distorted_sensor_coords_gradient, undistorted_to_distorted_sensor_coords_hessian, &nparams, xguess, xscale, &fscale, iparams, rparams, xparams, &fvalue); *Xd = xparams[0]; *Yd = xparams[1]; #undef NPARAMS } void distorted_to_undistorted_sensor_coords (Xd, Yd, Xu, Yu) double Xd, Yd, *Xu, *Yu; { double distortion_factor; /* convert from distorted to undistorted sensor plane coordinates */ distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd)); *Xu = Xd * distortion_factor; *Yu = Yd * distortion_factor; } /************************************************************************/ void undistorted_to_distorted_image_coords (Xfu, Yfu, Xfd, Yfd) double Xfu, Yfu, *Xfd, *Yfd; { double Xu, Yu, Xd, Yd; /* convert from image to sensor coordinates */ Xu = cp.dpx * (Xfu - cp.Cx) / cp.sx; Yu = cp.dpy * (Yfu - cp.Cy); /* convert from undistorted sensor to distorted sensor plane coordinates */ undistorted_to_distorted_sensor_coords (Xu, Yu, &Xd, &Yd); /* convert from sensor to image coordinates */ *Xfd = Xd * cp.sx / cp.dpx + cp.Cx; *Yfd = Yd / cp.dpy + cp.Cy; } void distorted_to_undistorted_image_coords (Xfd, Yfd, Xfu, Yfu) double Xfd, Yfd, *Xfu, *Yfu; { double distortion_factor, Xd, Yd, Xu, Yu; /* convert from image to sensor coordinates */ Xd = cp.dpx * (Xfd - cp.Cx) / cp.sx; Yd = cp.dpy * (Yfd - cp.Cy); /* convert from distorted sensor to undistorted sensor plane coordinates */ distorted_to_undistorted_sensor_coords (Xd, Yd, &Xu, &Yu); /* convert from sensor to image coordinates */ *Xfu = Xu * cp.sx / cp.dpx + cp.Cx; *Yfu = Yu / cp.dpy + cp.Cy; } /***********************************************************************\ * This routine takes the position of a point in world coordinates [mm] * * and determines the position of its image in image coordinates [pix]. * \***********************************************************************/ void world_coord_to_image_coord (xw, yw, zw, Xf, Yf) double xw, yw, zw, *Xf, *Yf; { double xc, yc, zc, Xu, Yu, Xd, Yd; /* convert from world coordinates to camera coordinates */ xc = cc.r1 * xw + cc.r2 * yw + cc.r3 * zw + cc.Tx; yc = cc.r4 * xw + cc.r5 * yw + cc.r6 * zw + cc.Ty; zc = cc.r7 * xw + cc.r8 * yw + cc.r9 * zw + cc.Tz; /* convert from camera coordinates to undistorted sensor plane coordinates */ Xu = cc.f * xc / zc; Yu = cc.f * yc / zc; /* convert from undistorted to distorted sensor plane coordinates */ undistorted_to_distorted_sensor_coords (Xu, Yu, &Xd, &Yd); /* convert from distorted sensor plane coordinates to image coordinates */ *Xf = Xd * cp.sx / cp.dpx + cp.Cx; *Yf = Yd / cp.dpy + cp.Cy; } /***********************************************************************\ * This routine performs an inverse perspective projection to determine * * the position of a point in world coordinates that corresponds to a * * given position in image coordinates. To constrain the inverse * * projection to a single point the routine requires a Z world * * coordinate for the point in addition to the X and Y image coordinates.* \***********************************************************************/ void image_coord_to_world_coord (Xfd, Yfd, zw, xw, yw) double Xfd, Yfd, zw, *xw, *yw; { double Xd, Yd, Xu, Yu, common_denominator; /* convert from image to distorted sensor coordinates */ Xd = cp.dpx * (Xfd - cp.Cx) / cp.sx; Yd = cp.dpy * (Yfd - cp.Cy); /* convert from distorted sensor to undistorted sensor plane coordinates */ distorted_to_undistorted_sensor_coords (Xd, Yd, &Xu, &Yu); /* calculate the corresponding xw and yw world coordinates */ /* (these equations were derived by simply inverting */ /* the perspective projection equations using Macsyma) */ common_denominator = ((cc.r1 * cc.r8 - cc.r2 * cc.r7) * Yu + (cc.r5 * cc.r7 - cc.r4 * cc.r8) * Xu - cc.f * cc.r1 * cc.r5 + cc.f * cc.r2 * cc.r4); *xw = (((cc.r2 * cc.r9 - cc.r3 * cc.r8) * Yu + (cc.r6 * cc.r8 - cc.r5 * cc.r9) * Xu - cc.f * cc.r2 * cc.r6 + cc.f * cc.r3 * cc.r5) * zw + (cc.r2 * cc.Tz - cc.r8 * cc.Tx) * Yu + (cc.r8 * cc.Ty - cc.r5 * cc.Tz) * Xu - cc.f * cc.r2 * cc.Ty + cc.f * cc.r5 * cc.Tx) / common_denominator; *yw = -(((cc.r1 * cc.r9 - cc.r3 * cc.r7) * Yu + (cc.r6 * cc.r7 - cc.r4 * cc.r9) * Xu - cc.f * cc.r1 * cc.r6 + cc.f * cc.r3 * cc.r4) * zw + (cc.r1 * cc.Tz - cc.r7 * cc.Tx) * Yu + (cc.r7 * cc.Ty - cc.r4 * cc.Tz) * Xu - cc.f * cc.r1 * cc.Ty + cc.f * cc.r4 * cc.Tx) / common_denominator; } void skip_comment(fp) FILE *fp; { char c; if (feof (fp)) return; c=getc(fp); while(!feof (fp) && (isspace (c) || c == '#')) { if(c=='#') while(!feof (fp) && getc(fp)!='\n'); c=getc(fp); } if (!feof (fp)) ungetc(c,fp); } void load_cp_cc_data (fp, cp, cc) FILE *fp; struct camera_parameters *cp; struct calibration_constants *cc; { double sa, ca, sb, cb, sg, cg; skip_comment(fp); fscanf (fp, "%lf", &(cp->Ncx)); skip_comment(fp); fscanf (fp, "%lf", &(cp->Nfx)); skip_comment(fp); fscanf (fp, "%lf", &(cp->dx)); skip_comment(fp); fscanf (fp, "%lf", &(cp->dy)); skip_comment(fp); fscanf (fp, "%lf", &(cp->dpx)); skip_comment(fp); fscanf (fp, "%lf", &(cp->dpy)); skip_comment(fp); fscanf (fp, "%lf", &(cp->Cx)); skip_comment(fp); fscanf (fp, "%lf", &(cp->Cy)); skip_comment(fp); fscanf (fp, "%lf", &(cp->sx)); skip_comment(fp); fscanf (fp, "%lf", &(cc->f)); skip_comment(fp); fscanf (fp, "%lf", &(cc->kappa1)); skip_comment(fp); fscanf (fp, "%lf", &(cc->Tx)); skip_comment(fp); fscanf (fp, "%lf", &(cc->Ty)); skip_comment(fp); fscanf (fp, "%lf", &(cc->Tz)); skip_comment(fp); fscanf (fp, "%lf", &(cc->Rx)); skip_comment(fp); fscanf (fp, "%lf", &(cc->Ry)); skip_comment(fp); fscanf (fp, "%lf", &(cc->Rz)); sincos (cc->Rx, &sa, &ca); sincos (cc->Ry, &sb, &cb); sincos (cc->Rz, &sg, &cg); 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; skip_comment(fp); fscanf (fp, "%lf", &(cc->p1)); skip_comment(fp); fscanf (fp, "%lf", &(cc->p2)); } void print_cp_cc_data (fp, cp, cc) FILE *fp; struct camera_parameters *cp; struct calibration_constants *cc; { fprintf (fp, " f = %.6lf [mm]\n\n", cc->f); fprintf (fp, " kappa1 = %.6le [1/mm^2]\n\n", cc->kappa1); fprintf (fp, " Tx = %.6lf, Ty = %.6lf, Tz = %.6lf [mm]\n\n", cc->Tx, cc->Ty, cc->Tz); fprintf (fp, " Rx = %.6lf, Ry = %.6lf, Rz = %.6lf [deg]\n\n", cc->Rx * 180 / PI, cc->Ry * 180 / PI, cc->Rz * 180 / PI); fprintf (fp, " R\n"); fprintf (fp, " %9.6lf %9.6lf %9.6lf\n", cc->r1, cc->r2, cc->r3); fprintf (fp, " %9.6lf %9.6lf %9.6lf\n", cc->r4, cc->r5, cc->r6); fprintf (fp, " %9.6lf %9.6lf %9.6lf\n\n", cc->r7, cc->r8, cc->r9); fprintf (fp, " sx = %.6lf\n", cp->sx); fprintf (fp, " Cx = %.6lf, Cy = %.6lf [pixels]\n\n", cp->Cx, cp->Cy); fprintf (fp, " Tz / f = %.6lf\n\n", cc->Tz / cc->f); }