WebSVN - AndroidProjects - Rev 204 - /ARToolKit/util/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/video.h>
#include <AR/param.h>
#include <AR/matrix.h>
#include <AR/gsub_lite.h>
#include "calib_camera.h"

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

#define CALIB_CAMERA2_DEBUG 0

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

/* set up the video format globals */

#if defined(__sgi)
static char *vconf = "-size=FULL";
#elif defined(__linux)
# if defined(AR_INPUT_GSTREAMER)
char *vconf = "videotestsrc ! capsfilter caps=video/x-raw-rgb,bpp=24 ! identity name=artoolkit ! fakesink";
# elif defined(AR_INPUT_V4L)
static char *vconf = "-width=640 -height=480";
# elif defined(AR_INPUT_1394CAM)
static char *vconf = "-mode=640x480_YUV411";
# elif defined(AR_INPUT_DV)
static char *vconf = "";
# endif
#elif defined(_WIN32)
static char *vconf = "Data\\WDM_camera_flipV.xml";
#elif defined(__APPLE__)
static char *vconf = "-width=640 -height=480";
#else
static 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;
static int gThresh = THRESH;
static unsigned char *gClipImage = NULL;

static CALIB_PATT_T gPatt;
static double dist_factor[4];
static double mat[3][4];

static int point_num;
static int gDragStartX = -1, gDragStartY = -1, gDragEndX = -1, gDragEndY = -1; // x and y coordinates of start and end of mouse drag.

static int gStatus; // 0 = Waiting to grab image, 1 = Drawing bounding boxes, 2 = Placing warp lines.
static int check_num;

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

int main(int argc, char *argv[]);
static int init(int argc, char *argv[]);
static void Mouse(int button, int state, int x, int y);
static void Motion(int x, int y);
static void Keyboard(unsigned char key, 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 draw_warp_line(double a, double b , double c);
static void draw_line(void);
static void draw_line2(double *x, double *y, int num);
static void draw_warp_line(double a, double b , double c);
static void print_comment(int status);
static void save_param(void);

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("Calibrate camera parameters");

// Setup argl library for current context.
// Turn off distortion compensation.. we don't know distortion yet!
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;

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

// Start grabbing.
if (arVideoCapStart() != 0) {
fprintf(stderr, "init(): Unable to begin camera data capture.\n");
return (FALSE);
}
point_num = 0;
gStatus = 0;
print_comment(0);

glutMainLoop();

return (0);
}

