uvsim: /home/jgoppert/Projects/uvsim/uvsim/src/test/stereoCalib.cc Source File

00001 /* This is sample from the OpenCV book. The copyright notice is below */
00002 
00003 /* *************** License:**************************
00004    Oct. 3, 2008
00005    Right to use this code in any way you want without warrenty, support or any guarentee of it working.
00006 
00007    BOOK: It would be nice if you cited it:
00008    Learning OpenCV: Computer Vision with the OpenCV Library
00009      by Gary Bradski and Adrian Kaehler
00010      Published by O'Reilly Media, October 3, 2008
00011 
00012    AVAILABLE AT:
00013      http://www.amazon.com/Learning-OpenCV-Computer-Vision-Library/dp/0596516134
00014      Or: http://oreilly.com/catalog/9780596516130/
00015      ISBN-10: 0596516134 or: ISBN-13: 978-0596516130
00016 
00017    OTHER OPENCV SITES:
00018    * The source code is on sourceforge at:
00019      http://sourceforge.net/projects/opencvlibrary/
00020    * The OpenCV wiki page (As of Oct 1, 2008 this is down for changing over servers, but should come back):
00021      http://opencvlibrary.sourceforge.net/
00022    * An active user group is at:
00023      http://tech.groups.yahoo.com/group/OpenCV/
00024    * The minutes of weekly OpenCV development meetings are at:
00025      http://pr.willowgarage.com/wiki/OpenCV
00026    ************************************************** */
00027 
00028 #include "cv.h"
00029 #include "cxmisc.h"
00030 #include "highgui.h"
00031 #include <vector>
00032 #include <string>
00033 #include <algorithm>
00034 #include <stdio.h>
00035 #include <ctype.h>
00036 #include <iostream>
00037 
00038 //
00039 // Given a list of chessboard images, the number of corners (nx, ny)
00040 // on the chessboards, and a flag: useCalibrated for calibrated (0) or
00041 // uncalibrated (1: use cvStereoCalibrate(), 2: compute fundamental
00042 // matrix separately) stereo. Calibrate the cameras and display the
00043 // rectified results along with the computed disparity images.
00044 //
00045 static void
00046 StereoCalib(string imageList, int nx, int ny, int useUncalibrated)
00047 {
00048     int displayCorners = 1;
00049     int showUndistorted = 1;
00050     bool isVerticalStereo = false;//OpenCV can handle left-right
00051     //or up-down camera arrangements
00052     const int maxScale = 1;
00053     const float squareSize = .03f; //Set this to your actual square size
00054     FILE* f = fopen(imageList.c_str(), "rt");
00055     int i, j, lr, nframes, n = nx*ny, N = 0;
00056     vector<string> imageNames[2];
00057     vector<CvPoint3D32f> objectPoints;
00058     vector<CvPoint2D32f> points[2];
00059     vector<int> npoints;
00060     vector<uchar> active[2];
00061     vector<CvPoint2D32f> temp(n);
00062     CvSize imageSize = {0,0};
00063     // ARRAY AND VECTOR STORAGE:
00064     double M1[3][3], M2[3][3], D1[5], D2[5];
00065     double R[3][3], T[3], E[3][3], F[3][3];
00066     CvMat _M1 = cvMat(3, 3, CV_64F, M1 );
00067     CvMat _M2 = cvMat(3, 3, CV_64F, M2 );
00068     CvMat _D1 = cvMat(1, 5, CV_64F, D1 );
00069     CvMat _D2 = cvMat(1, 5, CV_64F, D2 );
00070     CvMat _R = cvMat(3, 3, CV_64F, R );
00071     CvMat _T = cvMat(3, 1, CV_64F, T );
00072     CvMat _E = cvMat(3, 3, CV_64F, E );
00073     CvMat _F = cvMat(3, 3, CV_64F, F );
00074     if ( displayCorners )
00075         cvNamedWindow( "corners", 1 );
00076 // READ IN THE LIST OF CHESSBOARDS:
00077     if ( !f )
00078     {
00079         fprintf(stderr, "can not open file %s\n", imageList.c_str() );
00080         return;
00081     }
00082     for (i=0;;i++)
00083     {
00084         char buf[1024];
00085         int count = 0, result=0;
00086         lr = i % 2;
00087         vector<CvPoint2D32f>& pts = points[lr];
00088         if ( !fgets( buf, sizeof(buf)-3, f ))
00089             break;
00090         size_t len = strlen(buf);
00091         while ( len > 0 && isspace(buf[len-1]))
00092             buf[--len] = '\0';
00093         if ( buf[0] == '#')
00094             continue;
00095         IplImage* img = cvLoadImage( buf, 0 );
00096         if ( !img )
00097             break;
00098         imageSize = cvGetSize(img);
00099         imageNames[lr].push_back(buf);
00100         //FIND CHESSBOARDS AND CORNERS THEREIN:
00101         for ( int s = 1; s <= maxScale; s++ )
00102         {
00103             IplImage* timg = img;
00104             if ( s > 1 )
00105             {
00106                 timg = cvCreateImage(cvSize(img->width*s,img->height*s),
00107                                      img->depth, img->nChannels );
00108                 cvResize( img, timg, CV_INTER_CUBIC );
00109             }
00110             result = cvFindChessboardCorners( timg, cvSize(nx, ny),
00111                                               &temp[0], &count,
00112                                               CV_CALIB_CB_ADAPTIVE_THRESH |
00113                                               CV_CALIB_CB_NORMALIZE_IMAGE);
00114             if ( timg != img )
00115                 cvReleaseImage( &timg );
00116             if ( result || s == maxScale )
00117                 for ( j = 0; j < count; j++ )
00118                 {
00119                     temp[j].x /= s;
00120                     temp[j].y /= s;
00121                 }
00122             if ( result )
00123                 break;
00124         }
00125         if ( displayCorners )
00126         {
00127             printf("%s\n", buf);
00128             IplImage* cimg = cvCreateImage( imageSize, 8, 3 );
00129 
00130             cvCvtColor( img, cimg, CV_GRAY2BGR );
00131             cvDrawChessboardCorners( cimg, cvSize(nx, ny), &temp[0],
00132                                      count, result );
00133             cvShowImage( "corners", cimg );
00134             cvReleaseImage( &cimg );
00135             int c = cvWaitKey(1000);
00136             if ( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
00137                 exit(-1);
00138         }
00139         else
00140             putchar('.');
00141         N = pts.size();
00142         pts.resize(N + n, cvPoint2D32f(0,0));
00143         active[lr].push_back((uchar)result);
00144         //assert( result != 0 );
00145         if ( result )
00146         {
00147             //Calibration will suffer without subpixel interpolation
00148             cvFindCornerSubPix( img, &temp[0], count,
00149                                 cvSize(11, 11), cvSize(-1,-1),
00150                                 cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,
00151                                                30, 0.01) );
00152             copy( temp.begin(), temp.end(), pts.begin() + N );
00153         }
00154         cvReleaseImage( &img );
00155     }
00156     fclose(f);
00157     printf("\n");
00158 // HARVEST CHESSBOARD 3D OBJECT POINT LIST:
00159     nframes = active[0].size();//Number of good chessboads found
00160         std::cout<<"nframes- "<<nframes<<std::endl;
00161     objectPoints.resize(nframes*n);
00162     for ( i = 0; i < ny; i++ )
00163         for ( j = 0; j < nx; j++ )
00164             objectPoints[i*nx + j] =
00165                 cvPoint3D32f(i*squareSize, j*squareSize, 0);
00166     for ( i = 1; i < nframes; i++ )
00167         copy( objectPoints.begin(), objectPoints.begin() + n,
00168               objectPoints.begin() + i*n );
00169     npoints.resize(nframes,n);
00170     N = nframes*n;
00171     CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] );
00172     CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
00173     CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
00174     CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] );
00175     cvSetIdentity(&_M1);
00176     cvSetIdentity(&_M2);
00177     cvZero(&_D1);
00178     cvZero(&_D2);
00179 
00180 // CALIBRATE THE STEREO CAMERAS
00181     printf("Running stereo calibration ...");
00182     fflush(stdout);
00183     cvStereoCalibrate( &_objectPoints, &_imagePoints1,
00184                        &_imagePoints2, &_npoints,
00185                        &_M1, &_D1, &_M2, &_D2,
00186                        imageSize, &_R, &_T, &_E, &_F,
00187                        cvTermCriteria(CV_TERMCRIT_ITER+
00188                                       CV_TERMCRIT_EPS, 100, 1e-5),
00189                        CV_CALIB_FIX_ASPECT_RATIO +
00190                        CV_CALIB_ZERO_TANGENT_DIST +
00191                        CV_CALIB_SAME_FOCAL_LENGTH );
00192     printf(" done\n");
00193 // CALIBRATION QUALITY CHECK
00194 // because the output fundamental matrix implicitly
00195 // includes all the output information,
00196 // we can check the quality of calibration using the
00197 // epipolar geometry constraint: m2^t*F*m1=0
00198     vector<CvPoint3D32f> lines[2];
00199     points[0].resize(N);
00200     points[1].resize(N);
00201     _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
00202     _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
00203     lines[0].resize(N);
00204     lines[1].resize(N);
00205     CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]);
00206     CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]);
00207 //Always work in undistorted space
00208     cvUndistortPoints( &_imagePoints1, &_imagePoints1,
00209                        &_M1, &_D1, 0, &_M1 );
00210     cvUndistortPoints( &_imagePoints2, &_imagePoints2,
00211                        &_M2, &_D2, 0, &_M2 );
00212     cvComputeCorrespondEpilines( &_imagePoints1, 1, &_F, &_L1 );
00213     cvComputeCorrespondEpilines( &_imagePoints2, 2, &_F, &_L2 );
00214     double avgErr = 0;
00215     for ( i = 0; i < N; i++ )
00216     {
00217         double err = fabs(points[0][i].x*lines[1][i].x +
00218                           points[0][i].y*lines[1][i].y + lines[1][i].z)
00219                      + fabs(points[1][i].x*lines[0][i].x +
00220                             points[1][i].y*lines[0][i].y + lines[0][i].z);
00221         avgErr += err;
00222     }
00223     printf( "avg err = %g\n", avgErr/(nframes*n) );
00224 
00225 //Save the calibration data
00226     cvSave("M1.xml",&_M1);
00227     cvSave("M2.xml",&_M2);
00228 
00229     cvSave("D1.xml",&_D1);
00230     cvSave("D2.xml",&_D2);
00231 
00232     cvSave("R.xml",&_R);
00233     cvSave("T.xml",&_T);
00234 
00235     cvSave("E.xml",&_E);
00236     cvSave("F.xml",&_F);
00237 
00238 //COMPUTE AND DISPLAY RECTIFICATION
00239     if ( showUndistorted )
00240     {
00241         CvMat* mx1 = cvCreateMat( imageSize.height,
00242                                   imageSize.width, CV_32F );
00243         CvMat* my1 = cvCreateMat( imageSize.height,
00244                                   imageSize.width, CV_32F );
00245         CvMat* mx2 = cvCreateMat( imageSize.height,
00246 
00247                                   imageSize.width, CV_32F );
00248         CvMat* my2 = cvCreateMat( imageSize.height,
00249                                   imageSize.width, CV_32F );
00250         CvMat* img1r = cvCreateMat( imageSize.height,
00251                                     imageSize.width, CV_8U );
00252         CvMat* img2r = cvCreateMat( imageSize.height,
00253                                     imageSize.width, CV_8U );
00254         CvMat* disp = cvCreateMat( imageSize.height,
00255                                    imageSize.width, CV_16S );
00256         CvMat* vdisp = cvCreateMat( imageSize.height,
00257                                     imageSize.width, CV_8U );
00258         CvMat* pair;
00259         double R1[3][3], R2[3][3], P1[3][4], P2[3][4];
00260         CvMat _R1 = cvMat(3, 3, CV_64F, R1);
00261         CvMat _R2 = cvMat(3, 3, CV_64F, R2);
00262 // IF BY CALIBRATED (BOUGUET'S METHOD)
00263         if ( useUncalibrated == 0 )
00264         {
00265             CvMat _P1 = cvMat(3, 4, CV_64F, P1);
00266             CvMat _P2 = cvMat(3, 4, CV_64F, P2);
00267             cvStereoRectify( &_M1, &_M2, &_D1, &_D2, imageSize,
00268                              &_R, &_T,
00269                              &_R1, &_R2, &_P1, &_P2, 0,
00270                              0/*CV_CALIB_ZERO_DISPARITY*/ );
00271             isVerticalStereo = fabs(P2[1][3]) > fabs(P2[0][3]);
00272             //Precompute maps for cvRemap()
00273             cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_P1,mx1,my1);
00274             cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_P2,mx2,my2);
00275         }
00276 //OR ELSE HARTLEY'S METHOD
00277         else if ( useUncalibrated == 1 || useUncalibrated == 2 )
00278             // use intrinsic parameters of each camera, but
00279             // compute the rectification transformation directly
00280             // from the fundamental matrix
00281         {
00282             std::cout<<"Using Uncalibrated method"<<std::endl;
00283             double H1[3][3], H2[3][3], iM[3][3];
00284             CvMat _H1 = cvMat(3, 3, CV_64F, H1);
00285             CvMat _H2 = cvMat(3, 3, CV_64F, H2);
00286             CvMat _iM = cvMat(3, 3, CV_64F, iM);
00287             //Just to show you could have independently used F
00288             if ( useUncalibrated == 2 )
00289                 cvFindFundamentalMat( &_imagePoints1,
00290                                       &_imagePoints2, &_F);
00291             cvStereoRectifyUncalibrated( &_imagePoints1,
00292                                          &_imagePoints2, &_F,
00293                                          imageSize,
00294                                          &_H1, &_H2, 3);
00295             cvInvert(&_M1, &_iM);
00296             cvMatMul(&_H1, &_M1, &_R1);
00297             cvMatMul(&_iM, &_R1, &_R1);
00298             cvInvert(&_M2, &_iM);
00299             cvMatMul(&_H2, &_M2, &_R2);
00300             cvMatMul(&_iM, &_R2, &_R2);
00301             //Precompute map for cvRemap()
00302             cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1);
00303 
00304             cvInitUndistortRectifyMap(&_M2,&_D1,&_R2,&_M2,mx2,my2);
00305 
00306         }
00307         else
00308             assert(0);
00309 
00310 
00311         cvNamedWindow( "rectified", 1 );
00312 // RECTIFY THE IMAGES AND FIND DISPARITY MAPS
00313         if ( !isVerticalStereo )
00314             pair = cvCreateMat( imageSize.height, imageSize.width*2,
00315                                 CV_8UC3 );
00316         else
00317             pair = cvCreateMat( imageSize.height*2, imageSize.width,
00318                                 CV_8UC3 );
00319 //Setup for finding stereo corrrespondences
00320         CvStereoBMState *BMState = cvCreateStereoBMState();
00321         assert(BMState != 0);
00322         BMState->preFilterSize=41;
00323         BMState->preFilterCap=31;
00324         BMState->SADWindowSize=41;
00325         BMState->minDisparity=-64;
00326         BMState->numberOfDisparities=128;
00327         BMState->textureThreshold=10;
00328         BMState->uniquenessRatio=15;
00329         for ( i = 0; i < nframes; i++ )
00330         {
00331             IplImage* img1=cvLoadImage(imageNames[0][i].c_str(),0);
00332             IplImage* img2=cvLoadImage(imageNames[1][i].c_str(),0);
00333             if ( img1 && img2 )
00334             {
00335                 CvMat part;
00336                 cvRemap( img1, img1r, mx1, my1 );
00337                 cvRemap( img2, img2r, mx2, my2 );
00338                 if ( !isVerticalStereo || useUncalibrated != 0 )
00339                 {
00340                     // When the stereo camera is oriented vertically,
00341                     // useUncalibrated==0 does not transpose the
00342                     // image, so the epipolar lines in the rectified
00343                     // images are vertical. Stereo correspondence
00344                     // function does not support such a case.
00345                     cvFindStereoCorrespondenceBM( img1r, img2r, disp,
00346                                                   BMState);
00347                     cvNormalize( disp, vdisp, 0, 256, CV_MINMAX );
00348                     cvNamedWindow( "disparity" );
00349                     cvShowImage( "disparity", vdisp );
00350                     cvSave("dispSC.xml",disp);
00351                     cvSave("vdispSC.xml",vdisp);
00352 
00353                 }
00354                 if ( !isVerticalStereo )
00355                 {
00356                     cvGetCols( pair, &part, 0, imageSize.width );
00357                     cvCvtColor( img1r, &part, CV_GRAY2BGR );
00358                     cvGetCols( pair, &part, imageSize.width,
00359                                imageSize.width*2 );
00360                     cvCvtColor( img2r, &part, CV_GRAY2BGR );
00361                     for ( j = 0; j < imageSize.height; j += 16 )
00362                         cvLine( pair, cvPoint(0,j),
00363                                 cvPoint(imageSize.width*2,j),
00364                                 CV_RGB(0,255,0));
00365                 }
00366                 else
00367                 {
00368                     cvGetRows( pair, &part, 0, imageSize.height );
00369                     cvCvtColor( img1r, &part, CV_GRAY2BGR );
00370                     cvGetRows( pair, &part, imageSize.height,
00371                                imageSize.height*2 );
00372                     cvCvtColor( img2r, &part, CV_GRAY2BGR );
00373                     for ( j = 0; j < imageSize.width; j += 16 )
00374                         cvLine( pair, cvPoint(j,0),
00375                                 cvPoint(j,imageSize.height*2),
00376                                 CV_RGB(0,255,0));
00377                 }
00378                 cvShowImage( "rectified", pair );
00379                 if ( cvWaitKey() == 27 )
00380                     break;
00381             }
00382             cvReleaseImage( &img1 );
00383             cvReleaseImage( &img2 );
00384         }
00385         cvReleaseStereoBMState(&BMState);
00386         cvReleaseMat( &mx1 );
00387         cvReleaseMat( &my1 );
00388         cvReleaseMat( &mx2 );
00389         cvReleaseMat( &my2 );
00390         cvReleaseMat( &img1r );
00391         cvReleaseMat( &img2r );
00392         cvReleaseMat( &disp );
00393     }
00394 }
00395 
00396 int main(void)
00397 {
00398     //StereoCalib("Images/stereo_calib.txt", 9, 6, 0);
00399     StereoCalib(std::string(DATADIR)+="/data/stereo_calib.txt",8,5,0);
00400     return 0;
00401 }