1 /* This is sample from the OpenCV book. The copyright notice is below */
3 /* *************** License:**************************
5 Right to use this code in any way you want without warrenty, support or any guarentee of it working.
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
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
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 ************************************************** */
42 // Given a list of chessboard images, the number of corners (nx, ny)
43 // on the chessboards, and a flag: useCalibrated for calibrated (0) or
44 // uncalibrated (1: use cvStereoCalibrate(), 2: compute fundamental
45 // matrix separately) stereo. Calibrate the cameras and display the
46 // rectified results along with the computed disparity images.
49 StereoCalib(const char* imageList, int useUncalibrated)
53 int displayCorners = 1;
54 int showUndistorted = 1;
55 bool isVerticalStereo = false;//OpenCV can handle left-right
56 //or up-down camera arrangements
57 const int maxScale = 1;
58 const float squareSize = 1.f; //Set this to your actual square size
59 FILE* f = fopen(imageList, "rt");
60 int i, j, lr, nframes = 0, n, N = 0;
61 vector<string> imageNames[2];
62 vector<CvPoint3D32f> objectPoints;
63 vector<CvPoint2D32f> points[2];
64 vector<CvPoint2D32f> temp_points[2];
66 // vector<uchar> active[2];
67 int is_found[2] = {0, 0};
68 vector<CvPoint2D32f> temp;
69 CvSize imageSize = {0,0};
70 // ARRAY AND VECTOR STORAGE:
71 double M1[3][3], M2[3][3], D1[5], D2[5];
72 double R[3][3], T[3], E[3][3], F[3][3];
74 CvMat _M1 = cvMat(3, 3, CV_64F, M1 );
75 CvMat _M2 = cvMat(3, 3, CV_64F, M2 );
76 CvMat _D1 = cvMat(1, 5, CV_64F, D1 );
77 CvMat _D2 = cvMat(1, 5, CV_64F, D2 );
78 CvMat matR = cvMat(3, 3, CV_64F, R );
79 CvMat matT = cvMat(3, 1, CV_64F, T );
80 CvMat matE = cvMat(3, 3, CV_64F, E );
81 CvMat matF = cvMat(3, 3, CV_64F, F );
83 CvMat matQ = cvMat(4, 4, CV_64FC1, Q);
88 cvNamedWindow( "corners", 1 );
89 // READ IN THE LIST OF CHESSBOARDS:
92 fprintf(stderr, "can not open file %s\n", imageList );
96 if( !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &nx, &ny) != 2 )
100 temp_points[0].resize(n);
101 temp_points[1].resize(n);
105 int count = 0, result=0;
107 vector<CvPoint2D32f>& pts = temp_points[lr];//points[lr];
108 if( !fgets( buf, sizeof(buf)-3, f ))
110 size_t len = strlen(buf);
111 while( len > 0 && isspace(buf[len-1]))
115 IplImage* img = cvLoadImage( buf, 0 );
118 imageSize = cvGetSize(img);
119 imageNames[lr].push_back(buf);
120 //FIND CHESSBOARDS AND CORNERS THEREIN:
121 for( int s = 1; s <= maxScale; s++ )
123 IplImage* timg = img;
126 timg = cvCreateImage(cvSize(img->width*s,img->height*s),
127 img->depth, img->nChannels );
128 cvResize( img, timg, CV_INTER_CUBIC );
130 result = cvFindChessboardCorners( timg, cvSize(nx, ny),
132 CV_CALIB_CB_ADAPTIVE_THRESH |
133 CV_CALIB_CB_NORMALIZE_IMAGE);
135 cvReleaseImage( &timg );
136 if( result || s == maxScale )
137 for( j = 0; j < count; j++ )
148 IplImage* cimg = cvCreateImage( imageSize, 8, 3 );
149 cvCvtColor( img, cimg, CV_GRAY2BGR );
150 cvDrawChessboardCorners( cimg, cvSize(nx, ny), &temp[0],
152 IplImage* cimg1 = cvCreateImage(cvSize(640, 480), IPL_DEPTH_8U, 3);
153 cvResize(cimg, cimg1);
154 cvShowImage( "corners", cimg1 );
155 cvReleaseImage( &cimg );
156 cvReleaseImage( &cimg1 );
157 int c = cvWaitKey(1000);
158 if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
164 //pts.resize(N + n, cvPoint2D32f(0,0));
165 //active[lr].push_back((uchar)result);
166 is_found[lr] = result > 0 ? 1 : 0;
167 //assert( result != 0 );
170 //Calibration will suffer without subpixel interpolation
171 cvFindCornerSubPix( img, &temp[0], count,
172 cvSize(11, 11), cvSize(-1,-1),
173 cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,
175 copy( temp.begin(), temp.end(), pts.begin() );
177 cvReleaseImage( &img );
181 if(is_found[0] == 1 && is_found[1] == 1)
183 assert(temp_points[0].size() == temp_points[1].size());
184 int current_size = points[0].size();
186 points[0].resize(current_size + temp_points[0].size(), cvPoint2D32f(0.0, 0.0));
187 points[1].resize(current_size + temp_points[1].size(), cvPoint2D32f(0.0, 0.0));
189 copy(temp_points[0].begin(), temp_points[0].end(), points[0].begin() + current_size);
190 copy(temp_points[1].begin(), temp_points[1].end(), points[1].begin() + current_size);
194 printf("Pair successfully detected...\n");
204 // HARVEST CHESSBOARD 3D OBJECT POINT LIST:
205 objectPoints.resize(nframes*n);
206 for( i = 0; i < ny; i++ )
207 for( j = 0; j < nx; j++ )
208 objectPoints[i*nx + j] =
209 cvPoint3D32f(i*squareSize, j*squareSize, 0);
210 for( i = 1; i < nframes; i++ )
211 copy( objectPoints.begin(), objectPoints.begin() + n,
212 objectPoints.begin() + i*n );
213 npoints.resize(nframes,n);
215 CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] );
216 CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
217 CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
218 CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] );
224 // CALIBRATE THE STEREO CAMERAS
225 printf("Running stereo calibration ...");
227 cvStereoCalibrate( &_objectPoints, &_imagePoints1,
228 &_imagePoints2, &_npoints,
229 &_M1, &_D1, &_M2, &_D2,
230 imageSize, &matR, &matT, &matE, &matF,
231 cvTermCriteria(CV_TERMCRIT_ITER+
232 CV_TERMCRIT_EPS, 100, 1e-5),
233 CV_CALIB_FIX_ASPECT_RATIO +
234 CV_CALIB_ZERO_TANGENT_DIST +
235 CV_CALIB_SAME_FOCAL_LENGTH +
239 // CALIBRATION QUALITY CHECK
240 // because the output fundamental matrix implicitly
241 // includes all the output information,
242 // we can check the quality of calibration using the
243 // epipolar geometry constraint: m2^t*F*m1=0
244 vector<CvPoint3D32f> lines[2];
247 _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
248 _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
251 CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]);
252 CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]);
253 //Always work in undistorted space
254 cvUndistortPoints( &_imagePoints1, &_imagePoints1,
255 &_M1, &_D1, 0, &_M1 );
256 cvUndistortPoints( &_imagePoints2, &_imagePoints2,
257 &_M2, &_D2, 0, &_M2 );
258 cvComputeCorrespondEpilines( &_imagePoints1, 1, &matF, &_L1 );
259 cvComputeCorrespondEpilines( &_imagePoints2, 2, &matF, &_L2 );
261 for( i = 0; i < N; i++ )
263 double err = fabs(points[0][i].x*lines[1][i].x +
264 points[0][i].y*lines[1][i].y + lines[1][i].z)
265 + fabs(points[1][i].x*lines[0][i].x +
266 points[1][i].y*lines[0][i].y + lines[0][i].z);
269 printf( "avg err = %g\n", avgErr/(nframes*n) );
271 // save intrinsic parameters
272 CvFileStorage* fstorage = cvOpenFileStorage("intrinsics.yml", NULL, CV_STORAGE_WRITE);
273 cvWrite(fstorage, "M1", &_M1);
274 cvWrite(fstorage, "D1", &_D1);
275 cvWrite(fstorage, "M2", &_M2);
276 cvWrite(fstorage, "D2", &_D2);
277 cvReleaseFileStorage(&fstorage);
279 //COMPUTE AND DISPLAY RECTIFICATION
280 if( showUndistorted )
282 CvMat* mx1 = cvCreateMat( imageSize.height,
283 imageSize.width, CV_32F );
284 CvMat* my1 = cvCreateMat( imageSize.height,
285 imageSize.width, CV_32F );
286 CvMat* mx2 = cvCreateMat( imageSize.height,
287 imageSize.width, CV_32F );
288 CvMat* my2 = cvCreateMat( imageSize.height,
289 imageSize.width, CV_32F );
290 CvMat* img1r = cvCreateMat( imageSize.height,
291 imageSize.width, CV_8U );
292 CvMat* img2r = cvCreateMat( imageSize.height,
293 imageSize.width, CV_8U );
294 CvMat* disp = cvCreateMat( imageSize.height,
295 imageSize.width, CV_16S );
296 double R1[3][3], R2[3][3], P1[3][4], P2[3][4];
297 CvMat _R1 = cvMat(3, 3, CV_64F, R1);
298 CvMat _R2 = cvMat(3, 3, CV_64F, R2);
299 // IF BY CALIBRATED (BOUGUET'S METHOD)
300 if( useUncalibrated == 0 )
302 CvMat _P1 = cvMat(3, 4, CV_64F, P1);
303 CvMat _P2 = cvMat(3, 4, CV_64F, P2);
305 cvStereoRectify( &_M1, &_M2, &_D1, &_D2, imageSize,
307 &_R1, &_R2, &_P1, &_P2, &matQ,
308 CV_CALIB_ZERO_DISPARITY,
309 1, imageSize, &roi1, &roi2);
311 CvFileStorage* file = cvOpenFileStorage("extrinsics.yml", NULL, CV_STORAGE_WRITE);
312 cvWrite(file, "R", &matR);
313 cvWrite(file, "T", &matT);
314 cvWrite(file, "R1", &_R1);
315 cvWrite(file, "R2", &_R2);
316 cvWrite(file, "P1", &_P1);
317 cvWrite(file, "P2", &_P2);
318 cvWrite(file, "Q", &matQ);
319 cvReleaseFileStorage(&file);
321 isVerticalStereo = fabs(P2[1][3]) > fabs(P2[0][3]);
322 if(!isVerticalStereo)
323 roi2.x += imageSize.width;
325 roi2.y += imageSize.height;
326 //Precompute maps for cvRemap()
327 cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_P1,mx1,my1);
328 cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_P2,mx2,my2);
330 //OR ELSE HARTLEY'S METHOD
331 else if( useUncalibrated == 1 || useUncalibrated == 2 )
332 // use intrinsic parameters of each camera, but
333 // compute the rectification transformation directly
334 // from the fundamental matrix
336 double H1[3][3], H2[3][3], iM[3][3];
337 CvMat _H1 = cvMat(3, 3, CV_64F, H1);
338 CvMat _H2 = cvMat(3, 3, CV_64F, H2);
339 CvMat _iM = cvMat(3, 3, CV_64F, iM);
340 //Just to show you could have independently used F
341 if( useUncalibrated == 2 )
342 cvFindFundamentalMat( &_imagePoints1,
343 &_imagePoints2, &matF);
344 cvStereoRectifyUncalibrated( &_imagePoints1,
345 &_imagePoints2, &matF,
348 cvInvert(&_M1, &_iM);
349 cvMatMul(&_H1, &_M1, &_R1);
350 cvMatMul(&_iM, &_R1, &_R1);
351 cvInvert(&_M2, &_iM);
352 cvMatMul(&_H2, &_M2, &_R2);
353 cvMatMul(&_iM, &_R2, &_R2);
354 //Precompute map for cvRemap()
355 cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1);
357 cvInitUndistortRectifyMap(&_M2,&_D1,&_R2,&_M2,mx2,my2);
363 cvReleaseMat( &mx1 );
364 cvReleaseMat( &my1 );
365 cvReleaseMat( &mx2 );
366 cvReleaseMat( &my2 );
367 cvReleaseMat( &img1r );
368 cvReleaseMat( &img2r );
369 cvReleaseMat( &disp );
373 int main(int argc, char** argv)
375 StereoCalib(argc > 1 ? argv[1] : "stereo_calib.txt", 0);