git.maemo.org Git - opencv/blob - apps/StereoDemo/main.cpp

   1 /****************************************************************************************\
   2 *                                                                                        *
   3 *                                        Stereo Demo                                     *
   4 *                                                                                        *
   5 \****************************************************************************************/
   6
   7 #include "cvaux.h"
   8 #include "highgui.h"
   9 #include <limits.h>
  10 #include <stdio.h>
  11 #include <ctype.h>
  12 #include <time.h>
  13
  14 volatile int g_undistort = 0;
  15 volatile int g_calib = 0;
  16 volatile int g_rectify = 0;
  17 volatile int g_save = 0;
  18 volatile int g_calc_stereo = 1;
  19 volatile int g_view_rectified = 0;
  20 volatile int g_snap = 0;
  21 volatile int g_write_avi = 0;
  22
  23 void callback( IplImage* image1,IplImage* image2 );
  24 void callback1( IplImage* image );
  25
  26 CvCalibFilter CalibFilter;
  27 double EtalonParams[3] = { 6, 8, 3 };
  28 IplImage* vis_disp = NULL;
  29 //StereoMatcher stereo_matcher;
  30
  31 CvVideoWriter* VideoWriterL = NULL;
  32 CvVideoWriter* VideoWriterR = NULL;
  33 CvVideoWriter* VideoWriterAux = NULL;
  34
  35
  36 void toggle_calib(void);
  37 void toggle_undistort(void);
  38
  39 int maxdisp = 100;
  40
  41 void calc_dense_disparity(IplImage* image1, IplImage* image2);
  42
  43 void print_help(void)
  44 {
  45     printf("Help:\n"
  46                    "h - this help screen\n"
  47                    "g - start/stop calibration\n"
  48                    "spacebar - take a snapshot to send to calibration\n"
  49                    "s - save camera parameters to camera.txt\n"
  50                    "l - load camera parameters from camera.txt\n"
  51                    "u - toggle undistortion\n"
  52                    "r - toggle rectification\n"
  53                    "t - reverse the cameras order\n"
  54                    "n - save the couple of frames\n"
  55                    "d - on/off computing disparity\n"
  56                    "v - toggle view of the rectified images\n"
  57                    "a - start/stop writing avi files\n"
  58         );
  59 }
  60
  61 #define SOURCE_AVI 1
  62 #define SOURCE_CAM 2
  63
  64 #ifndef FLTK
  65 int main( int argc, char** argv )
  66 {
  67     int source = SOURCE_CAM;
  68     if( argc == 3 )
  69     {
  70         source = SOURCE_AVI;
  71     }
  72     g_calc_stereo = 1;
  73
  74     CvCapture* captureL; //left
  75     CvCapture* captureR; //right
  76
  77     IplImage* left_gray = 0;
  78     IplImage* right_gray = 0;
  79
  80     CalibFilter.SetEtalon( CV_CALIB_ETALON_CHESSBOARD, EtalonParams );
  81     CalibFilter.SetCameraCount(2);
  82     CalibFilter.SetFrames(15);
  83
  84     if( source == SOURCE_CAM ) //input from cameras
  85     {
  86         captureL = cvCaptureFromCAM( 0 ); //first camera
  87         captureR = cvCaptureFromCAM( 1 ); //second camera
  88     }
  89     else
  90     {
  91         captureL = cvCaptureFromAVI( argv[1] ); //first avi file
  92         captureR = cvCaptureFromAVI( argv[2] ); //second avi file
  93     }
  94
  95     if( !captureL || !captureR )
  96     {  printf("can't run camera/avi\n");  return 1; }
  97
  98     cvNamedWindow( "source left", 1 );
  99     cvNamedWindow( "source right", 1 );
 100
 101     print_help();
 102
 103     for(;;)
 104     {
 105         int cmd = cvWaitKey( 2 );
 106         if( cmd == '\x1b' )
 107             break;
 108
 109         //frame processing
 110         cvGrabFrame( captureL );
 111         //int time2 = clock();
 112         cvGrabFrame( captureR );
 113         //int time1 = clock();
 114         //printf("time diff = %d", time1 - time2);
 115         IplImage* frameL = cvRetrieveFrame( captureL );
 116         IplImage* frameR = cvRetrieveFrame( captureR );
 117
 118         if( !frameL || !frameR )
 119         {
 120             cvReleaseVideoWriter( &VideoWriterAux );
 121
 122             if( g_write_avi == 1 )
 123             {
 124                 cvReleaseVideoWriter( &VideoWriterL );
 125                 cvReleaseVideoWriter( &VideoWriterR );
 126             }
 127             break;
 128         }
 129
 130         if( source == SOURCE_AVI )
 131         {
 132             frameL->origin = frameR->origin = IPL_ORIGIN_TL;
 133             cvFlip( frameR, frameR, 0 );
 134             cvFlip( frameL, frameL, 0 );
 135         }
 136
 137         //convert color to gray
 138         if( !left_gray && !right_gray )
 139         {
 140             left_gray = cvCreateImage(cvSize( frameL->width,frameL->height), IPL_DEPTH_8U, 1 );
 141             right_gray = cvCreateImage(cvSize( frameL->width,frameL->height), IPL_DEPTH_8U, 1 );
 142             left_gray->origin = right_gray->origin = frameL->origin;
 143         }
 144
 145         cvCvtColor( frameL, left_gray, CV_BGR2GRAY );
 146         cvCvtColor( frameR, right_gray, CV_BGR2GRAY );
 147
 148         callback( left_gray, right_gray );
 149
 150         switch( tolower(cmd) )
 151         {
 152         case '1':
 153         case '2':
 154             break;
 155         case 'g':
 156             toggle_calib();
 157             break;
 158         case 's':
 159             CalibFilter.SaveCameraParams("cameras.txt");
 160             break;
 161         case 'l':
 162             CalibFilter.LoadCameraParams("cameras.txt");
 163             g_rectify ^= 1;
 164             break;
 165         case 'u':
 166             g_undistort ^= 1;
 167             break;
 168         case 'r':
 169             g_rectify ^= 1;
 170             break;
 171         case 'v':
 172             g_view_rectified ^= 1;
 173             break;
 174         case 'n':
 175             g_save = 1;
 176             break;
 177         case 't': {
 178             CvCapture* tmp = captureL;
 179             captureL = captureR;
 180             captureR = tmp;
 181             break; }
 182         case 'h':
 183             print_help();
 184             break;
 185         case 'd':
 186             g_calc_stereo ^=1;
 187             break;
 188         case 'a':
 189             g_write_avi ^=1;
 190             if( g_write_avi )
 191             {
 192                 VideoWriterL = cvCreateVideoWriter( "StereoL.avi", -1,
 193                                         10, cvSize(frameL->width,frameL->height)  );
 194                 VideoWriterR = cvCreateVideoWriter( "StereoR.avi", -1,
 195                                         10, cvSize(frameL->width,frameL->height)  );
 196             }
 197             else
 198             {
 199                 cvReleaseVideoWriter( &VideoWriterL );
 200                 cvReleaseVideoWriter( &VideoWriterR );
 201             }
 202             break;
 203         case ' ':
 204             g_snap = 1;
 205             break;
 206         }
 207     }
 208
 209
 210     cvReleaseCapture( &captureL );
 211     cvReleaseCapture( &captureR );
 212
 213     cvReleaseImage( &left_gray );
 214     cvReleaseImage( &right_gray );
 215
 216     cvReleaseImage(&vis_disp);
 217
 218
 219     return 0;
 220 }
 221 #endif
 222
 223 void toggle_undistort( void )
 224 {
 225     g_undistort ^= 1;
 226 }
 227
 228
 229 int prev_time = 0;
 230
 231 void toggle_calib( void )
 232 {
 233     prev_time = clock() - 1000;
 234     if( !g_calib )
 235         g_undistort = 0;
 236     g_calib ^= 1;
 237 }
 238
 239 void SaveStereoPair( IplImage** images, const char* rootname )
 240 {
 241     int i = 0;
 242     char name[1000];
 243
 244     for( i = 0; i < 1000; i++ )
 245     {
 246         sprintf( name, "%s%03dL.bmp", rootname, i );
 247         FILE* f = fopen( name, "rb" );
 248
 249         if( f )
 250         {
 251             fclose(f);
 252             continue;
 253         }
 254
 255         cvSaveImage( name, images[0] );
 256
 257         sprintf( name, "%s%03dR.bmp", rootname, i );
 258         cvSaveImage( name, images[1] );
 259
 260         break;
 261     }
 262 }
 263
 264 void callback( IplImage* image1, IplImage* image2 )
 265 {
 266     IplImage* images[] = { image1, image2 };
 267     int save = g_save != 0;
 268
 269 #if 1
 270     if( g_calib && g_snap )
 271     {
 272         bool found = CalibFilter.FindEtalon( images );
 273
 274         g_view_rectified = 0;
 275
 276         if( !save )
 277             CalibFilter.DrawPoints( images );
 278
 279         if( found )
 280         {
 281             int cur_time = clock();
 282             if( cur_time >= prev_time + 1000 )
 283             {
 284                 prev_time = cur_time;
 285                 CalibFilter.Push();
 286                 cvXorS( image1, cvScalarAll(255), image1 );
 287                 cvXorS( image2, cvScalarAll(255), image2 );
 288                 g_undistort = 0;
 289             }
 290
 291             if( CalibFilter.IsCalibrated() )
 292             {
 293                 g_calib ^= 1;
 294                 g_undistort = 1;
 295                 g_rectify = 1;
 296             }
 297         }
 298     }
 299
 300     if( !g_view_rectified )
 301     {
 302         cvDestroyWindow( "rectified left" );
 303         cvDestroyWindow( "rectified right" );
 304     }
 305
 306     //if( g_undistort )
 307     //    CalibFilter.Undistort( images, images );
 308
 309     cvShowImage( "source left", image1 );
 310     cvShowImage( "source right", image2 );
 311
 312     if ( g_calib && g_snap ){
 313         g_snap = 0;
 314         cvWaitKey( 1000 );
 315     }
 316
 317     if( save )
 318     {
 319         SaveStereoPair( images, "imgU" );
 320     }
 321
 322     if( g_rectify && CalibFilter.IsCalibrated() )
 323     {
 324         CalibFilter.Rectify( images, images );
 325
 326         //calc_disparity(image1, image2);
 327         if( save )
 328         {
 329             SaveStereoPair( images, "imgR" );
 330         }
 331     }
 332
 333     if( CalibFilter.IsCalibrated() && g_view_rectified)
 334     {
 335         IplImage *l;
 336         l = cvCreateImage( cvSize(image1->width, image1->height),
 337                            IPL_DEPTH_8U, 1 );
 338
 339         cvConvertScale( image1, l, 1, 0 );
 340         cvNamedWindow( "rectified left", 1 );
 341         cvNamedWindow( "rectified right", 1 );
 342         cvShowImage( "rectified left", l );
 343         cvConvertScale(image2, l, 1, 0);
 344         cvShowImage( "rectified right", l );
 345         cvReleaseImage( &l );
 346     }
 347
 348     if( g_calc_stereo )
 349     {
 350         calc_dense_disparity(image1, image2);
 351     }
 352
 353     if( g_write_avi )
 354     {
 355         cvWriteFrame( VideoWriterL, image1 );
 356         cvWriteFrame( VideoWriterR, image2 );
 357     }
 358 #endif
 359     g_save = 0;
 360 }
 361 int dense_inited = 0;
 362 void calc_dense_disparity(IplImage* image1, IplImage* image2)
 363 {
 364     if(!dense_inited)
 365     {
 366         cvNamedWindow("disparity", 1);
 367         cvCreateTrackbar( "maxdisp", "disparity", &maxdisp, 255, 0 );
 368         dense_inited = 1;
 369         vis_disp = cvCreateImage(cvSize(image1->width, image1->height), IPL_DEPTH_8U, 1);
 370     }
 371
 372     vis_disp->origin = image1->origin;
 373     cvFindStereoCorrespondence(
 374                    image1, image2,
 375                    CV_DISPARITY_BIRCHFIELD,
 376                    vis_disp,
 377                    maxdisp );
 378     cvConvertScale(vis_disp, vis_disp, 255.f/maxdisp);
 379     cvShowImage("disparity", vis_disp);
 380 }
 381
 382 /*void calc_disparity_matcher(IplImage* left, IplImage* right)
 383 {
 384     if( !stereo_matcher.IsInited() )
 385     {
 386         stereo_matcher.Init( cvSize(left->width, left->height ),
 387             60,
 388             STEREO_DP
 389             //STEREO_ID
 390             //STEREO_SEGM
 391             //STEREO_SEGM_MULTY
 392             );
 393         cvvNamedWindow("disparity", 1);
 394         cvvWaitKey(0);
 395     }
 396     if( stereo_matcher.m_max_disparity != thresh1 )
 397         stereo_matcher.Init( cvSize(left->width, left->height ),
 398             thresh1, STEREO_DP);
 399
 400     stereo_matcher.OnFrames( left, right );
 401
 402     //scale disparity
 403     iplMultiplyS( stereo_matcher.GetDisparity(),
 404                   stereo_matcher.GetDisparity(),
 405                   255/stereo_matcher.m_max_disparity );
 406
 407    // iplBlur( stereo_matcher.GetDisparity(),
 408      //             stereo_matcher.GetDisparity(), 3,3,1,1 );
 409
 410     cvErode( stereo_matcher.GetDisparity(),
 411                   stereo_matcher.GetDisparity(), NULL, 1 );
 412     //cvDilate( stereo_matcher.GetDisparity(),
 413     //              stereo_matcher.GetDisparity(), NULL, 1 );
 414
 415
 416
 417     cvvShowImage("left", left );
 418     cvvShowImage("right", right );
 419     cvvShowImage("disparity", stereo_matcher.GetDisparity() );
 420
 421
 422
 423     if( !VideoWriterAux )
 424     {
 425         VideoWriterAux = cvCreateVideoWriter( "StereoLD.avi", -1,
 426                                         5, cvSize(left->width*2,left->height)  );
 427     }
 428     IplImage* merged = cvCreateImage( cvSize(left->width*2,left->height),
 429                                       IPL_DEPTH_8U, 3 );
 430     IplROI roi;
 431     roi.coi = 0; roi.height = left->height; roi.width = left->width;
 432     roi.yOffset = 0;
 433     roi.xOffset = 0;
 434
 435     left->roi = &roi;
 436     merged->roi = &roi;
 437     iplCopy( left, merged);
 438
 439     IplROI roi2 = roi;
 440     stereo_matcher.GetDisparity()->roi = &roi2;
 441     roi.xOffset = left->width;
 442
 443     iplCopy( stereo_matcher.GetDisparity(), merged);
 444
 445     merged->roi = left->roi = stereo_matcher.GetDisparity()->roi = NULL;
 446
 447     cvWriteFrame( VideoWriterAux, merged );
 448
 449     //visualize disparity
 450     //cvvWaitKeyEx("left", 1000);
 451
 452 }
 453 */