WebSVN - AndroidProjects - Rev 204 - /ARToolKit/util/calib_cparam/calib

/*
*
* This file is part of ARToolKit.
*
* ARToolKit is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* ARToolKit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ARToolKit; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/

// ============================================================================
// Includes
// ============================================================================

#ifdef _WIN32
# include <windows.h>
#endif
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#ifndef __APPLE__
# include <GL/glut.h>
# ifdef GL_VERSION_1_2
# include <GL/glext.h>
# endif
#else
# include <GLUT/glut.h>
# include <OpenGL/glext.h>
#endif
#include <AR/config.h>
#include <AR/ar.h>
#include <AR/video.h>
#include <AR/param.h>
#include <AR/gsub_lite.h>
#include "calib_cparam.h"

// ============================================================================
// Constants
// ============================================================================

#define CALIB_CPARAM_DEBUG 0

// ============================================================================
// Global variables
// ============================================================================

/* set up the video format globals */

#if defined(__sgi)
char *vconf = "-size=FULL";
#elif defined(__linux)
# if defined(AR_INPUT_GSTREAMER)
char *vconf = "videotestsrc";
# elif defined(AR_INPUT_V4L)
char *vconf = "-width=640 -height=480";
# elif defined(AR_INPUT_1394CAM)
char *vconf = "-mode=640x480_YUV411";
# elif defined(AR_INPUT_DV)
char *vconf = "";
# endif
#elif defined(_WIN32)
char *vconf = "Data\\WDM_camera_flipV.xml";
#elif defined(__APPLE__)
char *vconf = "-width=640 -height=480";
#else
char *vconf = "";
#endif

static ARUint8 *gARTImage = NULL;
static ARParam gARTCparam; // Dummy parameter, to supply to gsub_lite.
static ARGL_CONTEXT_SETTINGS_REF gArglSettings = NULL;

static int gWin;
static int gXsize = 0;
static int gYsize = 0;
int refresh;

static unsigned char *gSaveARTImage = NULL;
static ARGL_CONTEXT_SETTINGS_REF gSaveArglSettings = NULL;

static ARParam param;
static double dist_factor[4];

int line_num;
int loop_num;
int line_mode[LINE_MAX];
double inter_coord[LOOP_MAX][LINE_MAX][LINE_MAX][3];
double line_info[LOOP_MAX][LINE_MAX][4];

int mode;
int line_num_current;
int loop_num_current;
double theta;
double radius;
double gStartX, gStartY, gEndX, gEndY;

// ============================================================================
// Functions
// ============================================================================

int main(int argc, char *argv[]);
static int init(int argc, char *argv[]);
static void Keyboard(unsigned char key, int x, int y);
static void Special(int key, int x, int y);
static void Mouse(int button, int state, int x, int y);
static void Quit(void);
static void Idle(void);
static void Visibility(int visible);
static void Reshape(int w, int h);
static void Display(void);
static void drawPrevLine(void);
static void drawNextLine(void);
static void draw_warp_line(double sx, double ex, double sy, double ey);
static void getCpara(void);
static void intersection( double line1[4], double line2[4], double *screen_coord );

int main(int argc, char *argv[])
{
glutInit(&argc, argv);
if (!init(argc, argv)) exit(-1);

glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
glutInitWindowSize(gXsize, gYsize);
glutInitWindowPosition(100,100);
gWin = glutCreateWindow("Camera calibration");

// Setup argl library for current context.
// Turn off distortion compensation.. we are calibrating.
if ((gArglSettings = arglSetupForCurrentContext()) == NULL) {
fprintf(stderr, "main(): arglSetupForCurrentContext() returned error.\n");
exit(-1);
}
arglDistortionCompensationSet(gArglSettings, FALSE);

// Make a dummy camera parameter to supply when calling arglDispImage().
gARTCparam.xsize = gXsize;
gARTCparam.ysize = gYsize;

// Setup a space to save our captured image.
if ((gSaveArglSettings = arglSetupForCurrentContext()) == NULL) {
fprintf(stderr, "grabImage(): arglSetupForCurrentContext() returned error.\n");
exit(-1);
}
arglDistortionCompensationSet(gSaveArglSettings, FALSE);
arMalloc(gSaveARTImage, unsigned char, gXsize*gYsize*AR_PIX_SIZE_DEFAULT);

// Register GLUT event-handling callbacks.
// NB: Idle() is registered by Visibility.
glutDisplayFunc(Display);
glutReshapeFunc(Reshape);
glutVisibilityFunc(Visibility);
glutKeyboardFunc(Keyboard);
glutSpecialFunc(Special);
glutMouseFunc(Mouse);

if (arVideoCapStart() != 0) {
fprintf(stderr, "init(): Unable to begin camera data capture.\n");
return (FALSE);
}
mode = 0;
line_num_current = 0;
loop_num_current = 0;

glutMainLoop();
return (0);
}

