wc2ic.c

标准tsai摄像机标定源代码。（两步标定）

/*******************************************************************************\
*                                                                               *
* This program allows a user to interactively convert 3D world coordinates      *
* into 2D image coordinates.  The perform the conversion the program requires   *
* a calibrated camera model of the form generated by ccal, ccal_fo, ncal,       *
* or ncal_fo.                                                                   *
*                                                                               *
* History                                                                       *
* -------                                                                       *
*                                                                               *
* 01-Apr-95  Reg Willson ([e-mail]) at 3M St. Paul, MN                 *
*       Filename changes for DOS port.                                          *
*                                                                               *
* 30-May-93  Reg Willson ([e-mail]) at Carnegie-Mellon University         *
*       Original implementation.                                                *
*                                                                               *
\*******************************************************************************/

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

main (argc, argv)
    int       argc;
    char    **argv;
{
    FILE     *data_fd;

    double    Xw,
              Yw,
              Zw,
              Xfd,
              Yfd,
              atof ();

    char      temp[256];

    if (argc != 2) {
	(void) fprintf (stderr, "syntax: %s datafile\n", argv[0]);
	exit (-1);
    }
    /* load up the camera parameters and calibration constants from the given data file */
    if ((data_fd = fopen (argv[1], "r")) == NULL) {
	(void) fprintf (stderr, "%s: unable to open file \"%s\"\n", argv[0], argv[1]);
	exit (-1);
    }

    load_cp_cc_data (data_fd, &cp, &cc);
    fclose (data_fd);

    (void) fprintf (stdout, "\n Input file: %s\n\n", argv[1]);

    print_cp_cc_data (stdout, &cp, &cc);

    while (1) {
	/* prompt for the world coordinates */
	(void) fprintf (stdout, "\n Enter Xw [mm] : ");
	if (gets (temp) == NULL)
	    break;
	Xw = atof (temp);

	(void) fprintf (stdout, "\n Enter Yw [mm] : ");
	if (gets (temp) == NULL)
	    break;
	Yw = atof (temp);

	(void) fprintf (stdout, "\n Enter Zw [mm] : ");
	if (gets (temp) == NULL)
	    break;
	Zw = atof (temp);

	/* determine the corresponding image coordinates */
	world_coord_to_image_coord (Xw, Yw, Zw, &Xfd, &Yfd);

	(void) fprintf (stdout,
			"\n    [Xw,Yw,Zw] = [%.2lf, %.2lf, %.2lf]  -->  [Xf,Yf] = [%.2lf, %.2lf]\n",
			Xw, Yw, Zw, Xfd, Yfd);
    }

    (void) fprintf (stdout, "\n\n");

    return 0;
}