artvertiser/artvertiser.cpp

   1 /*
   2  * Credits: Julian Oliver, 2008-2009 <julian@julianoliver.com>.
   3  *
   4  * This program is free software: you can redistribute it and/or modify it
   5  * under the terms of the GNU General Public License as published by the Free
   6  * Software Foundation, either version 3 of the License, or (at your option)
   7  * any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program.  If not, see <http://www.gnu.org/licenses/>.
  16  *
  17  * This code builds upon BazAR, in particular 'multigl.cpp'. It has been
  18  * modified to support texture and video-texture mapping to an OpenGL plane over
  19  * the ROI. The ROI in the model image is now read in from a file generated by
  20  * the training process. Pose estimation stabilisation, augmentation fades,
  21  * fonts, mouse input hooks, augmentation over archival video and other bits have
  22  * pieces have been added also.
  23  *
  24  * I've fixed a bug in BazAR's planar_object_recognizer::build_with_cache where
  25  * corner values for the ROI were only being set immediately after training, not
  26  * on plain init.
  27  *
  28  * Usage:
  29  *
  30  * There are four ways to use Artvertiser.
  31  *
  32  * With video substitution of the ROI:
  33  *
  34  *       ./artvertiser -m <model file> -a <avi file>
  35  *
  36  * With video substitution of the ROI and capture from an AVI file
  37  *
  38  *  ./artvertiser -m <model file> -a <avi file> -b <avi file>
  39  *
  40  * With image substitution of the ROI and capture from a v4l device
  41  *
  42  *      ./artvertiser -m <model file>
  43  *
  44  * See defines below for setting capture window size and V4L device index
  45  *
  46  */
  47
  48 #include <iostream>
  49 #include <sstream> // for conv int->str
  50 #include <vector>
  51 #include <cv.h>
  52 #include <highgui.h>
  53 #include <map>
  54
  55 #include <stdio.h>
  56 #include <time.h>
  57
  58
  59 #ifdef HAVE_CONFIG_H
  60 #include <config.h>
  61 #endif
  62
  63 #include <calib/camera.h>
  64 #include "multigrab.h"
  65
  66 #ifdef HAVE_APPLE_OPENGL_FRAMEWORK
  67 #include <GLUT/glut.h>
  68 #else
  69 #include <GL/glut.h>
  70 #endif
  71
  72 #include "/usr/include/freetype2/freetype/config/ftconfig.h"
  73 #include <FTGL/ftgl.h>
  74
  75 #include "FProfiler/FProfiler.h"
  76
  77 // framerate counter
  78 #include "framerate.h"
  79
  80
  81 #define IsRGB(s) ((s[0] == 'R') && (s[1] == 'G') && (s[2] == 'B'))
  82 #define IsBGR(s) ((s[0] == 'B') && (s[1] == 'G') && (s[2] == 'R'))
  83
  84 #ifndef GL_CLAMP_TO_BORDER
  85 #define GL_CLAMP_TO_BORDER 0x812D
  86 #endif
  87 #define GL_MIRROR_CLAMP_EXT 0x8742
  88
  89 #define WIDTH 640
  90 #define HEIGHT 480
  91 #define V4LDEVICE 1
  92
  93 #define NUMARTVERTS 5
  94
  95 //#define WIDTH 320
  96 //#define HEIGHT 240
  97
  98 MultiGrab *multi=0;
  99 CamCalibration *calib=0;
 100 CvPoint projPts[4];
 101 IplTexture *frameTexture=0;
 102 IplTexture *tex=0;
 103 IplImage *image = 0;
 104 CvCapture *capture = 0;
 105 CvCapture *avi_capture = 0;
 106 IplImage *avi_image = 0;
 107 IplImage *avi_frame = 0;
 108 IplImage *model_image = 0;
 109 IplImage *this_frame = 0;
 110 IplImage *last_frame  = 0;
 111 IplImage *diff= 0;
 112 IplImage *bit_frame= 0;
 113
 114 int v4l_device = V4LDEVICE;
 115 int video_width = WIDTH;
 116 int video_height = HEIGHT;
 117 int detect_width = WIDTH;
 118 int detect_height = HEIGHT;
 119
 120 // load some images. hard-coded for know until i get the path parsing together.
 121 IplImage *image1 = cvLoadImage("artvert1.png");
 122 IplImage *image2 = cvLoadImage("artvert2.png");
 123 IplImage *image3 = cvLoadImage("artvert3.png");
 124 IplImage *image4 = cvLoadImage("artvert4.png");
 125 IplImage *image5 = cvLoadImage("artvert5.png");
 126
 127 // define a container struct for each artvert
 128 struct artvert_struct
 129 {
 130     const char *artvert;
 131     IplImage *image;
 132     const char *date;
 133     const char *author;
 134     const char *advert;
 135     const char *street;
 136 };
 137
 138 typedef vector<artvert_struct> artverts_list;
 139 artverts_list artverts(5);
 140
 141 // create a vector for the images and initialise it.
 142 typedef vector<IplImage> imgVec;
 143
 144 bool frame_ok=false;
 145 bool cache_light=false;
 146 bool dynamic_light=false;
 147 bool sphere_object=false;
 148 bool avi_play=false;
 149 bool avi_play_init=false;
 150 bool lbutton_down = false;
 151 bool label = false;
 152
 153 double a_proj[3][4];
 154 double old_a_proj[3][4];
 155 float fade = 0.0;
 156 float fade_speed = 2;
 157 int difference = 0;
 158 int have_proj = 0;
 159 int nb_light_measures=0;
 160 int geom_calib_nb_homography;
 161 int current_cam = 0;
 162 int avi_init = 0;
 163 int found_match = 0;
 164 int augment = 0;
 165 int cnt=0;
 166
 167 vector<int> roi_vec;
 168 CvPoint2D32f *c1 = new CvPoint2D32f[4];
 169
 170 char *image_path;
 171 char *model_file = "model.bmp";
 172
 173 static void start();
 174 static void geomCalibStart(bool cache);
 175
 176 // initialise a couple of fonts.
 177 CvFont font, fontbold;
 178
 179 // Gl format for texturing
 180 GLenum format;
 181 GLuint imageID;
 182
 183 // ftgl font setup
 184 static FTFont *ftglFont;
 185
 186 // interface
 187 bool show_status = false;
 188 bool show_profile_results = false;
 189 string status_string = "";
 190
 191 /* use this to read paths from the file system
 192
 193    string getExtension(const string &file)
 194    {
 195    string::size_type dot = file.rfind('.');
 196    string lcpath = file;
 197    string suffix;
 198    transform(lcpath.begin(), lcpath.end(), lcpath.begin(), tolower);
 199    if(dot != string::npos)
 200    {
 201    suffix = lcpath.substr(dot + 1);
 202    }
 203    return suffix;
 204    }
 205  */
 206
 207 std::string date(int now)
 208 {
 209     time_t rawtime;
 210     struct tm *timeinfo;
 211     char tBuffer[80];
 212     time ( &rawtime );
 213     timeinfo = localtime ( &rawtime );
 214     strftime (tBuffer,80,"%I:%M:%S:%p, %d %b %Y",timeinfo);
 215     string timeStr;
 216     stringstream _timeStr;
 217     _timeStr << tBuffer;
 218     timeStr = _timeStr.str();
 219     return timeStr;
 220 }
 221
 222
 223 // mouse input using GLUT.
 224 void entry(int state)
 225 {
 226     if (state == GLUT_ENTERED)
 227         cout << "Mouse Entered" << endl;
 228     else
 229         cout << "Mouse Left" << endl;
 230 }
 231
 232 void mouse(int button, int state, int x, int y)
 233 {
 234     if (button == GLUT_RIGHT_BUTTON)
 235     {
 236         if (state == GLUT_DOWN)
 237         {
 238             label = true;
 239         }
 240
 241         else
 242         {
 243             label = false;
 244         }
 245     }
 246     else if (button == GLUT_LEFT_BUTTON)
 247     {
 248         if (state == GLUT_DOWN)
 249         {
 250             lbutton_down = true;
 251             if (cnt >= NUMARTVERTS-1)
 252             {
 253                 cnt = 0;
 254             }
 255             else
 256             {
 257                 cnt ++;
 258             }
 259         }
 260         else
 261             lbutton_down = false;
 262     }
 263     cout << "we are on image " << cnt << endl;
 264 }
 265
 266 // text drawing function
 267 static void drawText(IplImage *img, const char *text, CvPoint point, CvFont *font, CvScalar colour, double size)
 268 {
 269     cvInitFont( font, CV_FONT_HERSHEY_DUPLEX, size, size, 0, 1, CV_AA);
 270     //cvInitFont( font, CV_FONT_HERSHEY_PLAIN, size, size, 0, 1, CV_AA);
 271     cvPutText(img, text, point, font, colour);
 272 }
 273
 274 // read in ROI coords from txt file into vector.
 275 static vector<int>  readROI(const char *filename)
 276 {
 277     cout << filename << endl;
 278     string l;
 279     ifstream roi(filename);
 280     vector<int> v;
 281     int coord;
 282     char s[10];
 283     char *s1;
 284     int lines = 0;
 285
 286     if (roi.is_open())
 287     {
 288         while (!roi.eof())
 289         {
 290             getline(roi, l);
 291             strcpy(s, l.c_str());
 292             s1 = strtok(s, " ");
 293
 294             while (s1 != NULL)
 295             {
 296                 //roi_vec.push_back(atoi(s1));
 297                 v.push_back(atoi(s1));
 298                 s1 = strtok(NULL, " ,");
 299             }
 300         }
 301         roi.close();
 302     }
 303     else
 304     {
 305         cout << "roi file not found" << endl;
 306     }
 307     return v;
 308 }
 309
 310 //! GLUT callback on window size change
 311 static void reshape(int width, int height)
 312 {
 313     //GLfloat h = (GLfloat) height / (GLfloat) width;
 314     int winWidth  = video_width;
 315     int winHeight = video_height;
 316     glViewport(0,0,winWidth, winHeight);
 317     glutPostRedisplay();
 318 }
 319
 320 //! Print a command line help and exit.
 321 static void usage(const char *s)
 322 {
 323     cerr << "usage:\n" << s
 324          << "[-m <model image>] [-r]\n"
 325          //"   -a <path>  specify path to AVI (instead of v4l device)\n"
 326          "   -b <path>  specify path to AVI (instead of v4l device) MUST ALSO SPECIFY -vs"
 327          "   -m specifies model image\n"
 328          "   -r do not load any data\n"
 329          "   -t train a new classifier\n"
 330          "   -g recompute geometric calibration\n"
 331          "   -a <path>  load an AVI movie as an artvert\n"
 332          "   -l rebuild irradiance map from scratch\n"
 333          "   -vd <num>  V4L video device number (0-n)\n"
 334          "   -vs <width> <height>  video width and height (default 640x480)\n"
 335          "   -ds <width> <height>  frame size at which to run the detector (default to video width/height)\n";
 336     exit(1);
 337 }
 338
 339
 340
 341 void exit_handler()
 342 {
 343     printf("in exit_handler\n");
 344     // shutdown the capture
 345     if ( multi )
 346         delete multi;
 347 }
 348
 349
 350 /*!\brief Initialize everything
 351  *
 352  * - Parse the command line
 353  * - Initialize all the cameras
 354  * - Load or interactively build a model, with its classifier.
 355  * - Set the GLUT callbacks for geometric calibration or, if already done, for photometric calibration.
 356  */
 357
 358 static bool init( int argc, char** argv )
 359 {
 360     // register exit function
 361     atexit( &exit_handler );
 362
 363     // more from before init should be moved here
 364     bool redo_geom=false;
 365     bool redo_training=false;
 366     bool redo_lighting=false;
 367     char *avi_bg_path="";
 368     bool got_ds = false;
 369     bool video_source_is_avi = false;
 370
 371     // parse command line
 372     for (int i=1; i<argc; i++)
 373     {
 374         if (strcmp(argv[i], "-m") ==0)
 375         {
 376             if (i==argc-1) usage(argv[0]);
 377             model_file = argv[i+1];
 378             i++;
 379         }
 380         else if (strcmp(argv[i], "-r")==0)
 381         {
 382             redo_geom=redo_training=redo_lighting=true;
 383         }
 384         else if (strcmp(argv[i], "-g")==0)
 385         {
 386             redo_geom=true;
 387         }
 388         else if (strcmp(argv[i], "-l")==0)
 389         {
 390             redo_lighting=true;
 391         }
 392         else if (strcmp(argv[i], "-t")==0)
 393         {
 394             redo_training=true;
 395         }
 396         else if (strcmp(argv[i], "-a")==0)
 397         {
 398             avi_capture=cvCaptureFromAVI(argv[i+1]);
 399             avi_play=true;
 400         }
 401         else if (strcmp(argv[i], "-b")==0)
 402         {
 403             video_source_is_avi = true;
 404             avi_bg_path=argv[i+1];
 405             printf(" -b: loading from avi '%s'\n", avi_bg_path );
 406         }
 407         else if (strcmp(argv[i], "-i")==0)
 408         {
 409             image_path=argv[i+1];
 410         }
 411         else if (strcmp(argv[i], "-vd")==0)
 412         {
 413             v4l_device=atoi(argv[i+1]);
 414             printf(" -vd: using v4l device %i\n", v4l_device);
 415             i++;
 416         }
 417         else if (strcmp(argv[i], "-vs")==0)
 418         {
 419             video_width=atoi(argv[i+1]);
 420             video_height=atoi(argv[i+2]);
 421             if ( !got_ds )
 422             {
 423                 // also set detect size (if not already set)
 424                 detect_width = video_width;
 425                 detect_height = video_height;
 426             }
 427             printf(" -vs: video size is %ix%i\n", video_width, video_height );
 428             if ( video_width == 0 || video_height == 0 )
 429             {
 430                 usage(argv[0]);
 431                 exit(1);
 432             }
 433             i+=2;
 434         }
 435         else if ( strcmp(argv[i], "-ds")==0 )
 436         {
 437             detect_width = atoi(argv[i+1]);
 438             detect_height = atoi(argv[i+2]);
 439             printf(" -ds: detection frame size is %ix%i\n", detect_width, detect_height );
 440             if ( detect_width == 0 || detect_height == 0 )
 441             {
 442                 usage(argv[0]);
 443                 exit(1);
 444             }
 445             got_ds = true;
 446             i+=2;
 447         }
 448         else if (argv[i][0]=='-')
 449         {
 450             usage(argv[0]);
 451         }
 452
 453     }
 454
 455     // check for video size arg if necessary
 456     if ( video_source_is_avi )
 457     {
 458         // try to read from video
 459         CvCapture* temp_cap = cvCaptureFromAVI(avi_bg_path);
 460         video_width = (int)cvGetCaptureProperty( temp_cap, CV_CAP_PROP_FRAME_WIDTH );
 461         video_height = (int)cvGetCaptureProperty( temp_cap, CV_CAP_PROP_FRAME_HEIGHT );
 462         printf(" -b: read video width/height %i/%i from avi (ignoring -vs)\n", video_width, video_height );
 463         if ( !got_ds )
 464         {
 465             detect_width = video_width;
 466             detect_height = video_height;
 467         }
 468         cvReleaseCapture(&temp_cap);
 469     }
 470
 471     //cout << avi_bg_path << endl;
 472     cache_light = !redo_lighting;
 473
 474     multi = new MultiGrab(model_file);
 475
 476     if( multi->init(!redo_training, model_file, avi_bg_path, video_width, video_height, v4l_device, detect_width, detect_height ) ==0)
 477     {
 478         cerr <<"Initialization error.\n";
 479         return false;
 480     }
 481
 482     geomCalibStart(!redo_geom);
 483
 484     artvert_struct artvert1 = {"Arrebato, 1980", image1, "Feb, 2009", "Iván Zulueta", "Polo", "Madrid, Spain"};
 485     artvert_struct artvert2 = {"name2", image2, "2008", "simon innings", "Helmut Lang", "Parlance Avenue"};
 486     artvert_struct artvert3 = {"name3", image3, "2008", "simon innings", "Loreal", "Parlance Avenue"};
 487     artvert_struct artvert4 = {"name4", image4, "2008", "simon innings", "Hugo Boss", "Parlance Avenue"};
 488     artvert_struct artvert5 = {"name5", image5, "2008", "simon innings", "Burger King", "Parlance Avenue"};
 489
 490     artverts[0] = artvert1;
 491     artverts[1] = artvert2;
 492     artverts[2] = artvert3;
 493     artverts[3] = artvert4;
 494     artverts[4] = artvert5;
 495
 496     // copy char model_file before munging with strcat
 497     char s[strlen(model_file)];
 498     strcpy (s, model_file);
 499     strcat(s, ".roi");
 500     roi_vec = readROI(s);
 501
 502     // load model_image for use with diffing, later
 503     model_image = cvLoadImage(model_file);
 504
 505     c1[0].x = roi_vec[0];
 506     c1[0].y = roi_vec[1];
 507     c1[1].x = roi_vec[2];
 508     c1[1].y = roi_vec[3];
 509     c1[2].x = roi_vec[4];
 510     c1[2].y = roi_vec[5];
 511     c1[3].x = roi_vec[6];
 512     c1[3].y = roi_vec[7];
 513
 514     return true;
 515 }
 516
 517 /*! The keyboard callback: reacts to '+' and '-' to change the viewed cam, 'q' exits.
 518  * 'd' turns on/off the dynamic lightmap update.
 519  * 'f' goes fullscreen.
 520  */
 521 static void keyboard(unsigned char c, int x, int y)
 522 {
 523     switch (c)
 524     {
 525     case 'n' :
 526         if (augment == 1)
 527             augment = 0;
 528         else
 529             augment = 1;
 530     case '+' :
 531         if (current_cam < multi->cams.size()-1)
 532             current_cam++;
 533         break;
 534     case '-':
 535         if (current_cam >= 1)
 536             current_cam--;
 537         break;
 538     case 'q':
 539     case 27 /*esc*/:
 540         exit(0);
 541         break;
 542     case 'd':
 543         dynamic_light = !dynamic_light;
 544         break;
 545     case 'a':
 546         if (avi_play == true)
 547             avi_play = false;
 548         else
 549             avi_play = true;
 550     case 'f':
 551         glutFullScreen();
 552         break;
 553     case 'S':
 554         show_status = !show_status;
 555         break;
 556     case 'p':
 557         show_profile_results = true;
 558         break;
 559     case 'P':
 560         FProfiler::Clear();
 561         break;
 562     case 'i':
 563         if (cnt >= NUMARTVERTS-1)
 564             cnt = 0;
 565         else
 566             cnt ++;
 567         cout << "we are on image " << cnt << endl;
 568         break;
 569     default:
 570         break;
 571     }
 572
 573     if ( multi && show_status )
 574     {
 575         planar_object_recognizer &detector(multi->cams[current_cam]->detector);
 576         switch (c)
 577         {
 578         // detector settings
 579         case '1':
 580             detector.ransac_dist_threshold*=1.02f;
 581             break;
 582         case '!':
 583             detector.ransac_dist_threshold/=1.02f;
 584             break;
 585         case '2':
 586             detector.ransac_stop_support++;
 587             break;
 588         case '@':
 589             detector.ransac_stop_support--;
 590             break;
 591         case '3':
 592             detector.max_ransac_iterations+=10;
 593             break;
 594         case '#':
 595             detector.max_ransac_iterations-=10;
 596             break;
 597         case '4':
 598             detector.non_linear_refine_threshold*=1.02f;
 599             break;
 600         case '$':
 601             detector.non_linear_refine_threshold/=1.02f;
 602             break;
 603         case '5':
 604             detector.match_score_threshold*=1.02f;
 605             break;
 606         case '%':
 607             detector.match_score_threshold/=1.02f;
 608             break;
 609         case '6':
 610             detector.min_view_rate*=1.02f;
 611             break;
 612         case '^':
 613             detector.min_view_rate*=1.02f;
 614             break;
 615         case '7':
 616             detector.point_detector_tau++;
 617             break;
 618         case '&':
 619             detector.point_detector_tau--;
 620             break;
 621         default:
 622             break;
 623         }
 624     }
 625     glutPostRedisplay();
 626 }
 627
 628 static void emptyWindow()
 629 {
 630     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 631 }
 632
 633 int main(int argc, char *argv[])
 634 {
 635     glutInit(&argc, argv);
 636     glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
 637     glutInitWindowSize(video_width,video_height); // hard set the init window size
 638     //glutInitWindowSize(800,450); // hard set the init window size
 639
 640
 641     glutDisplayFunc(emptyWindow);
 642     glutReshapeFunc(reshape);
 643     glutCreateWindow("The Artvertiser 0.4");
 644     glutMouseFunc(mouse);
 645     glutEntryFunc(entry);
 646
 647     if (!init(argc,argv)) return -1;
 648
 649
 650     //ftglFont = new FTBufferFont("/usr/share/fonts/truetype/freefont/FreeMono.ttf");
 651     ftglFont = new FTBufferFont("fonts/FreeSans.ttf");
 652     ftglFont->FaceSize(12);
 653     ftglFont->CharMap(ft_encoding_unicode);
 654
 655     cvDestroyAllWindows();
 656
 657     glutKeyboardFunc(keyboard);
 658     glutMainLoop();
 659     return 0; /* ANSI C requires main to return int. */
 660 }
 661
 662 //!\brief  Draw a frame contained in an IplTexture object on an OpenGL viewport.
 663 static bool drawBackground(IplTexture *tex)
 664 {
 665     if (!tex || !tex->getIm()) return false;
 666
 667     IplImage *im = tex->getIm();
 668     int w = im->width-1;
 669     int h = im->height-1;
 670
 671     glMatrixMode(GL_PROJECTION);
 672     glLoadIdentity();
 673     glMatrixMode(GL_MODELVIEW);
 674     glLoadIdentity();
 675
 676     glDisable(GL_BLEND);
 677     glDisable(GL_DEPTH_TEST);
 678
 679     tex->loadTexture();
 680
 681     glBegin(GL_QUADS);
 682     glColor4f(1,1,1,1);
 683
 684     glTexCoord2f(tex->u(0), tex->v(0));
 685     glVertex2f(-1, 1);
 686     glTexCoord2f(tex->u(w), tex->v(0));
 687     glVertex2f(1, 1);
 688     glTexCoord2f(tex->u(w), tex->v(h));
 689     glVertex2f(1, -1);
 690     glTexCoord2f(tex->u(0), tex->v(h));
 691     glVertex2f(-1, -1);
 692     glEnd();
 693
 694     tex->disableTexture();
 695
 696     return true;
 697 }
 698
 699 /*! \brief A draw callback during camera calibration
 700  *
 701  * GLUT calls that function during camera calibration when repainting the
 702  * window is required.
 703  * During geometric calibration, no 3D is known: we just plot 2d points
 704  * where some feature points have been recognized.
 705  */
 706 static void geomCalibDraw(void)
 707 {
 708     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 709
 710     glDisable(GL_LIGHTING);
 711     drawBackground(frameTexture);
 712
 713     if (!multi) return;
 714
 715     IplImage *im = multi->cams[current_cam]->frame;
 716     planar_object_recognizer &detector(multi->cams[current_cam]->detector);
 717     if (!im) return;
 718
 719     glMatrixMode(GL_PROJECTION);
 720     glLoadIdentity();
 721     glOrtho(0, im->width-1, im->height-1, 0, -1, 1);
 722
 723     glMatrixMode(GL_MODELVIEW);
 724     glLoadIdentity();
 725
 726     glDisable(GL_BLEND);
 727     glDisable(GL_LIGHTING);
 728     glDisable(GL_DEPTH_TEST);
 729
 730     if (detector.object_is_detected)
 731     {
 732         glPointSize(2);
 733         glBegin(GL_POINTS);
 734         glColor4f(0,1,0,1);
 735         for (int i=0; i<detector.match_number; ++i)
 736         {
 737             image_object_point_match * match = detector.matches+i;
 738             if (match->inlier)
 739             {
 740                 int s = (int)(match->image_point->scale);
 741                 float x=PyrImage::convCoordf(match->image_point->u, s, 0);
 742                 float y=PyrImage::convCoordf(match->image_point->v, s, 0);
 743                 glVertex2f(x,y);
 744             }
 745         }
 746         glEnd();
 747     }
 748
 749     glutSwapBuffers();
 750 }
 751
 752 /*!\brief Called when geometric calibration ends. It makes
 753  * sure that the CamAugmentation object is ready to work.
 754  */
 755 static void geomCalibEnd()
 756 {
 757
 758     if (!multi->model.augm.LoadOptimalStructureFromFile("camera_c.txt", "camera_r_t.txt"))
 759     {
 760         cout << "failed to load camera calibration.\n";
 761         exit(-1);
 762     }
 763     glutIdleFunc(0);
 764     //glutDisplayFunc(0);
 765     delete calib;
 766     calib=0;
 767 }
 768
 769 /*! Called by GLUT during geometric calibration when there's nothing else to do.
 770  * This function grab frames from camera(s), run the 2D detection on every image,
 771  * and keep the result in memory for calibration. When enough homographies have
 772  * been detected, it tries to actually calibrate the cameras.
 773  */
 774 static void geomCalibIdle(void)
 775 {
 776     // acquire images
 777     multi->grabFrames();
 778
 779     // detect the calibration object in every image
 780     // (this loop could be paralelized)
 781     int nbdet=0;
 782     for (int i=0; i<multi->cams.size(); ++i)
 783     {
 784         if (multi->cams[i]->detect()) nbdet++;
 785     }
 786
 787     if(!frameTexture) frameTexture = new IplTexture;
 788     frameTexture->setImage(multi->cams[current_cam]->frame);
 789
 790     if (nbdet>0)
 791     {
 792         for (int i=0; i<multi->cams.size(); ++i)
 793         {
 794             if (multi->cams[i]->detector.object_is_detected)
 795             {
 796                 add_detected_homography(i, multi->cams[i]->detector, *calib);
 797             }
 798             else
 799             {
 800                 calib->AddHomography(i);
 801             }
 802         }
 803         geom_calib_nb_homography++;
 804     }
 805
 806     if (geom_calib_nb_homography>=150)
 807     {
 808         if (calib->Calibrate(
 809                     50, // max hom
 810                     (multi->cams.size() > 1 ? 1:2),   // padding or random
 811                     (multi->cams.size() > 1 ? 0:3),
 812                     1,   // padding ratio 1/2
 813                     0,
 814                     0,
 815                     0.0078125,  //alpha
 816                     0.9,                //beta
 817                     0.001953125,//gamma
 818                     10,   // iter
 819                     0.05, //eps
 820                     3   //postfilter eps
 821                 ))
 822         {
 823             calib->PrintOptimizedResultsToFile1();
 824             geomCalibEnd();
 825             start();
 826             return;
 827         }
 828     }
 829     glutPostRedisplay();
 830 }
 831
 832 /*!\brief Start geometric calibration. If the calibration can be loaded from disk,
 833  * continue directly with photometric calibration.
 834  */
 835 static void geomCalibStart(bool cache)
 836 {
 837     if (cache && multi->model.augm.LoadOptimalStructureFromFile("camera_c.txt", "camera_r_t.txt"))
 838     {
 839         start();
 840         return;
 841     }
 842
 843     // construct a CamCalibration object and register all the cameras
 844     calib = new CamCalibration();
 845
 846     for (int i=0; i<multi->cams.size(); ++i)
 847     {
 848         calib->AddCamera(multi->cams[i]->width, multi->cams[i]->height);
 849     }
 850
 851     geom_calib_nb_homography=0;
 852     glutDisplayFunc(geomCalibDraw);
 853     glutIdleFunc(geomCalibIdle);
 854 }
 855
 856 //#define DEBUG_SHADER
 857 /*! The paint callback during photometric calibration and augmentation. In this
 858  * case, we have access to 3D data. Thus, we can augment the calibration target
 859  * with cool stuff.
 860  */
 861 static void draw(void)
 862 {
 863
 864     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 865     glDisable(GL_LIGHTING);
 866
 867     drawBackground(frameTexture);
 868
 869     string cnt_str;
 870     stringstream cnt_out;
 871     cnt_out << cnt;
 872     cnt_str = cnt_out.str();
 873
 874     //IplImage *pre_mask = cvCreateImage(cvSize(WIDTH, HEIGHT), 8, 3);
 875
 876     if (!multi) return;
 877
 878     IplImage *im = multi->model.image;
 879
 880     if (!im) return;
 881
 882     int now = glutGet(GLUT_ELAPSED_TIME);
 883     /* elapsed time
 884     cout << now/1000.0 << endl;
 885     */
 886     // pixel shift
 887
 888     if (frame_ok and augment == 1)
 889     {
 890
 891         found_match = 1;
 892
 893         // here i calculate pixel shift to help us remove pose estimation jitter
 894         IplImage *_frame = frameTexture->getIm();
 895
 896         if (!difference)
 897         {
 898             this_frame=cvCreateImage( cvSize(video_width, video_height),8,1);
 899             bit_frame=cvCreateImage(cvSize(video_width, video_height),8,1);
 900             last_frame=cvCloneImage(this_frame);
 901             diff=cvCreateImage(cvSize(video_width, video_height), 8 ,1);
 902             difference=1;
 903         }
 904
 905         cvCvtColor(_frame, this_frame,CV_BGR2GRAY);
 906         cvAbsDiff(this_frame,last_frame,diff);
 907         //threshold image
 908         cvThreshold(diff,bit_frame,30,255,CV_THRESH_BINARY);
 909         last_frame=cvCloneImage(this_frame);
 910         // calculate white pixels (pixel shift)
 911         int pixel_shift = cvCountNonZero(bit_frame);
 912         //cvSaveImage("diff.png", bit_frame);
 913         //cout << pixel_shift << endl;
 914
 915         // Fetch object -> image, world->image and world -> object matrices
 916         CvMat *proj = multi->model.augm.GetProjectionMatrix(current_cam);
 917         CvMat *old_proj = multi->model.augm.GetProjectionMatrix(current_cam);
 918         CvMat *world = multi->model.augm.GetObjectToWorld();
 919         Mat3x4 moveObject, rot, obj2World, movedRT_;
 920         moveObject.setTranslate(im->width/2,im->height/2,-120*3/4);
 921         rot.setRotate(Vec3(1,0,0),2*M_PI*180.0/360.0);
 922         moveObject.mul(rot);
 923         CvMat cvMoveObject = cvMat(3,4,CV_64FC1, moveObject.m);
 924         CvMat movedRT=cvMat(3,4,CV_64FC1,movedRT_.m);
 925
 926         // pose only during movement
 927         if (pixel_shift >= 200 || !have_proj)
 928         {
 929             // memcpy or vectorisation speedup?
 930             for( int i = 0; i < 3; i++ )
 931             {
 932                 for( int j = 0; j < 4; j++ )
 933                 {
 934                     a_proj[i][j] = cvmGet( proj, i, j );
 935                     obj2World.m[i][j] = cvmGet(world, i, j);
 936                 }
 937             }
 938             have_proj = 1;
 939             memcpy(old_a_proj, a_proj, sizeof(a_proj));
 940         }
 941         else // copy last known good projection over current
 942         {
 943             memcpy(a_proj, old_a_proj, sizeof(old_a_proj));
 944         }
 945
 946         CamCalibration::Mat3x4Mul( world, &cvMoveObject, &movedRT);
 947         // translate into OpenGL PROJECTION and MODELVIEW matrices
 948         PerspectiveCamera c;
 949         //c.loadTdir(a_proj, multi->cams[0]->frame->width, multi->cams[0]->frame->height);
 950         c.loadTdir(a_proj, multi->cams[0]->detect_width, multi->cams[0]->detect_height );
 951         c.flip();
 952         c.setPlanes(100,1000000); // near/far clip planes
 953         cvReleaseMat(&proj);
 954
 955         // must set the model view after drawing the background.
 956         c.setGlProjection();
 957         c.setGlModelView();
 958
 959         /*! this is the beginning of prototype code for an occlusion mask built
 960          *  by comparison of the tracked plane with that of the model image
 961
 962         // create a copy of frame texture and warp to model image perspective
 963         CvPoint2D32f *c2 = new CvPoint2D32f[4];
 964         // update corner points of ROI in pixel space
 965         c2[0].x = cvRound(multi->cams[0]->detector.detected_u_corner1);
 966         c2[0].y = cvRound(multi->cams[0]->detector.detected_v_corner1);
 967         c2[1].x = cvRound(multi->cams[0]->detector.detected_u_corner2);
 968         c2[1].y = cvRound(multi->cams[0]->detector.detected_v_corner2);
 969         c2[2].x = cvRound(multi->cams[0]->detector.detected_u_corner3);
 970         c2[2].y = cvRound(multi->cams[0]->detector.detected_v_corner3);
 971         c2[3].x = cvRound(multi->cams[0]->detector.detected_u_corner4);
 972         c2[3].y = cvRound(multi->cams[0]->detector.detected_v_corner4);
 973
 974         CvMat* mmat = cvCreateMat(3,3, CV_32FC1);
 975         IplImage *warped = cvCreateImage(cvSize(model_image->width, model_image->height), 8, 3);
 976         mmat = cvGetPerspectiveTransform(c2, c1, mmat);
 977         cvWarpPerspective(frameTexture->getIm(), warped, mmat);
 978         cvReleaseMat(&mmat);
 979
 980         // find difference between model image and frame
 981         IplImage *i1=cvCreateImage(cvSize(im->width,im->height),im->depth,1);
 982         IplImage *i2=cvCreateImage(cvSize(im->width,im->height),im->depth,1);
 983         IplImage *diff=cvCreateImage(cvSize(im->width,im->height),im->depth,1);
 984         IplImage *display=cvCreateImage(cvSize(im->width,im->height),im->depth,1);
 985
 986         cvCvtColor(im, i1,CV_BGR2GRAY);
 987         cvCvtColor(warped, i2,CV_BGR2GRAY);
 988         cvAbsDiff(i2,i1,diff);
 989         cvThreshold(diff, display, 35, 255, CV_THRESH_BINARY);
 990
 991         cvReleaseImage(&warped);
 992         cvSaveImage("checkdiff.png", display);
 993         */
 994
 995         /* circles at corners of ROI. useful for debugging.
 996         cvCircle(frameTexture->getIm(), c1, 10, CV_RGB(255, 0, 0), 2);
 997         cvCircle(frameTexture->getIm(), c2, 10, CV_RGB(255, 0, 0), 2);
 998         cvCircle(frameTexture->getIm(), c3, 10, CV_RGB(255, 0, 0), 2);
 999         cvCircle(frameTexture->getIm(), c4, 10, CV_RGB(255, 0, 0), 2);
1000
1001         tex = new IplTexture;
1002         tex->setImage(frameTexture->getIm());
1003         drawBackground(tex);
1004         */
1005
1006
1007
1008 #ifndef DEBUG_SHADER
1009
1010         glEnable(GL_DEPTH_TEST);
1011         glEnable(GL_BLEND);
1012         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
1013         glDisable(GL_CULL_FACE);
1014         // multiply texture colour by surface colour of poly
1015         glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
1016         glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
1017
1018         if (avi_play == true)
1019         {
1020             IplImage *avi_frame = 0;
1021             IplImage *avi_image = 0;
1022             avi_frame = cvQueryFrame( avi_capture );
1023             avi_image = cvCreateImage(cvSize(video_width/2, video_height/2), 8, 3);
1024             cvResize(avi_frame, avi_image, 0);
1025             avi_image->origin = avi_frame->origin;
1026             GLenum format = IsBGR(avi_image->channelSeq) ? GL_BGR_EXT : GL_RGBA;
1027
1028             if (!avi_play_init)
1029             {
1030                 glGenTextures(1, &imageID);
1031                 glBindTexture(GL_TEXTURE_2D, imageID);
1032                 glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
1033                 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
1034                 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
1035                 glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, avi_image->width, avi_image->height, 0, format, GL_UNSIGNED_BYTE, avi_image->imageData);
1036                 avi_play_init=true;
1037             }
1038             else
1039             {
1040                 glBindTexture(GL_TEXTURE_2D, imageID);
1041                 glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, avi_image->width, avi_image->height, format, GL_UNSIGNED_BYTE, avi_image->imageData);
1042             }
1043         }
1044         else
1045         {
1046             glBindTexture(GL_TEXTURE_2D, imageID);
1047             glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
1048             glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
1049             IplImage image = *artverts[cnt].image;
1050             GLenum format = IsBGR(image.channelSeq) ? GL_BGR_EXT : GL_RGBA;
1051             glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, image.width, image.height, 0, format, GL_UNSIGNED_BYTE, image.imageData);
1052         }
1053
1054         glEnable(GL_TEXTURE_2D);
1055
1056         glHint(GL_POLYGON_SMOOTH, GL_NICEST);
1057         glEnable(GL_POLYGON_SMOOTH);
1058
1059         // create a simple fade-in when we have a track
1060         if (fade < 1.0)
1061         {
1062             fade += 0.1 * fade_speed;
1063         }
1064         else
1065         {
1066             fade = 1;
1067         }
1068
1069         glColor4f(1.0, 1.0, 1.0, fade);
1070
1071         glBegin(GL_QUADS);
1072         glTexCoord2f(0, 0);
1073         glVertex3f(roi_vec[0], roi_vec[1], 0);
1074         glTexCoord2f(1, 0);
1075         glVertex3f(roi_vec[2], roi_vec[3], 0);
1076         glTexCoord2f(1, 1);
1077         glVertex3f(roi_vec[4], roi_vec[5], 0);
1078         glTexCoord2f(0, 1);
1079         glVertex3f(roi_vec[6], roi_vec[7], 0);
1080         glEnd();
1081
1082         glDisable(GL_TEXTURE_2D);
1083
1084         /*! 'label' is a boolean set by the right mouse button and toggles the
1085          * in-scene artvert label.
1086          */
1087
1088         if (label)
1089         {
1090             glBegin(GL_LINE_LOOP);
1091             glColor3f(0.0, 1.0, 0.0);
1092             glVertex3f(roi_vec[0]-10, roi_vec[1]-10, 0);
1093             glVertex3f(roi_vec[2]+10, roi_vec[3]-10, 0);
1094             glVertex3f(roi_vec[4]+10, roi_vec[5]+10, 0);
1095             glVertex3f(roi_vec[6]-10, roi_vec[7]+10, 0);
1096             glVertex3f(roi_vec[0]-10, roi_vec[1]-10, 0);
1097             glEnd();
1098
1099             glTranslatef(roi_vec[2]+12, roi_vec[3], 0);
1100             glRotatef(180, 1.0, 0.0, 0.0);
1101             glRotatef(-45, 0.0, 1.0, 0.0);
1102             glColor4f(0.0, 1.0, 0.0, 1);
1103
1104             glBegin(GL_LINE_LOOP);
1105             glVertex3f(0, 10, -.2);
1106             glVertex3f(150, 10, -.2);
1107             glVertex3f(150, -60, -.2);
1108             glVertex3f(0, -60, -.2);
1109             glEnd();
1110
1111             glColor4f(0.0, 1.0, 0.0, .5);
1112
1113             glBegin(GL_QUADS);
1114             glVertex3f(0, 10, -.2);
1115             glVertex3f(150, 10, -.2);
1116             glVertex3f(150, -60, -.2);
1117             glVertex3f(0, -60, -.2);
1118             glEnd();
1119
1120             // render the text in the label
1121             glColor4f(1.0, 1.0, 1.0, 1);
1122             ftglFont->Render(artverts[cnt].artvert);
1123             glTranslatef(0, -12, 0);
1124             ftglFont->Render(artverts[cnt].date);
1125             glTranslatef(0, -12, 0);
1126             ftglFont->Render(artverts[cnt].author);
1127             glTranslatef(0, -12, 0);
1128             ftglFont->Render(artverts[cnt].advert);
1129             glTranslatef(0, -12, 0);
1130             ftglFont->Render(artverts[cnt].street);
1131         }
1132
1133 #else
1134 #endif
1135         //glEnd();
1136
1137 #ifndef DEBUG_SHADER
1138         // apply the object transformation matrix
1139         Mat3x4 w2e(c.getWorldToEyeMat());
1140         w2e.mul(moveObject);
1141         c.setWorldToEyeMat(w2e);
1142         c.setGlModelView();
1143 #endif
1144
1145         if (multi->model.map.isReady())
1146         {
1147             glDisable(GL_LIGHTING);
1148 #ifdef DEBUG_SHADER
1149             multi->model.map.enableShader(current_cam, world);
1150 #else
1151             multi->model.map.enableShader(current_cam, &movedRT);
1152 #endif
1153         }
1154         cvReleaseMat(&world);
1155         {
1156             CvScalar c =cvGet2D(multi->model.image, multi->model.image->height/2, multi->model.image->width/2);
1157             glColor3d(c.val[2], c.val[1], c.val[0]);
1158         }
1159         if (multi->model.map.isReady())
1160             multi->model.map.disableShader();
1161         else
1162             glDisable(GL_LIGHTING);
1163
1164         if ( avi_play == true  )
1165         {
1166             cvReleaseImage(&avi_image);
1167             cvReleaseImage(&avi_frame);
1168         }
1169     }
1170
1171     else if (!frame_ok)
1172     {
1173         fade = 0.0;
1174         found_match=0;
1175     }
1176
1177     glLoadIdentity();
1178     // we need to setup a new projection matrix for the title font.
1179     glMatrixMode(GL_PROJECTION);
1180     glLoadIdentity();
1181     glMatrixMode(GL_MODELVIEW);
1182     glLoadIdentity();
1183     glTranslatef(-.98, 0.9, 0.0);
1184     glScalef(.003, .003, .003);
1185     ftglFont->FaceSize(32);
1186     glColor4f(1.0, 1.0, 1.0, 1);
1187     ftglFont->Render("the artvertiser 0.4");
1188     glTranslatef(0, -(video_height+30), 0);
1189     glColor4f(1.0, 1.0, 1.0, .6);
1190     //ftglFont->FaceSize(16);
1191     //ftglFont->Render(date(now).c_str());
1192     if (found_match == 1 and (now/1000)%2== 0)
1193     {
1194         glTranslatef(video_width-295, video_height+35, 0);
1195         glColor4f(0.0, 1.0, 0.0, .8);
1196         glBegin(GL_TRIANGLES);
1197         glVertex3f(140, 0, 0);
1198         glVertex3f(150, 10, 0);
1199         glVertex3f(140, 20, 0);
1200         glEnd();
1201         glTranslatef(70, 5, 0);
1202         ftglFont->FaceSize(16);
1203         ftglFont->Render("tracking");
1204     }
1205
1206     // reset the ftgl font size for next pass
1207     ftglFont->FaceSize(12);
1208
1209
1210     if ( show_status )
1211     {
1212         // not using exactly as defined in framerate.h because it's stupid
1213         frameEnd(0.8f, 0.8f, 0.8f, 0.9, 0.95 );
1214         frameStart();
1215
1216         // now status string
1217         string draw_string = status_string;
1218         if ( multi )
1219         {
1220             // show detector settings
1221             planar_object_recognizer &detector(multi->cams[current_cam]->detector);
1222             char detector_settings_string[1024];
1223             sprintf( detector_settings_string, "ransac dist %4.2f  stop %i  iter %i   detected points %i match count %i,\n"
1224                     "refine %6.4f  score %6.4f  viewrate %6.4f  tau %2i",
1225                     detector.ransac_dist_threshold,
1226                     detector.ransac_stop_support,
1227                     detector.max_ransac_iterations,
1228                     detector.detected_point_number,
1229                     detector.match_number,
1230                     detector.non_linear_refine_threshold,
1231                     detector.match_score_threshold,
1232                     detector.min_view_rate,
1233                     detector.point_detector_tau );
1234
1235             draw_string += "\n" + string(detector_settings_string);
1236         }
1237         drawGlutString( draw_string.c_str(), 0.8f, 0.8f, 0.8f, 0.01f, 0.1f );
1238     }
1239
1240
1241     glutSwapBuffers();
1242     cvReleaseImage(&image); // cleanup used image
1243     glFlush();
1244 }
1245
1246 /*! GLUT calls this during photometric calibration or augmentation phase when
1247  * there's nothing else to do. This function does the 2D detection and bundle
1248  * adjusts the 3D pose of the calibration pattern.
1249  */
1250 static void idle()
1251 {
1252     PROFILE_SECTION_PUSH( "idle loop" );
1253
1254     // acquire images
1255     multi->grabFrames();
1256
1257     // detect the calibration object in every image
1258     // (this loop could be paralelized)
1259     int nbdet=1;
1260
1261     multi->cams[0]->detect();
1262
1263     if(!frameTexture) frameTexture = new IplTexture;
1264     frameTexture->setImage(multi->cams[current_cam]->frame);
1265
1266     frame_ok=false;
1267
1268     multi->model.augm.Clear();
1269     if (multi->cams[0]->detector.object_is_detected)
1270     {
1271         add_detected_homography(0, multi->cams[0]->detector, multi->model.augm);
1272     }
1273     else
1274     {
1275         multi->model.augm.AddHomography();
1276     }
1277
1278     frame_ok = multi->model.augm.Accomodate(4, 1e-4);
1279
1280     if (frame_ok)
1281     {
1282         // fetch surface normal in world coordinates
1283         CvMat *mat = multi->model.augm.GetObjectToWorld();
1284         float normal[3];
1285         for (int j=0; j<3; j++) normal[j] = cvGet2D(mat, j, 2).val[0];
1286         cvReleaseMat(&mat);
1287     }
1288
1289
1290     glutPostRedisplay();
1291
1292     PROFILE_SECTION_POP();
1293
1294     if ( show_profile_results )
1295     {
1296         // show profiler output
1297         printf("showing results\n");
1298         FProfiler::Display();
1299         show_profile_results = false;
1300     }
1301 }
1302
1303 //! Starts photometric calibration.
1304 static void start()
1305 {
1306     glutIdleFunc(idle);
1307     glutDisplayFunc(draw);
1308 }
1309 //EOF
1310