#ifndef __TF_H__ #define __TF_H__ #include #include #include #include #include #include #include #include #include #include #include #include #include //#include "errors.h" #define SIGNBIT(a) (((a) > 0) ? 0 : 1) /* Some math libraries may not have the sincos() routine */ #ifdef _SINCOS_ void sincos(); #define SINCOS(x,s,c) sincos(x,&s,&c) #else double sin(), cos(); #define SINCOS(x,s,c) s=sin(x);c=cos(x) #endif #define SQR(a) ((a) * (a)) #define SQRT3 1.732050807568877293527446341505872366943 #define SQRT(x) sqrt(fabs(x)) #define MAX(a,b) (((a) > (b)) ? (a) : (b)) #define CUB(a) ((a)*(a)*(a)) /* Parameters controlling MINPACK's lmdif() optimization routine. */ /* See the file lmdif.f for definitions of each parameter. */ #define REL_SENSOR_TOLERANCE_ftol 1.0E-5 /* [pix] */ #define REL_PARAM_TOLERANCE_xtol 1.0E-7 #define ORTHO_TOLERANCE_gtol 0.0 #define MAXFEV (1000*n) #define EPSFCN 1.0E-16 /* Do not set to zero! */ #define MODE 1 /* variables are scalled internally */ #define FACTOR 100.0 /********************************************************* *********************************************************/ typedef struct _camera_constants_t { double Ncx; /* [sel] Number of sensor elements in camera's x direction */ double Nfx; /* [pix] Number of pixels in frame grabber's x direction */ double dx; /* [mm/sel] X dimension of camera's sensor element (in mm) */ double dy; /* [mm/sel] Y dimension of camera's sensor element (in mm) */ double dpx; /* [mm/pix] Effective X dimension of pixel in frame grabber */ double dpy; /* [mm/pix] Effective Y dimension of pixel in frame grabber */ double Cx; /* [pix] Z axis intercept of camera coordinate system */ double Cy; /* [pix] Z axis intercept of camera coordinate system */ double sx; /* [] Scale factor to compensate for any error in dpx */ } *camera_constants_t; /************************************************************************** * The world_coords are the world positions xw, yw, and zw of each target * * The image_coords are **************************************************************************/ typedef struct _calibration_data_t { int ntargets; /* number of targets */ r3_t *world_coords; /* world coordinates of each target [mm] */ r2_t *image_coords; /* observed image coordinates of each target [pixels] */ } *calibration_data_t; typedef struct _calibration_aux_data_t { int ntargets; /* number of targets */ r2_t *distorted_coords; /* computed image coordinates of each target [mm] */ double *r_squared; /* squared distance from image center to each target [mm] */ } *calibration_aux_data_t; /********************************************************* * TRANSFORMATION MATRIX * * 0 1 2 3 * * ------------------------------ * * 0 | 1.00 0.00 0.00 0.00 | * * S = 1 | Tx r1 r2 r3 | * * 2 | Ty r4 r5 r6 | * * 3 | Tz r7 r8 r9 | * * ------------------------------ * *********************************************************/ typedef struct _calibration_parameters_t { double f; /* [mm] */ double kappa; /* [1/mm^2] */ r4x4_t S; /*world to camera transformation matrix [mm->mm]*/ } *calibration_parameters_t; typedef struct _rmxn_t { int nrows; int ncols; double **c; } *rmxn_t; typedef struct _mat_rm_t { int nrows; int ncols; double *c; } *mat_rm_t; typedef struct _rm_t { int size; double *c; } *rm_t; typedef struct _weights_t { int ntargets; /* [number of targets] */ double *c; /* */ } *weights_t; /*Functions*/ void tf_tsai (calibration_data_t cdat, camera_constants_t camc, calibration_parameters_t cpar, weights_t wgts, FILE *ferr); void tf_compute_xd_yd_and_r_squared (calibration_data_t cdat, camera_constants_t camc, calibration_aux_data_t caux); rm_t tf_compute_U (calibration_data_t cdat, calibration_aux_data_t caux, weights_t wgts); void tf_compute_tx_and_ty (calibration_data_t cdat, calibration_parameters_t cpar, calibration_aux_data_t caux, rm_t U); void tf_compute_sx (camera_constants_t camc, calibration_parameters_t cpar, rm_t U); void tf_compute_better_R (calibration_parameters_t cpar, camera_constants_t camc, rm_t U, r3_t *R); void tf_solve_RPY_transform (calibration_parameters_t cpar, r3_t *R); void tf_compute_approximate_f_and_Tz (calibration_parameters_t cpar, calibration_data_t cdat, calibration_aux_data_t caux, weights_t wgts); void tf_compute_exact_f_and_Tz (calibration_parameters_t cpar, calibration_data_t cdat, calibration_aux_data_t caux, weights_t wgts); void tf_compute_exact_f_and_Tz_error (doublereal *params, doublereal *err, doublereal *uwerr, weights_t wgts, calibration_data_t cdat, calibration_parameters_t cpar, calibration_aux_data_t caux); /*################################# functions of tdata.c #################################*/ calibration_data_t tf_get_calibration_data_from_file (char *fname); weights_t tf_get_weights_data_from_file (char *fname); /*############################### functions of tstructures.c ##############################*/ camera_constants_t tf_get_canon_optura_parameters (void); calibration_parameters_t tf_create_calibration_constant_structure (void); void tf_free_calibration_constant_structure (calibration_parameters_t cpar); calibration_aux_data_t tf_create_auxiliary_calibration_aux_data_structure (int ntargets); void tf_free_auxiliary_calibration_aux_data_structure (calibration_aux_data_t caux); void tf_copy_vector_rm (rm_t source, rm_t target); mat_rm_t tf_alloc_mat_rm (int nrows, int ncols); void tf_free_mat_rm_structure (mat_rm_t m); rm_t tf_alloc_rm (int size); void tf_free_rm_structure (rm_t v); rmxn_t tf_alloc_rmxn (int nrows, int ncols); void tf_free_rmxn_structure (rmxn_t m); void tf_free_calibration_data_structure (calibration_data_t cdat); void tf_free_calibration_data_structure (calibration_data_t cdat); void tf_free_weights_structure (weights_t wgts); /*################################# functions of tmatrix.c #################################*/ void tf_solve_system_mxn (mat_rm_t M, rm_t a, rm_t b, weights_t wgts); void tf_apply_weiths (mat_rm_t m, weights_t wgts); mat_rm_t tf_transpose_mat_rm (mat_rm_t m); void tf_print_mat_rm (mat_rm_t m); /*################################# functions of terrors.c #################################*/ /******************************************************************************* * This routine call functions that calculates 4 types of errors by the * parameters calculated by the Tsai algorithm. *******************************************************************************/ void tf_print_error_stats (calibration_data_t cdat, camera_constants_t camc, calibration_parameters_t cpar, FILE *ferr); /******************************************************************************* * This routine print all the parameters of the calibration parameters structure. *******************************************************************************/ void tf_print_calibration_parameters (calibration_parameters_t cpar, FILE *ferr); /******************************************************************************* * This routine print all the parameters of the calibration data structure. *******************************************************************************/ void tf_print_calibration_data (calibration_data_t cdat, FILE *ferr); /******************************************************************************* * This routine print the weiths used in the iteration of Tsai method. *******************************************************************************/ void tf_print_weights (weights_t wgts, FILE *ferr); /******************************************************************************* * This routine print all the parameters of the calibration aux data structure. *******************************************************************************/ void tf_print_calibration_aux_data (calibration_aux_data_t caux, FILE *ferr); /******************************************************************************* * This routine calculates the mean, standard deviation, max, and sum-of-squared * * error of the magnitude of the error, in distorted image coordinates, between * * the measured location of a feature point in the image plane and the image of * * the 3D feature point as projected through the calibrated model. * * The calculation is for all of the points in the calibration data set. * *******************************************************************************/ void tf_distorted_image_plane_error (calibration_data_t cdat, camera_constants_t camc, calibration_parameters_t cpar, FILE *ferr); /******************************************************************************* * This routine calculates the mean, standard deviation, max, and sum-of-squared * * error of the magnitude of the error, in undistorted image coordinates, between* * the measured location of a feature point in the image plane and the image of * * the 3D feature point as projected through the calibrated model. * * The calculation is for all of the points in the calibration data set. * *******************************************************************************/ void tf_undistorted_image_plane_error (calibration_data_t cdat, camera_constants_t camc, calibration_parameters_t cpar, FILE *ferr); /******************************************************************************* * This routine calculates the mean, standard deviation, max, and sum-of-squared * * error of the distance of closest approach (i.e. 3D error) between the point * * in object space and the line of sight formed by back projecting the measured * * 2D coordinates out through the camera model. * * The calculation is for all of the points in the calibration data set. * *******************************************************************************/ void tf_object_space_error_stats (calibration_data_t cdat, camera_constants_t camc, calibration_parameters_t cpar, FILE *ferr); /******************************************************************************* * This routine performs an error measure proposed by Weng in IEEE PAMI, * * October 1992. * *******************************************************************************/ void tf_normalized_calibration_error (calibration_data_t cdat, camera_constants_t camc, calibration_parameters_t cpar, FILE *ferr); /*********************************************************************** * This routine takes the position of a point in world coordinates [mm] * * and determines the position of its image in image coordinates [pix]. * ***********************************************************************/ void tf_world_coord_to_image_coord (calibration_parameters_t cpar, calibration_data_t cdat, camera_constants_t camc, double *xf, double *yf, int i); /*********************************************************************** * This routine takes the position of a point in world coordinates [mm] * * and determines the position with some pre-defined distortion * * of its image in image coordinates [pix]. * ***********************************************************************/ void tf_world_coord_to_distorted_image_coord (double xc, double yc, double zc, calibration_parameters_t cpar, camera_constants_t camc, double *Xf, double *Yf); /*************************************************************************** * This routine converts from undistorted to distorted sensor coordinates. * * The technique involves algebraically solving the cubic polynomial * * Ru = Rd * (1 + kappa * Rd**2) * * using the Cardan method. * * * * Note: for kappa1 < 0 the distorted sensor plane extends out to a * * maximum barrel distortion radius of Rd = sqrt (-1/(3 * kappa1)). * * * * To see the logic used in this routine try graphing the above * * polynomial for positive and negative kappa's. * **************************************************************************/ void tf_undistorted_to_distorted_sensor_coord (calibration_parameters_t cpar, double Xu, double Yu, double *Xd, double *Yd); /*********************************************************************** * This routine takes the position of a point in world coordinates [mm] * * and determines its position in camera coordinates [mm]. * ***********************************************************************/ void tf_world_coord_to_camera_coord (calibration_parameters_t cpar, calibration_data_t cdat, double *xc, double *yc, double *zc, int i); /************************************************************************ * This cube root routine handles negative arguments (unlike cbrt). * ************************************************************************/ double CBRT (double x); #endif