static void draw_warp_line( double a, double b , double c )
{
ARdouble x, y;
ARdouble x0 = 0, y0 = 0;
ARdouble x1, y1;
int i;
glLineWidth( 1.0f );
glBegin(GL_LINE_STRIP);
if( a*a >= b*b ) {
for( i = -20; i <= ysize+20; i+=10 ) {
x = -(b*i + c)/a;
y = i;
arParamIdeal2Observ( dist_factor, x, y, &x1, &y1, dist_function_version );
if( i != -20 ) {
argDrawLineByObservedPos( x0, y0, x1, y1 );
}
x0 = x1;
y0 = y1;
}
}
else {
for( i = -20; i <= xsize+20; i+=10 ) {
x = i;
y = -(a*i + c)/b;
arParamIdeal2Observ( dist_factor, x, y, &x1, &y1, dist_function_version );
if( i != -20 ) {
argDrawLineByObservedPos( x0, y0, x1, y1 );
}
x0 = x1;
y0 = y1;
}
}
glEnd();
}
static void drawFeatureRect( int x, int y, int ts1, int ts2 )
{
glColor3f( 1.0f, 0.0f, 0.0f );
glLineWidth( 2.0f );
argDrawLineByObservedPos( x-ts1, y-ts1, x+ts2, y-ts1 );
argDrawLineByObservedPos( x+ts2, y-ts1, x+ts2, y+ts2 );
argDrawLineByObservedPos( x+ts2, y+ts2, x-ts1, y+ts2 );
argDrawLineByObservedPos( x-ts1, y+ts2, x-ts1, y-ts1 );
}
static void dispFunc( void )
{
int x, y;
int i, j;
char str[32];
glClear( GL_COLOR_BUFFER_BIT );
#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
argViewportSetPixFormat( vp[page/AR2_BLUR_IMAGE_MAX], AR_PIXEL_FORMAT_MONO );
argDrawMode2D( vp[page/AR2_BLUR_IMAGE_MAX] );
argDrawImage( imageSet->scale[page/AR2_BLUR_IMAGE_MAX]->imgBWBlur[page%AR2_BLUR_IMAGE_MAX] );
#else
argViewportSetPixFormat( vp[page], AR_PIXEL_FORMAT_MONO );
argDrawMode2D( vp[page] );
argDrawImage( imageSet->scale[page]->imgBW );
#endif
if (display_fset) {
for( i = 0; i < featureSet->list[page].num; i++ ) {
x = featureSet->list[page].coord[i].x;
y = featureSet->list[page].coord[i].y;
drawFeatureRect( x, y, AR2_DEFAULT_TS1, AR2_DEFAULT_TS2 );
sprintf(str, "%d", i);
glColor3f( 0.0f, 0.0f, 1.0f );
argDrawStringsByObservedPos(str, x, y);
}
ARLOG("fset: Num of feature points: %d\n", featureSet->list[page].num);
}
if (display_fset2) {
for( i = j = 0; i < refDataSet->num; i++ ) {
if( refDataSet->refPoint[i].refImageNo != page ) continue;
x = refDataSet->refPoint[i].coord2D.x;
y = refDataSet->refPoint[i].coord2D.y;
glColor3f( 0.0f, 1.0f, 0.0f );
argDrawLineByObservedPos(x-5, y-5, x+5, y+5);
argDrawLineByObservedPos(x+5, y-5, x-5, y+5);
j++;
}
ARLOG("fset2: Num of feature points: %d\n", j);
#if 0
for (i = 0; i < refDataSet->pageNum; i++) {
for (j = 0; j < refDataSet->pageInfo[i].imageNum; j++) {
if (refDataSet->pageInfo[i].imageInfo[j].imageNo == page) {
ARLOG("fset2: Image size: %dx%d\n", refDataSet->pageInfo[i].imageInfo[j].width, refDataSet->pageInfo[i].imageInfo[j].height);
}
}
}
#endif
}
argSwapBuffers();
}
static void dispImage(void)
{
AR2VideoBufferT *buff;
double x, y;
int ssx, eex, ssy, eey;
int i;
if( status == 0 ) {
while (!(buff = arVideoGetImage()) || !buff->fillFlag) arUtilSleep(2);
argDrawMode2D( vp );
argDrawImage(buff->buff);
}
else if( status == 1 ) {
argDrawMode2D( vp );
argDrawImage( patt.savedImage[patt.loop_num-1] );
for( i = 0; i < point_num; i++ ) {
x = patt.point[patt.loop_num-1][i].x_coord;
y = patt.point[patt.loop_num-1][i].y_coord;
glColor3f( 1.0f, 0.0f, 0.0f );
argDrawLineByObservedPos( x-10, y, x+10, y );
argDrawLineByObservedPos( x, y-10, x, y+10 );
}
if( sx != -1 && sy != -1 ) {
if( sx < ex ) { ssx = sx; eex = ex; }
else { ssx = ex; eex = sx; }
if( sy < ey ) { ssy = sy; eey = ey; }
else { ssy = ey; eey = sy; }
dispClipImage( ssx, ssy, eex-ssx+1, eey-ssy+1, clipImage );
}
}
else if( status == 2 ) {
argDrawMode2D( vp );
argDrawImage( patt.savedImage[check_num] );
for( i = 0; i < patt.h_num*patt.v_num; i++ ) {
x = patt.point[check_num][i].x_coord;
y = patt.point[check_num][i].y_coord;
glColor3f( 1.0f, 0.0f, 0.0f );
argDrawLineByObservedPos( x-10, y, x+10, y );
argDrawLineByObservedPos( x, y-10, x, y+10 );
}
draw_line();
}
argSwapBuffers();
}
static void mainLoop(void)
{
ARUint8 *dataPtr;
int cornerCount;
char buf[256];
int i;
if ((dataPtr = arVideoGetImage()) == NULL)
{
arUtilSleep(2);
return;
}
glClear(GL_COLOR_BUFFER_BIT);
argDrawMode2D(vp);
argDrawImage(dataPtr);
// Convert to grayscale, results will go to arIPI->image, which also provides the backing for calibImage.
arImageProcLuma(arIPI, dataPtr);
cornerFlag = cvFindChessboardCorners(calibImage, cvSize(chessboardCornerNumY, chessboardCornerNumX),
corners, &cornerCount, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
if (cornerFlag)
glColor4ub(255, 0, 0, 255);
else
glColor4ub(0, 255, 0, 255);
glLineWidth(2.0f);
// ARLOG("Detected corners = %d\n", cornerCount);
for (i = 0; i < cornerCount; i++)
{
argDrawLineByObservedPos(corners[i].x - 5, corners[i].y - 5, corners[i].x + 5, corners[i].y + 5);
argDrawLineByObservedPos(corners[i].x - 5, corners[i].y + 5, corners[i].x + 5, corners[i].y - 5);
// ARLOG(" %f, %f\n", corners[i].x, corners[i].y);
sprintf(buf, "%d\n", i);
argDrawStringsByObservedPos(buf, corners[i].x, corners[i].y + 20);
}
sprintf(buf, "Captured Image: %2d/%2d\n", capturedImageNum, calibImageNum);
argDrawStringsByObservedPos(buf, 10, 30);
argSwapBuffers();
}
static void mainLoop( void )
{
AR2VideoBufferT *videoBuffL;
AR2VideoBufferT *videoBuffR;
ARUint8 *dataPtrL;
ARUint8 *dataPtrR;
int cornerFlagL;
int cornerFlagR;
int cornerCountL;
int cornerCountR;
char buf[256];
int i;
if ((videoBuffL = ar2VideoGetImage(vidL))) {
gVideoBuffL = videoBuffL;
}
if ((videoBuffR = ar2VideoGetImage(vidR))) {
gVideoBuffR = videoBuffR;
}
if (gVideoBuffL && gVideoBuffR) {
// Warn about significant time differences.
i = ((int)gVideoBuffR->time_sec - (int)gVideoBuffL->time_sec) * 1000
+ ((int)gVideoBuffR->time_usec - (int)gVideoBuffL->time_usec) / 1000;
if( i > 20 ) {
ARLOG("Time diff = %d[msec]\n", i);
} else if( i < -20 ) {
ARLOG("Time diff = %d[msec]\n", i);
}
dataPtrL = gVideoBuffL->buff;
dataPtrR = gVideoBuffR->buff;
glClear(GL_COLOR_BUFFER_BIT);
argDrawMode2D( vpL );
argDrawImage( dataPtrL );
argDrawMode2D( vpR );
argDrawImage( dataPtrR );
copyImage( dataPtrL, (ARUint8 *)calibImageL->imageData, xsizeL*ysizeL, pixFormatL );
cornerFlagL = cvFindChessboardCorners(calibImageL, cvSize(chessboardCornerNumY,chessboardCornerNumX),
cornersL, &cornerCountL, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS );
copyImage( dataPtrR, (ARUint8 *)calibImageR->imageData, xsizeR*ysizeR, pixFormatR );
cornerFlagR = cvFindChessboardCorners(calibImageR, cvSize(chessboardCornerNumY,chessboardCornerNumX),
cornersR, &cornerCountR, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS );
argDrawMode2D( vpL );
if(cornerFlagL) glColor3f(1.0f, 0.0f, 0.0f);
else glColor3f(0.0f, 1.0f, 0.0f);
glLineWidth(2.0f);
//ARLOG("Detected corners = %d\n", cornerCount);
for( i = 0; i < cornerCountL; i++ ) {
argDrawLineByObservedPos(cornersL[i].x-5, cornersL[i].y-5, cornersL[i].x+5, cornersL[i].y+5);
argDrawLineByObservedPos(cornersL[i].x-5, cornersL[i].y+5, cornersL[i].x+5, cornersL[i].y-5);
//ARLOG(" %f, %f\n", cornersL[i].x, cornersL[i].y);
sprintf(buf, "%d\n", i);
argDrawStringsByObservedPos(buf, cornersL[i].x, cornersL[i].y+20);
}
argDrawMode2D( vpR );
if(cornerFlagR) glColor3f(1.0f, 0.0f, 0.0f);
else glColor3f(0.0f, 1.0f, 0.0f);
glLineWidth(2.0f);
//ARLOG("Detected corners = %d\n", cornerCount);
for( i = 0; i < cornerCountR; i++ ) {
argDrawLineByObservedPos(cornersR[i].x-5, cornersR[i].y-5, cornersR[i].x+5, cornersR[i].y+5);
argDrawLineByObservedPos(cornersR[i].x-5, cornersR[i].y+5, cornersR[i].x+5, cornersR[i].y-5);
//ARLOG(" %f, %f\n", cornersR[i].x, cornersR[i].y);
sprintf(buf, "%d\n", i);
argDrawStringsByObservedPos(buf, cornersR[i].x, cornersR[i].y+20);
}
if( cornerFlagL && cornerFlagR ) {
cornerFlag = 1;
glColor3f(1.0f, 0.0f, 0.0f);
}
else {
cornerFlag = 0;
glColor3f(0.0f, 1.0f, 0.0f);
}
argDrawMode2D( vpL );
sprintf(buf, "Captured Image: %2d/%2d\n", capturedImageNum, calibImageNum);
argDrawStringsByIdealPos(buf, 10, 30);
argSwapBuffers();
gVideoBuffL = gVideoBuffR = NULL;
} else arUtilSleep(2);
}