static int init(int argc, char *argv[])
{
printf("Input center coordinates: X = ");
scanf("%lf", &dist_factor[0]);
while (getchar() != '\n');
printf(" : Y = ");
scanf("%lf", &dist_factor[1]);
while (getchar() != '\n');
printf("Input distortion ratio: F = ");
scanf("%lf", &dist_factor[2]);
while (getchar() != '\n');
printf("Input size adjustment factor: S = ");
scanf("%lf", &dist_factor[3]);
while (getchar() != '\n');

initLineModel(&line_num, &loop_num, line_mode, inter_coord);

// Open the video path.
if (arVideoOpen(vconf) < 0) {
fprintf(stderr, "init(): Unable to open connection to camera.\n");
return (FALSE);
}

// Find the size of the window.
if (arVideoInqSize(&gXsize, &gYsize) < 0) return (FALSE);
fprintf(stdout, "Camera image size (x,y) = (%d,%d)\n", gXsize, gYsize);

// Allocate space for a save image.
arMalloc(gSaveARTImage, unsigned char, gXsize * gYsize * AR_PIX_SIZE_DEFAULT);

param.xsize = gXsize;
param.ysize = gYsize;
param.dist_factor[0] = dist_factor[0];
param.dist_factor[1] = dist_factor[1];
param.dist_factor[2] = dist_factor[2];
param.dist_factor[3] = dist_factor[3];

return (TRUE);
}

static void thetaRadiusSet(void) {
if (line_num_current == 0) {
if (line_mode[line_num_current] == L_HORIZONTAL) {
theta = 90;
radius = gYsize/2;
} else {
theta = 0;
radius = gXsize/2;
}
} else {
if (line_mode[line_num_current] == L_HORIZONTAL) {
if (line_mode[line_num_current] == line_mode[line_num_current-1]) {
radius += 10.0;
} else {
theta = 90;
radius = gYsize/2;
}
} else {
if (line_mode[line_num_current] == line_mode[line_num_current-1]) {
radius += 10.0;
} else {
theta = 0;
radius = gXsize/2;
}
}
}
}

static void eventCancel(void) {
if (mode == 0) {
// Cancel from mode 0 should quit program.
arVideoCapStop();
Quit();
} else if (mode == 1) {
if (line_num_current == 0) {
// Cancel from mode 1, first line, should go back to mode 0.
arVideoCapStart();
mode = 0;
} else {
// Cancel from mode 1, non-first line, should go back to first line.
line_num_current = 0;
thetaRadiusSet();
glutPostRedisplay();
}
}
}

static void Mouse(int button, int state, int x, int y)
{
ARUint8 *image;

if (state == GLUT_DOWN) {
if (button == GLUT_RIGHT_BUTTON) {
eventCancel();
} else if (button == GLUT_LEFT_BUTTON && mode == 0) {
// Processing a new image.
// Copy an image to saved image buffer.
do {
image = arVideoGetImage();
} while (image == NULL);
memcpy(gSaveARTImage, image, gXsize*gYsize*AR_PIX_SIZE_DEFAULT);
printf("Grabbed image.\n");
arVideoCapStop();
mode = 1;
line_num_current = 0;
thetaRadiusSet();
glutPostRedisplay();
}
}
}

static void Keyboard(unsigned char key, int x, int y)
{
ARParam iparam;
double trans[3][4];
char name[256];
int k = 1;

if( mode == 0 ) return;

switch( key ) {
case 0x1B:
gStartX = -1.0;
gStartY = -1.0;
gEndX = -1.0;
gEndY = -1.0;
// fall through..
case 0x0D:
line_info[loop_num_current][line_num_current][0] = gStartX;
line_info[loop_num_current][line_num_current][1] = gStartY;
line_info[loop_num_current][line_num_current][2] = gEndX;
line_info[loop_num_current][line_num_current][3] = gEndY;
line_num_current++;
if (line_num_current < line_num) {
thetaRadiusSet();
} else {
// Completed placements for this image.
loop_num_current++;
if (loop_num_current < loop_num) {
arVideoCapStart();
mode = 0;
} else {
// All done. Calculate parameter, save it, and exit.
getCpara();
arParamDecomp( &param, &iparam, trans );
arParamDisp( &iparam );

printf("Input filename: ");
scanf("%s", name);
arParamSave( name, 1, &iparam );

Quit();
}
}
break;
default:
k = 0;
break;
}
if (k) glutPostRedisplay();
}

