ic2wc.c

做立体视觉时的标定算法

/*******************************************************************************\
*                                                                               *
* This program allows a user to interactively convert 2D image coordinates      *
* into 3D world coordinates.  The perform the conversion the program requires   *
* a calibrated camera model of the form generated by ccal, ccal_fo, n_al,       *
* 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"

#define MIN_X	0
#define MAX_X	575

#define MIN_Y	0
#define MAX_Y	383

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

    double    Xw,
              Yw,
              Zw,
              Xf,
              Yf,
              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 image coordinates and Zw coordinate */
	(void) fprintf (stdout, "\n Enter Xf [%d:%d] : ", MIN_X, MAX_X);
	if (gets (temp) == NULL)
	    break;
	Xf = atof (temp);

	(void) fprintf (stdout, "\n Enter Yf [%d:%d] : ", MIN_Y, MAX_Y);
	if (gets (temp) == NULL)
	    break;
	Yf = atof (temp);

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

	/* determine the corresponding X and Y world coordinates */
	image_coord_to_world_coord (Xf, Yf, Zw, &Xw, &Yw);

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

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

    return 0;
}