ncsyn.c

做立体视觉时的标定算法

/*******************************************************************************\
*                                                                               *
* This program generates a synthetic set of calibration data for use            *
* with Tsai's camera calibration algorithm.                                     *
*                                                                               *
* History                                                                       *
* -------                                                                       *
*                                                                               *
* 06-Jun-94  Reg Willson ([e-mail]) at Carnegie-Mellon University         *
*       Fixed bug in call to print_cp_cc_data ().  The second and third         *
*       arguments should be pointers to the data structures.                    *
*       Bug reported by Chien-Ping Lu <[e-mail]>        *
*                                                                               *
* 30-Nov-93  Reg Willson ([e-mail]) at Carnegie-Mellon University         *
*       Updated to use new calibration statistics routines.                     *
*                                                                               *
* 01-May-93  Reg Willson ([e-mail]) at Carnegie-Mellon University         *
*       Modified to use utility routines.                                       *
*                                                                               *
* 07-Feb-93  Reg Willson ([e-mail]) at Carnegie-Mellon University         *
*       Original implementation.                                                *
*                                                                               *
\*******************************************************************************/

#include <stdio.h>
#include <math.h>
#include "cal_main.h"

#define PI              3.14159265358979323846264338327950288419716939937511

#define TARGET_STDEV	0.04	/* 0.04 standard deviation of target dimensions in [mm] */
#define SENSOR_STDEV	0.02	/* 0.02 standard deviation of sensor measurements in [pixels] */

double    gasdev ();		/* random number generator */

int       idum = -1362;		/* initial seed for gasdev */


/* reasonable calibration constants for coplanar calibration */
void      cc_generate_calibration_constants ()
{
    cc.f = 90.0;		/* [mm] (focal length) */
    cc.kappa1 = -0.0006;	/* [1/mm^2] (distortion coefficient) */

    cc.Tx = -100.0;		/* [mm] (x translation) */
    cc.Ty = -85.0;		/* [mm] (y translation) */
    cc.Tz = 2000.0;		/* [mm] (z translation) */

    cc.Rx = 30.0 * PI / 180;	/* [rad] (x rotation) */
    cc.Ry = 1.0 * PI / 180;	/* [rad] (y rotation) */
    cc.Rz = 2.0 * PI / 180;	/* [rad] (z rotation) */
    apply_RPY_transform ();

    cc.Tx = -150.0;		/* [mm] (x translation) */
    cc.Ty = -75.0;		/* [mm] (y translation) */
    cc.Tz = 2200.0;		/* [mm] (z translation) */

    cc.Rx = 20.0 * PI / 180;	/* [rad] (x rotation) */
    cc.Ry = 5.0 * PI / 180;	/* [rad] (y rotation) */
    cc.Rz = -1.0 * PI / 180;	/* [rad] (z rotation) */
    apply_RPY_transform ();
}


/* reasonable calibration constants for noncoplanar calibration */
void      nc_generate_calibration_constants ()
{
    cc.f = 70.0;		/* [mm] (focal length) */
    cc.kappa1 = -0.0006;	/* [1/mm^2] (distortion coefficient) */
   
    cc.Tx = -100.0;		/* [mm] (x translation) */
    cc.Ty = -85.0;		/* [mm] (y translation) */
    cc.Tz = 2000.0;		/* [mm] (z translation) */

    cc.Rx = 30.0 * PI / 180;	/* [rad] (x rotation) */
    cc.Ry = 1.0 * PI / 180;	/* [rad] (y rotation) */
    cc.Rz = 2.0 * PI / 180;	/* [rad] (z rotation) */
    apply_RPY_transform ();
}



void      generate_calibration_data ()
{
#define X_COUNT		10	/* number of columns */
#define Y_COUNT		10	/* number of rows */
#define Z_COUNT		 3	/* number of planes */

#define X_ORIGIN        10	/* [mm] */
#define Y_ORIGIN        10	/* [mm] */
#define Z_ORIGIN	 0	/* [mm] */

#define X_SPACING       20	/* inter plane spacing [mm] */
#define Y_SPACING	20	/* inter plane spacing [mm] */
#define Z_SPACING	20	/* intra plane spacing [mm] */

    int       i,
              j,
              k;

    cd.point_count = 0;
    for (k = 0; k < Z_COUNT; k++)
	for (i = 0; i < X_COUNT; i++)
	    for (j = 0; j < Y_COUNT; j++) {
		/* generate a point in 3D world coordinates */
		cd.xw[cd.point_count] = i * X_SPACING + X_ORIGIN;
		cd.yw[cd.point_count] = j * Y_SPACING + Y_ORIGIN;
		cd.zw[cd.point_count] = k * Z_SPACING + Z_ORIGIN;

		/* generate the corresponding 2D image coordinates, including dimensional */
		/* noise - gaussian, zero mean, TARGET_STDEV standard deviation */
		world_coord_to_image_coord (cd.xw[cd.point_count] + gasdev (&idum) * TARGET_STDEV,
					    cd.yw[cd.point_count] + gasdev (&idum) * TARGET_STDEV,
					    cd.zw[cd.point_count] + gasdev (&idum) * TARGET_STDEV,
					    &(cd.Xf[cd.point_count]),
					    &(cd.Yf[cd.point_count]));

		/* add in measurement noise - gaussian, zero mean, SENSOR_STDEV standard deviation */
		cd.Xf[cd.point_count] += gasdev (&idum) * SENSOR_STDEV;
		cd.Yf[cd.point_count] += gasdev (&idum) * SENSOR_STDEV;

		cd.point_count += 1;
	    }
}


main ()
{
#define CX_OFFSET       4	/* x offset for center of radial lens distortion */
#define CY_OFFSET       8	/* y offset for center of radial lens distortion */
#define SX_FACTOR	1	/* perturbation factor for x scale factor */

    /* initialize the camera parameters (cp) with the photometrics constants */
    initialize_photometrics_parms ();

    /* perturb the image center a bit */
    cp.Cx += CX_OFFSET;
    cp.Cy += CY_OFFSET;

    /* perturb the scaling factor a bit */
    cp.sx *= SX_FACTOR;

    /* generate a synthetic set of calibration constants (cc) */
    nc_generate_calibration_constants ();

    /* generate a synthetic set of calibration data (cd) */
    generate_calibration_data ();

    fprintf (stderr, "\nNon-coplanar synthetic calibration data generation:\n");

    fprintf (stderr, "\ncamera type: %s\n\n", camera_type);

    print_cp_cc_data (stderr, &cp, &cc);

    print_error_stats (stderr);

    dump_cd_data (stdout, &cd);

    return 0;
}