static int init(int argc, char *argv[])
{
double length;
char line[512];
int i, j;

gPatt.h_num = H_NUM;
gPatt.v_num = V_NUM;
gPatt.loop_num = 0;
if (gPatt.h_num < 3 || gPatt.v_num < 3) exit(0);

printf("Input the distance between each marker dot, in millimeters: ");
scanf("%lf", &length);
while (getchar() != '\n');

gPatt.world_coord = (CALIB_COORD_T *)malloc(sizeof(CALIB_COORD_T) * gPatt.h_num * gPatt.v_num);
for (j = 0; j < gPatt.v_num; j++) {
for (i = 0; i < gPatt.h_num; i++) {
gPatt.world_coord[j*gPatt.h_num+i].x_coord = length * i;
gPatt.world_coord[j*gPatt.h_num+i].y_coord = length * j;
}
}

// Add command-line arguments to vconf string.
strcpy(line, vconf);
for (i = 1; i < argc; i++) {
strcat(line, " ");
strcat(line, argv[i]);
}

// Open the video path.
if (arVideoOpen(line) < 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 clipping image (luminance only).
arMalloc(gClipImage, unsigned char, gXsize * gYsize);

return (TRUE);
}

static void grabImage(void) {
ARUint8 *image;

// Processing a new image.
// Copy an image to saved image buffer.
do {
image = arVideoGetImage();
} while (image == NULL);
gPatt.loop_num++;
if ((gPatt.arglSettings[gPatt.loop_num-1] = arglSetupForCurrentContext()) == NULL) {
fprintf(stderr, "grabImage(): arglSetupForCurrentContext() returned error.\n");
exit(-1);
}
arglDistortionCompensationSet(gPatt.arglSettings[gPatt.loop_num-1], FALSE);
arMalloc((gPatt.savedImage)[gPatt.loop_num-1], unsigned char, gXsize*gYsize*AR_PIX_SIZE_DEFAULT);
memcpy((gPatt.savedImage)[gPatt.loop_num-1], image, gXsize*gYsize*AR_PIX_SIZE_DEFAULT);
printf("Grabbed image %d.\n", gPatt.loop_num);
arMalloc(gPatt.point[gPatt.loop_num-1], CALIB_COORD_T, gPatt.h_num*gPatt.v_num);
}

static void ungrabImage(void) {
if (gPatt.loop_num == 0) {printf("error!!\n"); exit(0);}
free(gPatt.point[gPatt.loop_num-1]);
gPatt.point[gPatt.loop_num-1] = NULL;
free(gPatt.savedImage[gPatt.loop_num-1]);
gPatt.savedImage[gPatt.loop_num-1] = NULL;
arglCleanup(gPatt.arglSettings[gPatt.loop_num-1]);
gPatt.loop_num--;
}

void checkFit(void) {
printf("\n-----------\n");
if (check_num < gPatt.loop_num) {
printf("Checking fit on image %3d of %3d.\n", check_num + 1, gPatt.loop_num);
if (check_num + 1 < gPatt.loop_num) {
printf("Press mouse button to check fit of next image.\n");
} else {
printf("Press mouse button to calculate parameter.\n");
}
glutPostRedisplay();
} else {
if (gPatt.loop_num > 1) {
if (calc_inp(&gPatt, dist_factor, gXsize, gYsize, mat) < 0) {
printf("Calibration failed. Exiting.\n");
exit(0);
} else {
printf("Calibration succeeded. Enter filename to save camera parameter to below.\n");
save_param();
}
} else {
printf("Not enough images to proceed with calibration. Exiting.\n");
}
Quit();
}
}

static void eventCancel(void) {
// What was cancelled?
if (gStatus == 0) {
// Cancelled grabbing.
// Live video will not be needed from here on.
arVideoCapStop();
if (gPatt.loop_num == 0) {
// No images with all features identified, so quit.
Quit();
} else {
// At least one image with all features identified,
// so calculate distortion.
calc_distortion(&gPatt, gXsize, gYsize, dist_factor);
printf("--------------\n");
printf("Center X: %f\n", dist_factor[0]);
printf(" Y: %f\n", dist_factor[1]);
printf("Dist Factor: %f\n", dist_factor[2]);
printf("Size Adjust: %f\n", dist_factor[3]);
printf("--------------\n");
// Distortion calculation done. Check fit.
gStatus = 2;
check_num = 0;
checkFit();
}
} else if (gStatus == 1) {
// Cancelled rubber-bounding.
ungrabImage();
// Restart grabbing.
point_num = 0;
arVideoCapStart();
gStatus = 0;
if (gPatt.loop_num == 0) print_comment(0);
else print_comment(4);
}
}

static void Mouse(int button, int state, int x, int y)
{
unsigned char *p;
int ssx, ssy, eex, eey;
int i, j, k;

if (button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN) {
eventCancel();
} else if (button == GLUT_LEFT_BUTTON) {
if (state == GLUT_DOWN) {
if (gStatus == 0 && gPatt.loop_num < LOOP_MAX) {
// End grabbing.
// Begin waiting for drag for rubber-bounding of a feature.
grabImage();
gDragStartX = gDragStartY = gDragEndX = gDragEndY = -1;
arVideoCapStop();
gStatus = 1;
print_comment(1);
} else if (gStatus == 1) {
if (point_num < gPatt.h_num*gPatt.v_num) {
// Drag for rubber-bounding of a feature has begun.
gDragStartX = gDragEndX = x;
gDragStartY = gDragEndY = y;
} else {
// Feature point locations have been accepted.
printf("### Image no.%d ###\n", gPatt.loop_num);
for( j = 0; j < gPatt.v_num; j++ ) {
for( i = 0; i < gPatt.h_num; i++ ) {
printf("%2d, %2d: %6.2f, %6.2f\n", i+1, j+1,
gPatt.point[gPatt.loop_num-1][j*gPatt.h_num+i].x_coord,
gPatt.point[gPatt.loop_num-1][j*gPatt.h_num+i].y_coord);
}
}
printf("\n\n");
// Restart grabbing.
point_num = 0;
arVideoCapStart();
gStatus = 0;
if (gPatt.loop_num < LOOP_MAX) print_comment(4);
else print_comment(5);
}
} else if (gStatus == 2) {
check_num++;
checkFit();
}
} else if (state == GLUT_UP) {
if (gStatus == 1
&& gDragStartX != -1 && gDragStartY != -1
&& gDragEndX != -1 && gDragEndY != -1
&& point_num < gPatt.h_num*gPatt.v_num) {
// Drag for rubber-bounding of a feature has finished. Begin identification
// of center of white region in gClipImage.
if (gDragStartX < gDragEndX) { ssx = gDragStartX; eex = gDragEndX; }
else { ssx = gDragEndX; eex = gDragStartX; }
if (gDragStartY < gDragEndY) { ssy = gDragStartY; eey = gDragEndY; }
else { ssy = gDragEndY; eey = gDragStartY; }

gPatt.point[gPatt.loop_num-1][point_num].x_coord = 0.0;
gPatt.point[gPatt.loop_num-1][point_num].y_coord = 0.0;
p = gClipImage;
k = 0;
for (j = 0; j < (eey-ssy+1); j++) {
for (i = 0; i < (eex-ssx+1); i++) {
gPatt.point[gPatt.loop_num-1][point_num].x_coord += i * *p;
gPatt.point[gPatt.loop_num-1][point_num].y_coord += j * *p;
k += *p;
p++;
}
}
if (k != 0) {
gPatt.point[gPatt.loop_num-1][point_num].x_coord /= k;
gPatt.point[gPatt.loop_num-1][point_num].y_coord /= k;
gPatt.point[gPatt.loop_num-1][point_num].x_coord += ssx;
gPatt.point[gPatt.loop_num-1][point_num].y_coord += ssy;
point_num++;
printf("Marked feature position %3d of %3d\n", point_num, gPatt.h_num*gPatt.v_num);
if (point_num == gPatt.h_num*gPatt.v_num) print_comment(2);
}
gDragStartX = gDragStartY = gDragEndX = gDragEndY = -1;
glutPostRedisplay();
}
}
}
}

static void Motion(int x, int y)
{
unsigned char *p, *p1;
int ssx, ssy, eex, eey;
int i, j;

// During mouse drag.
if (gStatus == 1 && gDragStartX != -1 && gDragStartY != -1) {

// Set x,y of end of mouse drag region.
gDragEndX = x;
gDragEndY = y;

// Check that start is above and to left of end.
if (gDragStartX < gDragEndX) { ssx = gDragStartX; eex = gDragEndX; }
else { ssx = gDragEndX; eex = gDragStartX; }
if (gDragStartY < gDragEndY) { ssy = gDragStartY; eey = gDragEndY; }
else { ssy = gDragEndY; eey = gDragStartY; }

// Threshold clipping area, copy it into gClipImage.
p1 = gClipImage;
for (j = ssy; j <= eey; j++) {
p = &(gPatt.savedImage[gPatt.loop_num-1][(j*gXsize+ssx)*AR_PIX_SIZE_DEFAULT]);
for (i = ssx; i <= eex; i++) {
#if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGRA)
*p1 = (((255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ABGR)
*p1 = (((255*3 - (*(p+1) + *(p+2) + *(p+3))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ARGB)
*p1 = (((255*3 - (*(p+1) + *(p+2) + *(p+3))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR)
*p1 = (((255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGBA)
*p1 = (((255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB)
*p1 = (((255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_2vuy)
*p1 = ((255 - *(p+1)) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_yuvs)
*p1 = ((255 - *(p+0)) < gThresh ? 0 : 255);
#elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_MONO)
*p1 = ((255 - *(p)) < gThresh ? 0 : 255);
#else
# error Unknown default pixel format defined in config.h
#endif
p += AR_PIX_SIZE_DEFAULT;
p1++;
}
}
// Tell GLUT the display has changed.
glutPostRedisplay();
}
}

static void Keyboard(unsigned char key, int x, int y)
{
switch (key) {
case 0x1B:
eventCancel();
break;
case 'T':
case 't':
printf("Enter new threshold value (now = %d): ", gThresh);
scanf("%d",&gThresh); while( getchar()!='\n' );
printf("\n");
break;
case '1':
gThresh -= 5;
if (gThresh < 0) gThresh = 0;
break;
case '2':
gThresh += 5;
if (gThresh > 255) gThresh = 255;
break;
default:
break;
}
}

static void Quit(void)
{
if (gClipImage) {
free(gClipImage);
gClipImage = NULL;
}
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 (gStatus == 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)
{
double x, y;
int ssx, eex, ssy, eey;
int i;

// 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 (gStatus == 0) {

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

} else if (gStatus == 1) {

arglDispImage(gPatt.savedImage[gPatt.loop_num-1], &gARTCparam, 1.0, gPatt.arglSettings[gPatt.loop_num-1]);
// Draw red crosses on the points in the image.
for (i = 0; i < point_num; i++) {
x = gPatt.point[gPatt.loop_num-1][i].x_coord;
y = gPatt.point[gPatt.loop_num-1][i].y_coord;
glColor3f(1.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glVertex2d(x-10.0, (GLdouble)(gYsize-1)-y);
glVertex2d(x+10.0, (GLdouble)(gYsize-1)-y);
glVertex2d(x, (GLdouble)(gYsize-1)-(y-10.0));
glVertex2d(x, (GLdouble)(gYsize-1)-(y+10.0));
glEnd();
}

// Draw the current mouse drag clipping area.
if (gDragStartX != -1 && gDragStartY != -1
&& gDragEndX != -1 && gDragEndY != -1) {
if (gDragStartX < gDragEndX) { ssx = gDragStartX; eex = gDragEndX; }
else { ssx = gDragEndX; eex = gDragStartX; }
if (gDragStartY < gDragEndY) { ssy = gDragStartY; eey = gDragEndY; }
else { ssy = gDragEndY; eey = gDragStartY; }
#if 1
if (gClipImage) {
glPixelZoom(1.0f, -1.0f); // ARToolKit bitmap 0.0 is at upper-left, OpenGL bitmap 0.0 is at lower-left.
glRasterPos2f((GLfloat)(ssx), (GLfloat)(gYsize-1-ssy));
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glDrawPixels(eex-ssx+1, eey-ssy+1, GL_LUMINANCE, GL_UNSIGNED_BYTE, gClipImage);
}
#else
glColor3f(0.0f, 0.0f, 1.0f);
glBegin(GL_LINE_LOOP);
glVertex2i(gDragStartX, (gYsize-1)-gDragStartY);
glVertex2i(gDragEndX, (gYsize-1)-gDragStartY);
glVertex2i(gDragEndX, (gYsize-1)-gDragEndY);
glVertex2i(gDragStartX, (gYsize-1)-gDragEndY);
glEnd();
#endif
}

} else if (gStatus == 2) {

arglDispImage(gPatt.savedImage[check_num], &gARTCparam, 1.0, gPatt.arglSettings[check_num]);
for (i = 0; i < gPatt.h_num*gPatt.v_num; i++) {
x = gPatt.point[check_num][i].x_coord;
y = gPatt.point[check_num][i].y_coord;
glColor3f(1.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glVertex2d(x-10.0, (GLdouble)(gYsize-1)-y);
glVertex2d(x+10.0, (GLdouble)(gYsize-1)-y);
glVertex2d(x, (GLdouble)(gYsize-1)-(y-10.0));
glVertex2d(x, (GLdouble)(gYsize-1)-(y+10.0));
glEnd();
}
draw_line();

}

endOrtho2D();
glutSwapBuffers();
}

static void draw_line(void)
{
double *x, *y;
int max;
// int num; // unreferenced
int i, j, k, l;
int p;

max = (gPatt.v_num > gPatt.h_num) ? gPatt.v_num : gPatt.h_num;
arMalloc(x, double, max);
arMalloc(y, double, max);

i = check_num;

for (j = 0; j < gPatt.v_num; j++) {
for (k = 0; k < gPatt.h_num; k++) {
x[k] = gPatt.point[i][j*gPatt.h_num+k].x_coord;
y[k] = gPatt.point[i][j*gPatt.h_num+k].y_coord;
}
draw_line2(x, y, gPatt.h_num);
}

for (j = 0; j < gPatt.h_num; j++) {
for (k = 0; k < gPatt.v_num; k++) {
x[k] = gPatt.point[i][k*gPatt.h_num+j].x_coord;
y[k] = gPatt.point[i][k*gPatt.h_num+j].y_coord;
}
draw_line2(x, y, gPatt.v_num);
}

for (j = 3 - gPatt.v_num; j < gPatt.h_num - 2; j++) {
p = 0;
for (k = 0; k < gPatt.v_num; k++) {
l = j+k;
if (l < 0 || l >= gPatt.h_num) continue;
x[p] = gPatt.point[i][k*gPatt.h_num+l].x_coord;
y[p] = gPatt.point[i][k*gPatt.h_num+l].y_coord;
p++;
}
draw_line2(x, y, p);
}

for (j = 2; j < gPatt.h_num + gPatt.v_num - 3; j++) {
p = 0;
for (k = 0; k < gPatt.v_num; k++) {
l = j-k;
if (l < 0 || l >= gPatt.h_num) continue;
x[p] = gPatt.point[i][k*gPatt.h_num+l].x_coord;
y[p] = gPatt.point[i][k*gPatt.h_num+l].y_coord;
p++;
}
draw_line2(x, y, p);
}

free(x);
free(y);
}

static void draw_line2( double *x, double *y, int num )
{
ARMat *input, *evec;
ARVec *ev, *mean;
double a, b, c;
int i;

ev = arVecAlloc(2);
mean = arVecAlloc(2);
evec = arMatrixAlloc(2, 2);

input = arMatrixAlloc(num, 2);
for (i = 0; i < num; i++) {
arParamObserv2Ideal(dist_factor, x[i], y[i],
&(input->m[i*2+0]), &(input->m[i*2+1]));
}
if (arMatrixPCA(input, evec, ev, mean) < 0) exit(0);
a = evec->m[1];
b = -evec->m[0];
c = -(a*mean->v[0] + b*mean->v[1]);

arMatrixFree(input);
arMatrixFree(evec);
arVecFree(mean);
arVecFree(ev);

draw_warp_line(a, b, c);
}

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

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 print_comment(int status)
{
printf("\n-----------\n");
switch(status) {
case 0:
printf("Press mouse button to grab first image,\n");
printf("or press right mouse button or [esc] to quit.\n");
break;
case 1:
printf("Press mouse button and drag mouse to rubber-bound features (%d x %d),\n", gPatt.h_num, gPatt.v_num);
printf("or press right mouse button or [esc] to cancel rubber-bounding & retry grabbing.\n");
break;
case 2:
printf("Press mouse button to save feature positions,\n");
printf("or press right mouse button or [esc] to discard feature positions & retry grabbing.\n");
break;
case 4:
printf("Press mouse button to grab next image,\n");
printf("or press right mouse button or [esc] to calculate distortion parameter.\n");
break;
case 5:
printf("Press right mouse button or [esc] to calculate distortion parameter.\n");
break;
}
}

static void save_param(void)
{
char name[256];
ARParam param;
int i, j;

param.xsize = gXsize;
param.ysize = gYsize;
for (i = 0; i < 4; i++) param.dist_factor[i] = dist_factor[i];
for (j = 0; j < 3; j++) {
for (i = 0; i < 4; i++) {
param.mat[j][i] = mat[j][i];
}
}
arParamDisp( &param );

printf("Filename: ");
scanf( "%s", name );
arParamSave( name, 1, &param );

return;
}

Subversion Repositories AndroidProjects

(root)/ARToolKit/util/calib_camera2/main.c - Rev 204