static void Special(int key, int x, int y)
{
double mx, my;
int k = 1;

if (mode == 0) return;

if (line_mode[line_num_current] == L_HORIZONTAL) {
switch (key) {
case GLUT_KEY_UP:
radius -= 1.0;
break;
case GLUT_KEY_DOWN:
radius += 1.0;
break;
case GLUT_KEY_LEFT:
mx = (gStartX + gEndX)/ 2.0;
my = (gStartY + gEndY)/ 2.0;
theta -= 0.25;
radius = cos( (double)(theta*3.141592/180.0) ) * mx
+ sin( (double)(theta*3.141592/180.0) ) * my;
break;
case GLUT_KEY_RIGHT:
mx = (gStartX + gEndX)/ 2.0;
my = (gStartY + gEndY)/ 2.0;
theta += 0.25;
radius = cos( (double)(theta*3.141592/180.0) ) * mx
+ sin( (double)(theta*3.141592/180.0) ) * my;
break;
default:
k = 0;
break;
}
} else {
switch (key) {
case GLUT_KEY_UP:
mx = (gStartX + gEndX)/ 2.0;
my = (gStartY + gEndY)/ 2.0;
theta += 0.25;
radius = cos( (double)(theta*3.141592/180.0) ) * mx
+ sin( (double)(theta*3.141592/180.0) ) * my;
break;
case GLUT_KEY_DOWN:
mx = (gStartX + gEndX)/ 2.0;
my = (gStartY + gEndY)/ 2.0;
theta -= 0.25;
radius = cos( (double)(theta*3.141592/180.0) ) * mx
+ sin( (double)(theta*3.141592/180.0) ) * my;
break;
case GLUT_KEY_LEFT:
radius -= 1.0;
break;
case GLUT_KEY_RIGHT:
radius += 1.0;
break;
default:
k = 0;
break;
}
}

if (k) glutPostRedisplay();
}

static void Quit(void)
{
if (gSaveArglSettings) arglCleanup(gSaveArglSettings);
if (gArglSettings) arglCleanup(gArglSettings);
if (gWin) glutDestroyWindow(gWin);
arVideoClose();
exit(0);
}

static void Idle(void)
{
static int ms_prev;
int ms;
float s_elapsed;
ARUint8 *image;

// Find out how long since Idle() last ran.
ms = glutGet(GLUT_ELAPSED_TIME);
s_elapsed = (float)(ms - ms_prev) * 0.001;
if (s_elapsed < 0.01f) return; // Don't update more often than 100 Hz.
ms_prev = ms;

// Grab a video frame.
if (mode == 0) {
if ((image = arVideoGetImage()) != NULL) {
gARTImage = image;
// Tell GLUT the display has changed.
glutPostRedisplay();
}
}
}

//
// This function is called on events when the visibility of the
// GLUT window changes (including when it first becomes visible).
//
static void Visibility(int visible)
{
if (visible == GLUT_VISIBLE) {
glutIdleFunc(Idle);
} else {
glutIdleFunc(NULL);
}
}

//
// This function is called when the
// GLUT window is resized.
//
static void Reshape(int w, int h)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glViewport(0, 0, (GLsizei) w, (GLsizei) h);

glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();

// Call through to anyone else who needs to know about window sizing here.
}

static void beginOrtho2D(int xsize, int ysize) {
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluOrtho2D(0.0, xsize, 0.0, ysize);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
}

static void endOrtho2D(void) {
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
}

static void Display(void)
{
// Select correct buffer for this context.
glDrawBuffer(GL_BACK);
glClear(GL_COLOR_BUFFER_BIT);
glDisable(GL_DEPTH_TEST);
glDisable(GL_LIGHTING);
glDisable(GL_TEXTURE_2D);
beginOrtho2D(gXsize, gYsize);

if (mode == 0) {

arglDispImage(gARTImage, &gARTCparam, 1.0, gArglSettings); // zoom = 1.0.
arVideoCapNext();
gARTImage = NULL;

} else if (mode == 1) {

arglDispImage(gSaveARTImage, &gARTCparam, 1.0, gSaveArglSettings);

drawPrevLine();
drawNextLine();
}

endOrtho2D();
glutSwapBuffers();
}

static void drawPrevLine(void)
{
int i;

glColor3f(0.0,0.0,1.0);
for (i = 0; i < line_num_current; i++) {
if (line_info[loop_num_current][i][0] == -1.0) continue;
draw_warp_line(line_info[loop_num_current][i][0], line_info[loop_num_current][i][2],
line_info[loop_num_current][i][1], line_info[loop_num_current][i][3]);
}
}

