vcs.maemo.org Git - opencv/blob - samples/c/stereo_calib.cpp

   1 /* This is sample from the OpenCV book. The copyright notice is below */
   2
   3 /* *************** License:**************************
   4    Oct. 3, 2008
   5    Right to use this code in any way you want without warrenty, support or any guarentee of it working.
   6
   7    BOOK: It would be nice if you cited it:
   8    Learning OpenCV: Computer Vision with the OpenCV Library
   9      by Gary Bradski and Adrian Kaehler
  10      Published by O'Reilly Media, October 3, 2008
  11
  12    AVAILABLE AT:
  13      http://www.amazon.com/Learning-OpenCV-Computer-Vision-Library/dp/0596516134
  14      Or: http://oreilly.com/catalog/9780596516130/
  15      ISBN-10: 0596516134 or: ISBN-13: 978-0596516130
  16
  17    OTHER OPENCV SITES:
  18    * The source code is on sourceforge at:
  19      http://sourceforge.net/projects/opencvlibrary/
  20    * The OpenCV wiki page (As of Oct 1, 2008 this is down for changing over servers, but should come back):
  21      http://opencvlibrary.sourceforge.net/
  22    * An active user group is at:
  23      http://tech.groups.yahoo.com/group/OpenCV/
  24    * The minutes of weekly OpenCV development meetings are at:
  25      http://pr.willowgarage.com/wiki/OpenCV
  26    ************************************************** */
  27
  28 #include "cv.h"
  29 #include "cxmisc.h"
  30 #include "highgui.h"
  31 #include <vector>
  32 #include <string>
  33 #include <algorithm>
  34 #include <stdio.h>
  35 #include <ctype.h>
  36
  37 using namespace std;
  38
  39 //
  40 // Given a list of chessboard images, the number of corners (nx, ny)
  41 // on the chessboards, and a flag: useCalibrated for calibrated (0) or
  42 // uncalibrated (1: use cvStereoCalibrate(), 2: compute fundamental
  43 // matrix separately) stereo. Calibrate the cameras and display the
  44 // rectified results along with the computed disparity images.
  45 //
  46 static void
  47 StereoCalib(const char* imageList, int useUncalibrated)
  48 {
  49     int nx = 0, ny = 0;
  50     int displayCorners = 0;
  51     int showUndistorted = 1;
  52     bool isVerticalStereo = false;//OpenCV can handle left-right
  53                                       //or up-down camera arrangements
  54     const int maxScale = 1;
  55     const float squareSize = 1.f; //Set this to your actual square size
  56     FILE* f = fopen(imageList, "rt");
  57     int i, j, lr, nframes, n, N = 0;
  58     vector<string> imageNames[2];
  59     vector<CvPoint3D32f> objectPoints;
  60     vector<CvPoint2D32f> points[2];
  61     vector<int> npoints;
  62     vector<uchar> active[2];
  63     vector<CvPoint2D32f> temp;
  64     CvSize imageSize = {0,0};
  65     // ARRAY AND VECTOR STORAGE:
  66     double M1[3][3], M2[3][3], D1[5], D2[5];
  67     double R[3][3], T[3], E[3][3], F[3][3];
  68     CvMat _M1 = cvMat(3, 3, CV_64F, M1 );
  69     CvMat _M2 = cvMat(3, 3, CV_64F, M2 );
  70     CvMat _D1 = cvMat(1, 5, CV_64F, D1 );
  71     CvMat _D2 = cvMat(1, 5, CV_64F, D2 );
  72     CvMat _R = cvMat(3, 3, CV_64F, R );
  73     CvMat _T = cvMat(3, 1, CV_64F, T );
  74     CvMat _E = cvMat(3, 3, CV_64F, E );
  75     CvMat _F = cvMat(3, 3, CV_64F, F );
  76     char buf[1024];
  77
  78     if( displayCorners )
  79         cvNamedWindow( "corners", 1 );
  80 // READ IN THE LIST OF CHESSBOARDS:
  81     if( !f )
  82     {
  83         fprintf(stderr, "can not open file %s\n", imageList );
  84         return;
  85     }
  86
  87     if( !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &nx, &ny) != 2 )
  88         return;
  89     n = nx*ny;
  90     temp.resize(n);
  91
  92     for(i=0;;i++)
  93     {
  94         int count = 0, result=0;
  95         lr = i % 2;
  96         vector<CvPoint2D32f>& pts = points[lr];
  97         if( !fgets( buf, sizeof(buf)-3, f ))
  98             break;
  99         size_t len = strlen(buf);
 100         while( len > 0 && isspace(buf[len-1]))
 101             buf[--len] = '\0';
 102         if( buf[0] == '#')
 103             continue;
 104         IplImage* img = cvLoadImage( buf, 0 );
 105         if( !img )
 106             break;
 107         imageSize = cvGetSize(img);
 108         imageNames[lr].push_back(buf);
 109     //FIND CHESSBOARDS AND CORNERS THEREIN:
 110         for( int s = 1; s <= maxScale; s++ )
 111         {
 112             IplImage* timg = img;
 113             if( s > 1 )
 114             {
 115                 timg = cvCreateImage(cvSize(img->width*s,img->height*s),
 116                     img->depth, img->nChannels );
 117                 cvResize( img, timg, CV_INTER_CUBIC );
 118             }
 119             result = cvFindChessboardCorners( timg, cvSize(nx, ny),
 120                 &temp[0], &count,
 121                 CV_CALIB_CB_ADAPTIVE_THRESH |
 122                 CV_CALIB_CB_NORMALIZE_IMAGE);
 123             if( timg != img )
 124                 cvReleaseImage( &timg );
 125             if( result || s == maxScale )
 126                 for( j = 0; j < count; j++ )
 127             {
 128                 temp[j].x /= s;
 129                 temp[j].y /= s;
 130             }
 131             if( result )
 132                 break;
 133         }
 134         if( displayCorners )
 135         {
 136             printf("%s\n", buf);
 137             IplImage* cimg = cvCreateImage( imageSize, 8, 3 );
 138             cvCvtColor( img, cimg, CV_GRAY2BGR );
 139             cvDrawChessboardCorners( cimg, cvSize(nx, ny), &temp[0],
 140                 count, result );
 141             cvShowImage( "corners", cimg );
 142             cvReleaseImage( &cimg );
 143             int c = cvWaitKey(1000);
 144             if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
 145                 exit(-1);
 146         }
 147         else
 148             putchar('.');
 149         N = pts.size();
 150         pts.resize(N + n, cvPoint2D32f(0,0));
 151         active[lr].push_back((uchar)result);
 152     //assert( result != 0 );
 153         if( result )
 154         {
 155          //Calibration will suffer without subpixel interpolation
 156             cvFindCornerSubPix( img, &temp[0], count,
 157                 cvSize(11, 11), cvSize(-1,-1),
 158                 cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,
 159                 30, 0.01) );
 160             copy( temp.begin(), temp.end(), pts.begin() + N );
 161         }
 162         cvReleaseImage( &img );
 163     }
 164     fclose(f);
 165     printf("\n");
 166 // HARVEST CHESSBOARD 3D OBJECT POINT LIST:
 167     nframes = active[0].size();//Number of good chessboads found
 168     objectPoints.resize(nframes*n);
 169     for( i = 0; i < ny; i++ )
 170         for( j = 0; j < nx; j++ )
 171         objectPoints[i*nx + j] =
 172         cvPoint3D32f(i*squareSize, j*squareSize, 0);
 173     for( i = 1; i < nframes; i++ )
 174         copy( objectPoints.begin(), objectPoints.begin() + n,
 175         objectPoints.begin() + i*n );
 176     npoints.resize(nframes,n);
 177     N = nframes*n;
 178     CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] );
 179     CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
 180     CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
 181     CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] );
 182     cvSetIdentity(&_M1);
 183     cvSetIdentity(&_M2);
 184     cvZero(&_D1);
 185     cvZero(&_D2);
 186
 187 // CALIBRATE THE STEREO CAMERAS
 188     printf("Running stereo calibration ...");
 189     fflush(stdout);
 190     cvStereoCalibrate( &_objectPoints, &_imagePoints1,
 191         &_imagePoints2, &_npoints,
 192         &_M1, &_D1, &_M2, &_D2,
 193         imageSize, &_R, &_T, &_E, &_F,
 194         cvTermCriteria(CV_TERMCRIT_ITER+
 195         CV_TERMCRIT_EPS, 100, 1e-5),
 196         CV_CALIB_FIX_ASPECT_RATIO +
 197         CV_CALIB_ZERO_TANGENT_DIST +
 198         CV_CALIB_SAME_FOCAL_LENGTH );
 199     printf(" done\n");
 200 // CALIBRATION QUALITY CHECK
 201 // because the output fundamental matrix implicitly
 202 // includes all the output information,
 203 // we can check the quality of calibration using the
 204 // epipolar geometry constraint: m2^t*F*m1=0
 205     vector<CvPoint3D32f> lines[2];
 206     points[0].resize(N);
 207     points[1].resize(N);
 208     _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
 209     _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
 210     lines[0].resize(N);
 211     lines[1].resize(N);
 212     CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]);
 213     CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]);
 214 //Always work in undistorted space
 215     cvUndistortPoints( &_imagePoints1, &_imagePoints1,
 216         &_M1, &_D1, 0, &_M1 );
 217     cvUndistortPoints( &_imagePoints2, &_imagePoints2,
 218         &_M2, &_D2, 0, &_M2 );
 219     cvComputeCorrespondEpilines( &_imagePoints1, 1, &_F, &_L1 );
 220     cvComputeCorrespondEpilines( &_imagePoints2, 2, &_F, &_L2 );
 221     double avgErr = 0;
 222     for( i = 0; i < N; i++ )
 223     {
 224         double err = fabs(points[0][i].x*lines[1][i].x +
 225             points[0][i].y*lines[1][i].y + lines[1][i].z)
 226             + fabs(points[1][i].x*lines[0][i].x +
 227             points[1][i].y*lines[0][i].y + lines[0][i].z);
 228         avgErr += err;
 229     }
 230     printf( "avg err = %g\n", avgErr/(nframes*n) );
 231 //COMPUTE AND DISPLAY RECTIFICATION
 232     if( showUndistorted )
 233     {
 234         CvMat* mx1 = cvCreateMat( imageSize.height,
 235             imageSize.width, CV_32F );
 236         CvMat* my1 = cvCreateMat( imageSize.height,
 237             imageSize.width, CV_32F );
 238         CvMat* mx2 = cvCreateMat( imageSize.height,
 239
 240             imageSize.width, CV_32F );
 241         CvMat* my2 = cvCreateMat( imageSize.height,
 242             imageSize.width, CV_32F );
 243         CvMat* img1r = cvCreateMat( imageSize.height,
 244             imageSize.width, CV_8U );
 245         CvMat* img2r = cvCreateMat( imageSize.height,
 246             imageSize.width, CV_8U );
 247         CvMat* disp = cvCreateMat( imageSize.height,
 248             imageSize.width, CV_16S );
 249         CvMat* vdisp = cvCreateMat( imageSize.height,
 250             imageSize.width, CV_8U );
 251         CvMat* pair;
 252         double R1[3][3], R2[3][3], P1[3][4], P2[3][4];
 253         CvMat _R1 = cvMat(3, 3, CV_64F, R1);
 254         CvMat _R2 = cvMat(3, 3, CV_64F, R2);
 255 // IF BY CALIBRATED (BOUGUET'S METHOD)
 256         if( useUncalibrated == 0 )
 257         {
 258             CvMat _P1 = cvMat(3, 4, CV_64F, P1);
 259             CvMat _P2 = cvMat(3, 4, CV_64F, P2);
 260             cvStereoRectify( &_M1, &_M2, &_D1, &_D2, imageSize,
 261                 &_R, &_T,
 262                 &_R1, &_R2, &_P1, &_P2, 0,
 263                 0/*CV_CALIB_ZERO_DISPARITY*/ );
 264             isVerticalStereo = fabs(P2[1][3]) > fabs(P2[0][3]);
 265     //Precompute maps for cvRemap()
 266             cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_P1,mx1,my1);
 267             cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_P2,mx2,my2);
 268         }
 269 //OR ELSE HARTLEY'S METHOD
 270         else if( useUncalibrated == 1 || useUncalibrated == 2 )
 271      // use intrinsic parameters of each camera, but
 272      // compute the rectification transformation directly
 273      // from the fundamental matrix
 274         {
 275             double H1[3][3], H2[3][3], iM[3][3];
 276             CvMat _H1 = cvMat(3, 3, CV_64F, H1);
 277             CvMat _H2 = cvMat(3, 3, CV_64F, H2);
 278             CvMat _iM = cvMat(3, 3, CV_64F, iM);
 279     //Just to show you could have independently used F
 280             if( useUncalibrated == 2 )
 281                 cvFindFundamentalMat( &_imagePoints1,
 282                 &_imagePoints2, &_F);
 283             cvStereoRectifyUncalibrated( &_imagePoints1,
 284                 &_imagePoints2, &_F,
 285                 imageSize,
 286                 &_H1, &_H2, 3);
 287             cvInvert(&_M1, &_iM);
 288             cvMatMul(&_H1, &_M1, &_R1);
 289             cvMatMul(&_iM, &_R1, &_R1);
 290             cvInvert(&_M2, &_iM);
 291             cvMatMul(&_H2, &_M2, &_R2);
 292             cvMatMul(&_iM, &_R2, &_R2);
 293     //Precompute map for cvRemap()
 294             cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1);
 295
 296             cvInitUndistortRectifyMap(&_M2,&_D1,&_R2,&_M2,mx2,my2);
 297         }
 298         else
 299             assert(0);
 300         cvNamedWindow( "rectified", 1 );
 301 // RECTIFY THE IMAGES AND FIND DISPARITY MAPS
 302         if( !isVerticalStereo )
 303             pair = cvCreateMat( imageSize.height, imageSize.width*2,
 304             CV_8UC3 );
 305         else
 306             pair = cvCreateMat( imageSize.height*2, imageSize.width,
 307             CV_8UC3 );
 308 //Setup for finding stereo corrrespondences
 309         CvStereoBMState *BMState = cvCreateStereoBMState();
 310         assert(BMState != 0);
 311         BMState->preFilterSize=33;
 312         BMState->preFilterCap=33;
 313         BMState->SADWindowSize=33;
 314         if( useUncalibrated )
 315         {
 316             BMState->minDisparity=-64;
 317             BMState->numberOfDisparities=128;
 318         }
 319         else
 320         {
 321             BMState->minDisparity=-32;
 322             BMState->numberOfDisparities=192;
 323         }
 324         BMState->textureThreshold=10;
 325         BMState->uniquenessRatio=15;
 326         for( i = 0; i < nframes; i++ )
 327         {
 328             IplImage* img1=cvLoadImage(imageNames[0][i].c_str(),0);
 329             IplImage* img2=cvLoadImage(imageNames[1][i].c_str(),0);
 330             if( img1 && img2 )
 331             {
 332                 CvMat part;
 333                 cvRemap( img1, img1r, mx1, my1 );
 334                 cvRemap( img2, img2r, mx2, my2 );
 335                 if( !isVerticalStereo || useUncalibrated != 0 )
 336                 {
 337               // When the stereo camera is oriented vertically,
 338               // useUncalibrated==0 does not transpose the
 339               // image, so the epipolar lines in the rectified
 340               // images are vertical. Stereo correspondence
 341               // function does not support such a case.
 342                     cvFindStereoCorrespondenceBM( img1r, img2r, disp,
 343                         BMState);
 344                     cvNormalize( disp, vdisp, 0, 256, CV_MINMAX );
 345                     cvNamedWindow( "disparity" );
 346                     cvShowImage( "disparity", vdisp );
 347                 }
 348                 if( !isVerticalStereo )
 349                 {
 350                     cvGetCols( pair, &part, 0, imageSize.width );
 351                     cvCvtColor( img1r, &part, CV_GRAY2BGR );
 352                     cvGetCols( pair, &part, imageSize.width,
 353                         imageSize.width*2 );
 354                     cvCvtColor( img2r, &part, CV_GRAY2BGR );
 355                     for( j = 0; j < imageSize.height; j += 16 )
 356                         cvLine( pair, cvPoint(0,j),
 357                         cvPoint(imageSize.width*2,j),
 358                         CV_RGB(0,255,0));
 359                 }
 360                 else
 361                 {
 362                     cvGetRows( pair, &part, 0, imageSize.height );
 363                     cvCvtColor( img1r, &part, CV_GRAY2BGR );
 364                     cvGetRows( pair, &part, imageSize.height,
 365                         imageSize.height*2 );
 366                     cvCvtColor( img2r, &part, CV_GRAY2BGR );
 367                     for( j = 0; j < imageSize.width; j += 16 )
 368                         cvLine( pair, cvPoint(j,0),
 369                         cvPoint(j,imageSize.height*2),
 370                         CV_RGB(0,255,0));
 371                 }
 372                 cvShowImage( "rectified", pair );
 373                 if( cvWaitKey() == 27 )
 374                     break;
 375             }
 376             cvReleaseImage( &img1 );
 377             cvReleaseImage( &img2 );
 378         }
 379         cvReleaseStereoBMState(&BMState);
 380         cvReleaseMat( &mx1 );
 381         cvReleaseMat( &my1 );
 382         cvReleaseMat( &mx2 );
 383         cvReleaseMat( &my2 );
 384         cvReleaseMat( &img1r );
 385         cvReleaseMat( &img2r );
 386         cvReleaseMat( &disp );
 387     }
 388 }
 389
 390 int main(int argc, char** argv)
 391 {
 392     StereoCalib(argc > 1 ? argv[1] : "stereo_calib.txt", 1);
 393     return 0;
 394 }
 395