/*******************************************************************************\
*                                                                               *
* This file contains the class 'TsaiCal' which contains methods to convert      *
* from world to image coordinates. The constructor needs Camera Parameters in   *
* the format obtained from Tsai's Calibration C software.                       *
* This class allows the use of several calibrated cameras within a single       *
* application, although Tsai's Calibration C software will need to be used in   *
* advance to work out the Camera Parameters from calibration data of            *
* correspondences between points. (either coplanar calibration or non-coplanar) *
*                                                                               *
* Chris Needham - Leeds Vision Group, School of Computing. 28-Nov-2001          *
*                                                                               *
*********************************************************************************
*										*
* This file contains operating constants, data structure definitions and I/O	*
* variable declarations for the calibration routines contained in the file	*
* cal_main.c.						                        *
*                                                                               *
* 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                                                  *
*                                                                               *
\*******************************************************************************/
/* Commonly used macros */
#define ABS(a)          (((a) > 0) ? (a) : -(a))
#define SIGNBIT(a)      (((a) > 0) ? 0 : 1)
#define SQR(a)          ((a) * (a))
#define CUB(a)          ((a)*(a)*(a))
#define MAX(a,b)        (((a) > (b)) ? (a) : (b))
#define MIN(a,b)        (((a) < (b)) ? (a) : (b))

/* 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

/*******************************************************************************\
*										*
* Camera parameters are usually the fixed parameters of the given camera 	*
* system, typically obtained from manufacturers specifications.			*
*										*
* Cy and Cy (the center of radial lens distortion), may be calibrated		*
* separately or as part of the coplanar/noncoplanar calibration.		*
* The same with sx (x scale uncertainty factor).				*
*										*
\*******************************************************************************/
struct camera_parameters {
    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   */
};

/*******************************************************************************\
*										*
* Calibration constants are the model constants that are determined from the 	*
* calibration data.								*
*										*
\*******************************************************************************/
struct calibration_constants {
    double    f;		/* [mm]          */
    double    kappa1;		/* [1/mm^2]      */
    double    p1;		/* [1/mm]        */
    double    p2;		/* [1/mm]        */
    double    Tx;		/* [mm]          */
    double    Ty;		/* [mm]          */
    double    Tz;		/* [mm]          */
    double    Rx;		/* [rad]	 */
    double    Ry;		/* [rad]	 */
    double    Rz;		/* [rad]	 */
    double    r1;		/* []            */
    double    r2;		/* []            */
    double    r3;		/* []            */
    double    r4;		/* []            */
    double    r5;		/* []            */
    double    r6;		/* []            */
    double    r7;		/* []            */
    double    r8;		/* []            */
    double    r9;		/* []            */
};

// Can be initialised with TsaiCal() and then calling setup_cal(char* filename);
// or with TsaiCal(char* file);
class TsaiCal
{
private:
	char*			datafile;
	camera_parameters 	cp;
	calibration_constants 	cc;
	
public:
		TsaiCal(){};
		TsaiCal(char* file);
		~TsaiCal(){};

	void	setup_cal(char* filename);
	
        void    world_coord_to_image_coord (double xw, double yw, double zw,
                                            double* Xf, double* Yf);
        void    image_coord_to_world_coord (double Xfd, double Yfd, double zw,
                                            double* xw, double* yw);
        void    world_coord_to_camera_coord (double xw, double yw, double zw,
                                             double* xc, double* yc, double* zc);
        void    camera_coord_to_world_coord (double xc, double yc, double zc,
                                             double* xw, double* yw, double* zw);

        void    undistorted_to_distorted_sensor_coord (double Xu, double Yu, 
			                               double* Xd, double* Yd);
        void    distorted_to_undistorted_sensor_coord (double Xd, double Yd, 
			                               double* Xu, double* Yu);
        void    undistorted_to_distorted_image_coord (double Xfu, double Yfu, 
			                              double* Xfd, double* Yfd);
        void    distorted_to_undistorted_image_coord (double Xfd, double Yfd, 
			                              double* Xfu, double* Yfu);
private:
	void	load_cp_cc_data ();
};