static void drawNextLine(void)
{
double sin_theta, cos_theta;
double x1, x2;
double y1, y2;

sin_theta = sin( (double)(theta*3.141592/180.0) );
cos_theta = cos( (double)(theta*3.141592/180.0) );

if (cos_theta != 0) {
x1 = radius / cos_theta;
x2 = (radius - (gYsize-1)*sin_theta) / cos_theta;
} else {
x1 = x2 = -1.0;
}

if (sin_theta != 0) {
y1 = radius / sin_theta;
y2 = (radius - (gXsize-1)*cos_theta) / sin_theta;
} else {
y1 = y2 = -1.0;
}

gEndY = -1;
if (x1 >= 0 && x1 <= gXsize-1) {
gStartX = x1;
gStartY = 0;
if (x2 >= 0 && x2 <= gXsize-1) {
gEndX = x2;
gEndY = gYsize-1;
} else if (y1 >= 0 && y1 <= gYsize-1) {
gEndX = 0;
gEndY = y1;
} else if (y2 >= 0 && y2 <= gYsize-1) {
gEndX = gXsize-1;
gEndY = y2;
} else {
printf("???\n");
}
} else if (y1 >= 0 && y1 <= gYsize-1) {
gStartX = 0;
gStartY = y1;
if (x2 >= 0 && x2 <= gXsize-1) {
gEndX = x2;
gEndY = gYsize-1;
} else if (y2 >= 0 && y2 <= gYsize-1) {
gEndX = gXsize-1;
gEndY = y2;
} else {
printf("???\n");
}
} else if (x2 >= 0 && x2 <= gXsize-1) {
gStartX = x2;
gStartY = gYsize-1;
if (y2 >= 0 && y2 <= gYsize-1) {
gEndX = gXsize-1;
gEndY = y2;
} else {
printf("???\n");
}
}

glColor3f(1.0,1.0,1.0);
if (gEndY != -1) {
draw_warp_line(gStartX, gEndX, gStartY, gEndY);
}
}

static void draw_warp_line(double sx, double ex, double sy, double ey)
{
double a, b, c;
double x, y;
double x1, y1;
int i;

a = ey - sy;
b = sx - ex;
c = sy*ex - sx*ey;

glLineWidth(1.0f);
glBegin(GL_LINE_STRIP);
if (a*a >= b*b) {
for (i = -20; i <= gYsize+20; i+=10) {
x = -(b*i + c)/a;
y = i;

arParamIdeal2Observ(dist_factor, x, y, &x1, &y1);
glVertex2f(x1, gYsize-1-y1);
}
} else {
for (i = -20; i <= gXsize+20; i+=10) {
x = i;
y = -(a*i + c)/b;

arParamIdeal2Observ(dist_factor, x, y, &x1, &y1);
glVertex2f(x1, gYsize-1-y1);
}
}
glEnd();
}

static void getCpara(void)
{
double *world_coord;
double *screen_coord;
int point_num;
int i, j, k;

point_num = 0;
for (k = 0; k < loop_num; k++) {
for (i = 0; i < line_num; i++) {
for (j = 0; j < line_num; j++) {
if (inter_coord[k][i][j][0] != -10000.0) point_num++;
}
}
}
arMalloc(world_coord, double, point_num * 3);
arMalloc(screen_coord, double, point_num * 2);

point_num = 0;
for (k = 0; k < loop_num; k++) {
for (i = 0; i < line_num; i++) {
if (line_info[k][i][0] == -1.0) continue;
for (j = 0; j < line_num; j++) {
if (line_info[k][j][0] == -1.0) continue;
if (inter_coord[k][i][j][0] == -10000.0) continue;

world_coord[point_num*3+0] = inter_coord[k][i][j][0];
world_coord[point_num*3+1] = inter_coord[k][i][j][1];
world_coord[point_num*3+2] = inter_coord[k][i][j][2];
intersection(line_info[k][i], line_info[k][j],
&(screen_coord[point_num*2]));

point_num++;
}
}
}
printf("point_num = %d\n", point_num);
if (arParamGet((double (*)[3])world_coord, (double (*)[2])screen_coord, point_num, param.mat) < 0) {
printf("ddd error!!\n");
exit(0);
}

free(world_coord);
free(screen_coord);
}

static void intersection(double line1[4], double line2[4], double *screen_coord)
{
double a, b, c, d, e, f, g;

a = line1[1] - line1[3];
b = line1[2] - line1[0];
c = line1[0] * a + line1[1] * b;
d = line2[1] - line2[3];
e = line2[2] - line2[0];
f = line2[0] * d + line2[1] * e;

g = a*e - b*d;
if (g == 0.0) { printf("???\n"); exit(0); }

screen_coord[0] = (c * e - b * f) / g;
screen_coord[1] = (a * f - c * d) / g;
}

Subversion Repositories AndroidProjects

(root)/ARToolKit/util/calib_cparam/calib_cparam.c - Rev 204