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 "opencv2/calib3d/calib3d.hpp"
  29 #include "opencv2/highgui/highgui.hpp"
  30 #include "opencv2/imgproc/imgproc_c.h"
  31
  32 #include <vector>
  33 #include <string>
  34 #include <algorithm>
  35 #include <stdio.h>
  36 #include <ctype.h>
  37
  38 using namespace std;
  39
  40 //
  41 // Given a list of chessboard images, the number of corners (nx, ny)
  42 // on the chessboards, and a flag: useCalibrated for calibrated (0) or
  43 // uncalibrated (1: use cvStereoCalibrate(), 2: compute fundamental
  44 // matrix separately) stereo. Calibrate the cameras and display the
  45 // rectified results along with the computed disparity images.
  46 //
  47 static void
  48 StereoCalib(const char* path, const char* imageList, int useUncalibrated)
  49 {
  50     CvRect roi1, roi2;
  51     int nx = 0, ny = 0;
  52     int displayCorners = 1;
  53     int showUndistorted = 1;
  54     bool isVerticalStereo = false;//OpenCV can handle left-right
  55                                       //or up-down camera arrangements
  56     const int maxScale = 1;
  57     const float squareSize = 1.f; //Set this to your actual square size
  58     FILE* f = fopen(imageList, "rt");
  59     int i, j, lr, nframes = 0, n, N = 0;
  60     vector<string> imageNames[2];
  61     vector<CvPoint3D32f> objectPoints;
  62     vector<CvPoint2D32f> points[2];
  63     vector<CvPoint2D32f> temp_points[2];
  64     vector<int> npoints;
  65 //    vector<uchar> active[2];
  66     int is_found[2] = {0, 0};
  67     vector<CvPoint2D32f> temp;
  68     CvSize imageSize = {0,0};
  69     // ARRAY AND VECTOR STORAGE:
  70     double M1[3][3], M2[3][3], D1[5], D2[5];
  71     double R[3][3], T[3], E[3][3], F[3][3];
  72     double Q[4][4];
  73     CvMat _M1 = cvMat(3, 3, CV_64F, M1 );
  74     CvMat _M2 = cvMat(3, 3, CV_64F, M2 );
  75     CvMat _D1 = cvMat(1, 5, CV_64F, D1 );
  76     CvMat _D2 = cvMat(1, 5, CV_64F, D2 );
  77     CvMat matR = cvMat(3, 3, CV_64F, R );
  78     CvMat matT = cvMat(3, 1, CV_64F, T );
  79     CvMat matE = cvMat(3, 3, CV_64F, E );
  80     CvMat matF = cvMat(3, 3, CV_64F, F );
  81
  82     CvMat matQ = cvMat(4, 4, CV_64FC1, Q);
  83
  84     char buf[1024];
  85
  86     if( displayCorners )
  87         cvNamedWindow( "corners", 1 );
  88 // READ IN THE LIST OF CHESSBOARDS:
  89     if( !f )
  90     {
  91         fprintf(stderr, "can not open file %s\n", imageList );
  92         return;
  93     }
  94
  95     if( !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &nx, &ny) != 2 )
  96         return;
  97     n = nx*ny;
  98     temp.resize(n);
  99     temp_points[0].resize(n);
 100     temp_points[1].resize(n);
 101
 102     for(i=0;;i++)
 103     {
 104         int count = 0, result=0;
 105         lr = i % 2;
 106         vector<CvPoint2D32f>& pts = temp_points[lr];//points[lr];
 107         if( !fgets( buf, sizeof(buf)-3, f ))
 108             break;
 109         size_t len = strlen(buf);
 110         while( len > 0 && isspace(buf[len-1]))
 111             buf[--len] = '\0';
 112         if( buf[0] == '#')
 113             continue;
 114         char fullpath[1024];
 115         sprintf(fullpath, "%s/%s", path, buf);
 116         IplImage* img = cvLoadImage( fullpath, 0 );
 117         if( !img )
 118         {
 119             printf("Cannot read file %s\n", fullpath);
 120             return;
 121         }
 122         imageSize = cvGetSize(img);
 123         imageNames[lr].push_back(buf);
 124     //FIND CHESSBOARDS AND CORNERS THEREIN:
 125         for( int s = 1; s <= maxScale; s++ )
 126         {
 127             IplImage* timg = img;
 128             if( s > 1 )
 129             {
 130                 timg = cvCreateImage(cvSize(img->width*s,img->height*s),
 131                     img->depth, img->nChannels );
 132                 cvResize( img, timg, CV_INTER_CUBIC );
 133             }
 134             result = cvFindChessboardCorners( timg, cvSize(nx, ny),
 135                 &temp[0], &count,
 136                 CV_CALIB_CB_ADAPTIVE_THRESH |
 137                 CV_CALIB_CB_NORMALIZE_IMAGE);
 138             if( timg != img )
 139                 cvReleaseImage( &timg );
 140             if( result || s == maxScale )
 141                 for( j = 0; j < count; j++ )
 142             {
 143                 temp[j].x /= s;
 144                 temp[j].y /= s;
 145             }
 146             if( result )
 147                 break;
 148         }
 149         if( displayCorners )
 150         {
 151             printf("%s\n", buf);
 152             IplImage* cimg = cvCreateImage( imageSize, 8, 3 );
 153             cvCvtColor( img, cimg, CV_GRAY2BGR );
 154             cvDrawChessboardCorners( cimg, cvSize(nx, ny), &temp[0],
 155                 count, result );
 156             IplImage* cimg1 = cvCreateImage(cvSize(640, 480), IPL_DEPTH_8U, 3);
 157             cvResize(cimg, cimg1);
 158             cvShowImage( "corners", cimg1 );
 159             cvReleaseImage( &cimg );
 160             cvReleaseImage( &cimg1 );
 161             int c = cvWaitKey(1000);
 162             if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
 163                 exit(-1);
 164         }
 165         else
 166             putchar('.');
 167         //N = pts.size();
 168         //pts.resize(N + n, cvPoint2D32f(0,0));
 169         //active[lr].push_back((uchar)result);
 170         is_found[lr] = result > 0 ? 1 : 0;
 171     //assert( result != 0 );
 172         if( result )
 173         {
 174          //Calibration will suffer without subpixel interpolation
 175             cvFindCornerSubPix( img, &temp[0], count,
 176                 cvSize(11, 11), cvSize(-1,-1),
 177                 cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,
 178                 30, 0.01) );
 179             copy( temp.begin(), temp.end(), pts.begin() );
 180         }
 181         cvReleaseImage( &img );
 182
 183         if(lr)
 184         {
 185             if(is_found[0] == 1 && is_found[1] == 1)
 186             {
 187                 assert(temp_points[0].size() == temp_points[1].size());
 188                 int current_size = points[0].size();
 189
 190                 points[0].resize(current_size + temp_points[0].size(), cvPoint2D32f(0.0, 0.0));
 191                 points[1].resize(current_size + temp_points[1].size(), cvPoint2D32f(0.0, 0.0));
 192
 193                 copy(temp_points[0].begin(), temp_points[0].end(), points[0].begin() + current_size);
 194                 copy(temp_points[1].begin(), temp_points[1].end(), points[1].begin() + current_size);
 195
 196                 nframes++;
 197
 198                 printf("Pair successfully detected...\n");
 199             }
 200
 201             is_found[0] = 0;
 202             is_found[1] = 0;
 203
 204         }
 205     }
 206     fclose(f);
 207     printf("\n");
 208 // HARVEST CHESSBOARD 3D OBJECT POINT LIST:
 209     objectPoints.resize(nframes*n);
 210     for( i = 0; i < ny; i++ )
 211         for( j = 0; j < nx; j++ )
 212         objectPoints[i*nx + j] =
 213         cvPoint3D32f(i*squareSize, j*squareSize, 0);
 214     for( i = 1; i < nframes; i++ )
 215         copy( objectPoints.begin(), objectPoints.begin() + n,
 216         objectPoints.begin() + i*n );
 217     npoints.resize(nframes,n);
 218     N = nframes*n;
 219     CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] );
 220     CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
 221     CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
 222     CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] );
 223     cvSetIdentity(&_M1);
 224     cvSetIdentity(&_M2);
 225     cvZero(&_D1);
 226     cvZero(&_D2);
 227
 228 // CALIBRATE THE STEREO CAMERAS
 229     printf("Running stereo calibration ...");
 230     fflush(stdout);
 231     cvStereoCalibrate( &_objectPoints, &_imagePoints1,
 232         &_imagePoints2, &_npoints,
 233         &_M1, &_D1, &_M2, &_D2,
 234         imageSize, &matR, &matT, &matE, &matF,
 235         cvTermCriteria(CV_TERMCRIT_ITER+
 236         CV_TERMCRIT_EPS, 100, 1e-5),
 237         CV_CALIB_FIX_ASPECT_RATIO +
 238         CV_CALIB_ZERO_TANGENT_DIST +
 239         CV_CALIB_SAME_FOCAL_LENGTH +
 240         CV_CALIB_FIX_K3);
 241     printf(" done\n");
 242
 243 // CALIBRATION QUALITY CHECK
 244 // because the output fundamental matrix implicitly
 245 // includes all the output information,
 246 // we can check the quality of calibration using the
 247 // epipolar geometry constraint: m2^t*F*m1=0
 248     vector<CvPoint3D32f> lines[2];
 249     points[0].resize(N);
 250     points[1].resize(N);
 251     _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
 252     _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
 253     lines[0].resize(N);
 254     lines[1].resize(N);
 255     CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]);
 256     CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]);
 257 //Always work in undistorted space
 258     cvUndistortPoints( &_imagePoints1, &_imagePoints1,
 259         &_M1, &_D1, 0, &_M1 );
 260     cvUndistortPoints( &_imagePoints2, &_imagePoints2,
 261         &_M2, &_D2, 0, &_M2 );
 262     cvComputeCorrespondEpilines( &_imagePoints1, 1, &matF, &_L1 );
 263     cvComputeCorrespondEpilines( &_imagePoints2, 2, &matF, &_L2 );
 264     double avgErr = 0;
 265     for( i = 0; i < N; i++ )
 266     {
 267         double err = fabs(points[0][i].x*lines[1][i].x +
 268             points[0][i].y*lines[1][i].y + lines[1][i].z)
 269             + fabs(points[1][i].x*lines[0][i].x +
 270             points[1][i].y*lines[0][i].y + lines[0][i].z);
 271         avgErr += err;
 272     }
 273     printf( "avg err = %g\n", avgErr/(nframes*n) );
 274
 275     // save intrinsic parameters
 276     CvFileStorage* fstorage = cvOpenFileStorage("intrinsics.yml", NULL, CV_STORAGE_WRITE);
 277     cvWrite(fstorage, "M1", &_M1);
 278     cvWrite(fstorage, "D1", &_D1);
 279     cvWrite(fstorage, "M2", &_M2);
 280     cvWrite(fstorage, "D2", &_D2);
 281     cvReleaseFileStorage(&fstorage);
 282
 283 //COMPUTE AND DISPLAY RECTIFICATION
 284     if( showUndistorted )
 285     {
 286         CvMat* mx1 = cvCreateMat( imageSize.height,
 287             imageSize.width, CV_32F );
 288         CvMat* my1 = cvCreateMat( imageSize.height,
 289             imageSize.width, CV_32F );
 290         CvMat* mx2 = cvCreateMat( imageSize.height,
 291             imageSize.width, CV_32F );
 292         CvMat* my2 = cvCreateMat( imageSize.height,
 293             imageSize.width, CV_32F );
 294         CvMat* img1r = cvCreateMat( imageSize.height,
 295             imageSize.width, CV_8U );
 296         CvMat* img2r = cvCreateMat( imageSize.height,
 297             imageSize.width, CV_8U );
 298         CvMat* disp = cvCreateMat( imageSize.height,
 299             imageSize.width, CV_16S );
 300         double R1[3][3], R2[3][3], P1[3][4], P2[3][4];
 301         CvMat _R1 = cvMat(3, 3, CV_64F, R1);
 302         CvMat _R2 = cvMat(3, 3, CV_64F, R2);
 303 // IF BY CALIBRATED (BOUGUET'S METHOD)
 304         if( useUncalibrated == 0 )
 305         {
 306             CvMat _P1 = cvMat(3, 4, CV_64F, P1);
 307             CvMat _P2 = cvMat(3, 4, CV_64F, P2);
 308
 309             cvStereoRectify( &_M1, &_M2, &_D1, &_D2, imageSize,
 310                 &matR, &matT,
 311                 &_R1, &_R2, &_P1, &_P2, &matQ,
 312                 CV_CALIB_ZERO_DISPARITY,
 313                 1, imageSize, &roi1, &roi2);
 314
 315             CvFileStorage* file = cvOpenFileStorage("extrinsics.yml", NULL, CV_STORAGE_WRITE);
 316             cvWrite(file, "R", &matR);
 317             cvWrite(file, "T", &matT);
 318             cvWrite(file, "R1", &_R1);
 319             cvWrite(file, "R2", &_R2);
 320             cvWrite(file, "P1", &_P1);
 321             cvWrite(file, "P2", &_P2);
 322             cvWrite(file, "Q", &matQ);
 323             cvReleaseFileStorage(&file);
 324
 325             isVerticalStereo = fabs(P2[1][3]) > fabs(P2[0][3]);
 326             if(!isVerticalStereo)
 327                 roi2.x += imageSize.width;
 328             else
 329                 roi2.y += imageSize.height;
 330     //Precompute maps for cvRemap()
 331             cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_P1,mx1,my1);
 332             cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_P2,mx2,my2);
 333         }
 334 //OR ELSE HARTLEY'S METHOD
 335         else if( useUncalibrated == 1 || useUncalibrated == 2 )
 336      // use intrinsic parameters of each camera, but
 337      // compute the rectification transformation directly
 338      // from the fundamental matrix
 339         {
 340             double H1[3][3], H2[3][3], iM[3][3];
 341             CvMat _H1 = cvMat(3, 3, CV_64F, H1);
 342             CvMat _H2 = cvMat(3, 3, CV_64F, H2);
 343             CvMat _iM = cvMat(3, 3, CV_64F, iM);
 344     //Just to show you could have independently used F
 345             if( useUncalibrated == 2 )
 346                 cvFindFundamentalMat( &_imagePoints1,
 347                 &_imagePoints2, &matF);
 348             cvStereoRectifyUncalibrated( &_imagePoints1,
 349                 &_imagePoints2, &matF,
 350                 imageSize,
 351                 &_H1, &_H2, 3);
 352             cvInvert(&_M1, &_iM);
 353             cvMatMul(&_H1, &_M1, &_R1);
 354             cvMatMul(&_iM, &_R1, &_R1);
 355             cvInvert(&_M2, &_iM);
 356             cvMatMul(&_H2, &_M2, &_R2);
 357             cvMatMul(&_iM, &_R2, &_R2);
 358     //Precompute map for cvRemap()
 359             cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1);
 360
 361             cvInitUndistortRectifyMap(&_M2,&_D1,&_R2,&_M2,mx2,my2);
 362         }
 363         else
 364             assert(0);
 365
 366
 367         cvReleaseMat( &mx1 );
 368         cvReleaseMat( &my1 );
 369         cvReleaseMat( &mx2 );
 370         cvReleaseMat( &my2 );
 371         cvReleaseMat( &img1r );
 372         cvReleaseMat( &img2r );
 373         cvReleaseMat( &disp );
 374     }
 375 }
 376
 377 int main(int argc, char** argv)
 378 {
 379     if(argc > 1 && !strcmp(argv[1], "--help"))
 380     {
 381         printf("Usage:\n ./stereo_calib <path to images> <file wtih image list>\n");
 382         return 0;
 383     }
 384
 385     StereoCalib(argc > 1 ? argv[1] : ".", argc > 2 ? argv[2] : "stereo_calib.txt", 0);
 386     return 0;
 387 }
 388