cal_main.cpp

有关摄像头定标的C++代码

void      ncc_compute_exact_f_and_Tz ()
{
#define NPARAMS 3

    int       i;

    /* Parameters needed by MINPACK's lmdif() */

    long     m = cd.point_count;
    long     n = NPARAMS;
    double  x[NPARAMS];
    double *fvec;
    double  ftol = REL_SENSOR_TOLERANCE_ftol;
    double  xtol = REL_PARAM_TOLERANCE_xtol;
    double  gtol = ORTHO_TOLERANCE_gtol;
    long     maxfev = MAXFEV;
    double  epsfcn = EPSFCN;
    double  diag[NPARAMS];
    long     mode = MODE;
    double  factor = FACTOR;
    long     nprint = 0;
    long     info;
    long     nfev;
    double *fjac;
    long     ldfjac = m;
    long     ipvt[NPARAMS];
    double  qtf[NPARAMS];
    double  wa1[NPARAMS];
    double  wa2[NPARAMS];
    double  wa3[NPARAMS];
    double *wa4;

    /* allocate some workspace */
    if (( fvec = (double *) calloc ((unsigned int) m, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
       exit(-1);
    }

    if (( fjac = (double *) calloc ((unsigned int) m*n, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
       exit(-1);
    }

    if (( wa4 = (double *) calloc ((unsigned int) m, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
       exit(-1);
    }

    /* use the current calibration constants as an initial guess */
    x[0] = cc.f;
    x[1] = cc.Tz;
    x[2] = cc.kappa1;

    /* define optional scale factors for the parameters */
    if ( mode == 2 ) {
        for (i = 0; i < NPARAMS; i++)
            diag[i] = 1.0;             /* some user-defined values */
    }
 
    /* perform the optimization */
    lmdif_ (ncc_compute_exact_f_and_Tz_error,
            &m, &n, x, fvec, &ftol, &xtol, &gtol, &maxfev, &epsfcn,
            diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
            ipvt, qtf, wa1, wa2, wa3, wa4);

    /* update the calibration constants */
    cc.f = x[0];
    cc.Tz = x[1];
    cc.kappa1 = x[2];

    /* release allocated workspace */
    free (fvec);
    free (fjac);
    free (wa4);

#ifdef DEBUG
    /* print the number of function calls during iteration */
    fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
#endif

#undef NPARAMS
}


/************************************************************************/
void      ncc_three_parm_optimization ()
{
    ncc_compute_Xd_Yd_and_r_squared ();

    ncc_compute_U ();

    ncc_compute_Tx_and_Ty ();

    ncc_compute_sx ();

    ncc_compute_Xd_Yd_and_r_squared ();

    ncc_compute_better_R ();

    ncc_compute_approximate_f_and_Tz ();

    if (cc.f < 0) {
	/* try the other solution for the orthonormal matrix */
	cc.r3 = -cc.r3;
	cc.r6 = -cc.r6;
	cc.r7 = -cc.r7;
	cc.r8 = -cc.r8;
	solve_RPY_transform ();

	ncc_compute_approximate_f_and_Tz ();

        if (cc.f < 0) {
            fprintf (stderr, "error - possible handedness problem with data\n");
            exit (-1);
	}
    }

    ncc_compute_exact_f_and_Tz ();
}


/************************************************************************/
void      ncc_nic_optimization_error (long *m_ptr, long *n_ptr, double *params, double *err)
{
    int       i;

    double    xc,
              yc,
              zc,
              Xd_,
              Yd_,
              Xu_1,
              Yu_1,
              Xu_2,
              Yu_2,
              distortion_factor,
              Rx,
              Ry,
              Rz,
              Tx,
              Ty,
              Tz,
              kappa1,
              sx,
              f,
              r1,
              r2,
              r3,
              r4,
              r5,
              r6,
              r7,
              r8,
              r9,
              sa,
              sb,
              sg,
              ca,
              cb,
              cg;

    Rx = params[0];
    Ry = params[1];
    Rz = params[2];
    Tx = params[3];
    Ty = params[4];
    Tz = params[5];
    kappa1 = params[6];
    f = params[7];
    sx = params[8];

    SINCOS (Rx, sa, ca);
    SINCOS (Ry, sb, cb);
    SINCOS (Rz, sg, cg);
    r1 = cb * cg;
    r2 = cg * sa * sb - ca * sg;
    r3 = sa * sg + ca * cg * sb;
    r4 = cb * sg;
    r5 = sa * sb * sg + ca * cg;
    r6 = ca * sb * sg - cg * sa;
    r7 = -sb;
    r8 = cb * sa;
    r9 = ca * cb;

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

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

	/* convert from 2D image coordinates to distorted sensor coordinates */
	Xd_ = cp.dpx * (cd.Xf[i] - cp.Cx) / sx;
	Yd_ = cp.dpy * (cd.Yf[i] - cp.Cy);

	/* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
	distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_));
	Xu_2 = Xd_ * distortion_factor;
	Yu_2 = Yd_ * distortion_factor;

        /* record the error in the undistorted sensor coordinates */
        err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
    }
}


void      ncc_nic_optimization ()
{
#define NPARAMS 9

    int       i;

    /* Parameters needed by MINPACK's lmdif() */

    long     m = cd.point_count;
    long     n = NPARAMS;
    double  x[NPARAMS];
    double *fvec;
    double  ftol = REL_SENSOR_TOLERANCE_ftol;
    double  xtol = REL_PARAM_TOLERANCE_xtol;
    double  gtol = ORTHO_TOLERANCE_gtol;
    long     maxfev = MAXFEV;
    double  epsfcn = EPSFCN;
    double  diag[NPARAMS];
    long     mode = MODE;
    double  factor = FACTOR;
    long     nprint = 0;
    long     info;
    long     nfev;
    double *fjac;
    long     ldfjac = m;
    long     ipvt[NPARAMS];
    double  qtf[NPARAMS];
    double  wa1[NPARAMS];
    double  wa2[NPARAMS];
    double  wa3[NPARAMS];
    double *wa4;

    /* allocate some workspace */
    if (( fvec = (double *) calloc ((unsigned int) m, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
       exit(-1);
    }

    if (( fjac = (double *) calloc ((unsigned int) m*n, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
       exit(-1);
    }

    if (( wa4 = (double *) calloc ((unsigned int) m, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
       exit(-1);
    }

    /* use the current calibration and camera constants as a starting point */
    x[0] = cc.Rx;
    x[1] = cc.Ry;
    x[2] = cc.Rz;
    x[3] = cc.Tx;
    x[4] = cc.Ty;
    x[5] = cc.Tz;
    x[6] = cc.kappa1;
    x[7] = cc.f;
    x[8] = cp.sx;

    /* define optional scale factors for the parameters */
    if ( mode == 2 ) {
        for (i = 0; i < NPARAMS; i++)
            diag[i] = 1.0;             /* some user-defined values */
    }

    /* perform the optimization */
    lmdif_ (ncc_nic_optimization_error,
            &m, &n, x, fvec, &ftol, &xtol, &gtol, &maxfev, &epsfcn,
            diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
            ipvt, qtf, wa1, wa2, wa3, wa4);

    /* update the calibration and camera constants */
    cc.Rx = x[0];
    cc.Ry = x[1];
    cc.Rz = x[2];
    apply_RPY_transform ();

    cc.Tx = x[3];
    cc.Ty = x[4];
    cc.Tz = x[5];
    cc.kappa1 = x[6];
    cc.f = x[7];
    cp.sx = x[8];

    /* release allocated workspace */
    free (fvec);
    free (fjac);
    free (wa4);

#ifdef DEBUG
    /* print the number of function calls during iteration */
    fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
#endif

#undef NPARAMS
}


/************************************************************************/
void      ncc_full_optimization_error (long *m_ptr, long *n_ptr, double *params, double *err)
{
    int       i;

    double    xc,
              yc,
              zc,
              Xd_,
              Yd_,
              Xu_1,
              Yu_1,
              Xu_2,
              Yu_2,
              distortion_factor,
              Rx,
              Ry,
              Rz,
              Tx,
              Ty,
              Tz,
              kappa1,
              sx,
              f,
              Cx,
              Cy,
              r1,
              r2,
              r3,
              r4,
              r5,
              r6,
              r7,
              r8,
              r9,
              sa,
              sb,
              sg,
              ca,
              cb,
              cg;

    Rx = params[0];
    Ry = params[1];
    Rz = params[2];
    Tx = params[3];
    Ty = params[4];
    Tz = params[5];
    kappa1 = params[6];
    f = params[7];
    sx = params[8];
    Cx = params[9];
    Cy = params[10];

    SINCOS (Rx, sa, ca);
    SINCOS (Ry, sb, cb);
    SINCOS (Rz, sg, cg);
    r1 = cb * cg;
    r2 = cg * sa * sb - ca * sg;
    r3 = sa * sg + ca * cg * sb;
    r4 = cb * sg;
    r5 = sa * sb * sg + ca * cg;
    r6 = ca * sb * sg - cg * sa;
    r7 = -sb;
    r8 = cb * sa;
    r9 = ca * cb;

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

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

	/* convert from 2D image coordinates to distorted sensor coordinates */
	Xd_ = cp.dpx * (cd.Xf[i] - Cx) / sx;
	Yd_ = cp.dpy * (cd.Yf[i] - Cy);

	/* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
	distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_));
	Xu_2 = Xd_ * distortion_factor;
	Yu_2 = Yd_ * distortion_factor;

        /* record the error in the undistorted sensor coordinates */
        err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
    }
}


void      ncc_full_optimization ()
{
#define NPARAMS 11

    int     i;

    /* Parameters needed by MINPACK's lmdif() */
 
    long     m = cd.point_count;
    long     n = NPARAMS;
    double  x[NPARAMS];
    double *fvec;
    double  ftol = REL_SENSOR_TOLERANCE_ftol;
    double  xtol = REL_PARAM_TOLERANCE_xtol;
    double  gtol = ORTHO_TOLERANCE_gtol;
    long     maxfev = MAXFEV;
    double  epsfcn = EPSFCN;
    double  diag[NPARAMS];
    long     mode = MODE;
    double  factor = FACTOR;
    long     nprint = 0;
    long     info;
    long     nfev;
    double *fjac;
    long     ldfjac = m;
    long     ipvt[NPARAMS];
    double  qtf[NPARAMS];
    double  wa1[NPARAMS];
    double  wa2[NPARAMS];
    double  wa3[NPARAMS];
    double *wa4;

    /* allocate some workspace */
    if (( fvec = (double *) calloc ((unsigned int) m, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
       exit(-1);
    }
 
    if (( fjac = (double *) calloc ((unsigned int) m*n, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
       exit(-1);
    }
 
    if (( wa4 = (double *) calloc ((unsigned int) m, (unsigned int) sizeof(double))) == NULL ) {
       fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
       exit(-1);
    }

    /* use the current calibration and camera constants as a starting point */
    x[0] = cc.Rx;//旋转角度；
    x[1] = cc.Ry;
    x[2] = cc.Rz;
    x[3] = cc.Tx;//平移距离；
    x[4] = cc.Ty;
    x[5] = cc.Tz;
    x[6] = cc.kappa1;//畸变参数；
    x[7] = cc.f;//焦距；
    x[8] = cp.sx;//镜片扭曲参数；
    x[9] = cp.Cx;//光轴原点中心位置；
    x[10] = cp.Cy;

    /* define optional scale factors for the parameters */
    if ( mode == 2 ) {
        for (i = 0; i < NPARAMS; i++)
            diag[i] = 1.0;             /* some user-defined values */
    }
 
    /* perform the optimization */
    lmdif_ (ncc_full_optimization_error,
            &m, &n, x, fvec, &ftol, &xtol, &gtol, &maxfev, &epsfcn,
            diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
            ipvt, qtf, wa1, wa2, wa3, wa4);

    /* update the calibration and camera constants */
    cc.Rx = x[0];
    cc.Ry = x[1];
    cc.Rz = x[2];
    apply_RPY_transform ();

    cc.Tx = x[3];
    cc.Ty = x[4];
    cc.Tz = x[5];
    cc.kappa1 = x[6];
    cc.f = x[7];
    cp.sx = x[8];
    cp.Cx = x[9];
    cp.Cy = x[10];

    /* release allocated workspace */
    free (fvec);
    free (fjac);
    free (wa4);

#ifdef DEBUG
    /* print the number of function calls during iteration */
    fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
#endif

#undef NPARAMS
}


/************************************************************************/
//非共面标定主程序；
void      coplanar_calibration ()
{
    /* just do the basic 3 parameter (Tz, f, kappa1) optimization */
    cc_three_parm_optimization ();
}


void      coplanar_calibration_with_full_optimization ()
{
    /* start with a 3 parameter (Tz, f, kappa1) optimization */
    cc_three_parm_optimization ();

    /* do a 5 parameter (Tz, f, kappa1, Cx, Cy) optimization */
    cc_five_parm_optimization_with_late_distortion_removal ();

    /* do a better 5 parameter (Tz, f, kappa1, Cx, Cy) optimization */
    cc_five_parm_optimization_with_early_distortion_removal ();

    /* do a full optimization minus the image center */
    cc_nic_optimization ();

    /* do a full optimization including the image center */
    cc_full_optimization ();
}


void      noncoplanar_calibration ()
{
    /* just do the basic 3 parameter (Tz, f, kappa1) optimization */
    ncc_three_parm_optimization ();
}


void      noncoplanar_calibration_with_full_optimization ()
{
    /* start with a 3 parameter (Tz, f, kappa1) optimization */
    ncc_three_parm_optimization ();

    /* do a full optimization minus the image center */
    ncc_nic_optimization ();

    /* do a full optimization including the image center */
    ncc_full_optimization ();
}