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 warranty, support or any guarantee 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 OPENCV WEBSITES: 18 Homepage: http://opencv.org 19 Online docs: http://docs.opencv.org 20 Q&A forum: http://answers.opencv.org 21 Issue tracker: http://code.opencv.org 22 GitHub: https://github.com/Itseez/opencv/ 23 ************************************************** */ 24 25 #include "opencv2/calib3d/calib3d.hpp" 26 #include "opencv2/imgcodecs.hpp" 27 #include "opencv2/highgui/highgui.hpp" 28 #include "opencv2/imgproc/imgproc.hpp" 29 30 #include <vector> 31 #include <string> 32 #include <algorithm> 33 #include <iostream> 34 #include <iterator> 35 #include <stdio.h> 36 #include <stdlib.h> 37 #include <ctype.h> 38 39 using namespace cv; 40 using namespace std; 41 42 static int print_help() 43 { 44 cout << 45 " Given a list of chessboard images, the number of corners (nx, ny)\n" 46 " on the chessboards, and a flag: useCalibrated for \n" 47 " calibrated (0) or\n" 48 " uncalibrated \n" 49 " (1: use cvStereoCalibrate(), 2: compute fundamental\n" 50 " matrix separately) stereo. \n" 51 " Calibrate the cameras and display the\n" 52 " rectified results along with the computed disparity images. \n" << endl; 53 cout << "Usage:\n ./stereo_calib -w board_width -h board_height [-nr /*dot not view results*/] <image list XML/YML file>\n" << endl; 54 return 0; 55 } 56 57 58 static void 59 StereoCalib(const vector<string>& imagelist, Size boardSize, bool useCalibrated=true, bool showRectified=true) 60 { 61 if( imagelist.size() % 2 != 0 ) 62 { 63 cout << "Error: the image list contains odd (non-even) number of elements\n"; 64 return; 65 } 66 67 bool displayCorners = false;//true; 68 const int maxScale = 2; 69 const float squareSize = 1.f; // Set this to your actual square size 70 // ARRAY AND VECTOR STORAGE: 71 72 vector<vector<Point2f> > imagePoints[2]; 73 vector<vector<Point3f> > objectPoints; 74 Size imageSize; 75 76 int i, j, k, nimages = (int)imagelist.size()/2; 77 78 imagePoints[0].resize(nimages); 79 imagePoints[1].resize(nimages); 80 vector<string> goodImageList; 81 82 for( i = j = 0; i < nimages; i++ ) 83 { 84 for( k = 0; k < 2; k++ ) 85 { 86 const string& filename = imagelist[i*2+k]; 87 Mat img = imread(filename, 0); 88 if(img.empty()) 89 break; 90 if( imageSize == Size() ) 91 imageSize = img.size(); 92 else if( img.size() != imageSize ) 93 { 94 cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n"; 95 break; 96 } 97 bool found = false; 98 vector<Point2f>& corners = imagePoints[k][j]; 99 for( int scale = 1; scale <= maxScale; scale++ ) 100 { 101 Mat timg; 102 if( scale == 1 ) 103 timg = img; 104 else 105 resize(img, timg, Size(), scale, scale); 106 found = findChessboardCorners(timg, boardSize, corners, 107 CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE); 108 if( found ) 109 { 110 if( scale > 1 ) 111 { 112 Mat cornersMat(corners); 113 cornersMat *= 1./scale; 114 } 115 break; 116 } 117 } 118 if( displayCorners ) 119 { 120 cout << filename << endl; 121 Mat cimg, cimg1; 122 cvtColor(img, cimg, COLOR_GRAY2BGR); 123 drawChessboardCorners(cimg, boardSize, corners, found); 124 double sf = 640./MAX(img.rows, img.cols); 125 resize(cimg, cimg1, Size(), sf, sf); 126 imshow("corners", cimg1); 127 char c = (char)waitKey(500); 128 if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit 129 exit(-1); 130 } 131 else 132 putchar('.'); 133 if( !found ) 134 break; 135 cornerSubPix(img, corners, Size(11,11), Size(-1,-1), 136 TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 137 30, 0.01)); 138 } 139 if( k == 2 ) 140 { 141 goodImageList.push_back(imagelist[i*2]); 142 goodImageList.push_back(imagelist[i*2+1]); 143 j++; 144 } 145 } 146 cout << j << " pairs have been successfully detected.\n"; 147 nimages = j; 148 if( nimages < 2 ) 149 { 150 cout << "Error: too little pairs to run the calibration\n"; 151 return; 152 } 153 154 imagePoints[0].resize(nimages); 155 imagePoints[1].resize(nimages); 156 objectPoints.resize(nimages); 157 158 for( i = 0; i < nimages; i++ ) 159 { 160 for( j = 0; j < boardSize.height; j++ ) 161 for( k = 0; k < boardSize.width; k++ ) 162 objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0)); 163 } 164 165 cout << "Running stereo calibration ...\n"; 166 167 Mat cameraMatrix[2], distCoeffs[2]; 168 cameraMatrix[0] = Mat::eye(3, 3, CV_64F); 169 cameraMatrix[1] = Mat::eye(3, 3, CV_64F); 170 Mat R, T, E, F; 171 172 double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1], 173 cameraMatrix[0], distCoeffs[0], 174 cameraMatrix[1], distCoeffs[1], 175 imageSize, R, T, E, F, 176 CALIB_FIX_ASPECT_RATIO + 177 CALIB_ZERO_TANGENT_DIST + 178 CALIB_SAME_FOCAL_LENGTH + 179 CALIB_RATIONAL_MODEL + 180 CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5, 181 TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) ); 182 cout << "done with RMS error=" << rms << endl; 183 184 // CALIBRATION QUALITY CHECK 185 // because the output fundamental matrix implicitly 186 // includes all the output information, 187 // we can check the quality of calibration using the 188 // epipolar geometry constraint: m2^t*F*m1=0 189 double err = 0; 190 int npoints = 0; 191 vector<Vec3f> lines[2]; 192 for( i = 0; i < nimages; i++ ) 193 { 194 int npt = (int)imagePoints[0][i].size(); 195 Mat imgpt[2]; 196 for( k = 0; k < 2; k++ ) 197 { 198 imgpt[k] = Mat(imagePoints[k][i]); 199 undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]); 200 computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]); 201 } 202 for( j = 0; j < npt; j++ ) 203 { 204 double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] + 205 imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) + 206 fabs(imagePoints[1][i][j].x*lines[0][j][0] + 207 imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]); 208 err += errij; 209 } 210 npoints += npt; 211 } 212 cout << "average reprojection err = " << err/npoints << endl; 213 214 // save intrinsic parameters 215 FileStorage fs("../data/intrinsics.yml", FileStorage::WRITE); 216 if( fs.isOpened() ) 217 { 218 fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] << 219 "M2" << cameraMatrix[1] << "D2" << distCoeffs[1]; 220 fs.release(); 221 } 222 else 223 cout << "Error: can not save the intrinsic parameters\n"; 224 225 Mat R1, R2, P1, P2, Q; 226 Rect validRoi[2]; 227 228 stereoRectify(cameraMatrix[0], distCoeffs[0], 229 cameraMatrix[1], distCoeffs[1], 230 imageSize, R, T, R1, R2, P1, P2, Q, 231 CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]); 232 233 fs.open("extrinsics.yml", FileStorage::WRITE); 234 if( fs.isOpened() ) 235 { 236 fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q; 237 fs.release(); 238 } 239 else 240 cout << "Error: can not save the extrinsic parameters\n"; 241 242 // OpenCV can handle left-right 243 // or up-down camera arrangements 244 bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3)); 245 246 // COMPUTE AND DISPLAY RECTIFICATION 247 if( !showRectified ) 248 return; 249 250 Mat rmap[2][2]; 251 // IF BY CALIBRATED (BOUGUET'S METHOD) 252 if( useCalibrated ) 253 { 254 // we already computed everything 255 } 256 // OR ELSE HARTLEY'S METHOD 257 else 258 // use intrinsic parameters of each camera, but 259 // compute the rectification transformation directly 260 // from the fundamental matrix 261 { 262 vector<Point2f> allimgpt[2]; 263 for( k = 0; k < 2; k++ ) 264 { 265 for( i = 0; i < nimages; i++ ) 266 std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k])); 267 } 268 F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0); 269 Mat H1, H2; 270 stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3); 271 272 R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0]; 273 R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1]; 274 P1 = cameraMatrix[0]; 275 P2 = cameraMatrix[1]; 276 } 277 278 //Precompute maps for cv::remap() 279 initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]); 280 initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]); 281 282 Mat canvas; 283 double sf; 284 int w, h; 285 if( !isVerticalStereo ) 286 { 287 sf = 600./MAX(imageSize.width, imageSize.height); 288 w = cvRound(imageSize.width*sf); 289 h = cvRound(imageSize.height*sf); 290 canvas.create(h, w*2, CV_8UC3); 291 } 292 else 293 { 294 sf = 300./MAX(imageSize.width, imageSize.height); 295 w = cvRound(imageSize.width*sf); 296 h = cvRound(imageSize.height*sf); 297 canvas.create(h*2, w, CV_8UC3); 298 } 299 300 for( i = 0; i < nimages; i++ ) 301 { 302 for( k = 0; k < 2; k++ ) 303 { 304 Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg; 305 remap(img, rimg, rmap[k][0], rmap[k][1], INTER_LINEAR); 306 cvtColor(rimg, cimg, COLOR_GRAY2BGR); 307 Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h)); 308 resize(cimg, canvasPart, canvasPart.size(), 0, 0, INTER_AREA); 309 if( useCalibrated ) 310 { 311 Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf), 312 cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf)); 313 rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8); 314 } 315 } 316 317 if( !isVerticalStereo ) 318 for( j = 0; j < canvas.rows; j += 16 ) 319 line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8); 320 else 321 for( j = 0; j < canvas.cols; j += 16 ) 322 line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8); 323 imshow("rectified", canvas); 324 char c = (char)waitKey(); 325 if( c == 27 || c == 'q' || c == 'Q' ) 326 break; 327 } 328 } 329 330 331 static bool readStringList( const string& filename, vector<string>& l ) 332 { 333 l.resize(0); 334 FileStorage fs(filename, FileStorage::READ); 335 if( !fs.isOpened() ) 336 return false; 337 FileNode n = fs.getFirstTopLevelNode(); 338 if( n.type() != FileNode::SEQ ) 339 return false; 340 FileNodeIterator it = n.begin(), it_end = n.end(); 341 for( ; it != it_end; ++it ) 342 l.push_back((string)*it); 343 return true; 344 } 345 346 int main(int argc, char** argv) 347 { 348 Size boardSize; 349 string imagelistfn; 350 bool showRectified = true; 351 352 for( int i = 1; i < argc; i++ ) 353 { 354 if( string(argv[i]) == "-w" ) 355 { 356 if( sscanf(argv[++i], "%d", &boardSize.width) != 1 || boardSize.width <= 0 ) 357 { 358 cout << "invalid board width" << endl; 359 return print_help(); 360 } 361 } 362 else if( string(argv[i]) == "-h" ) 363 { 364 if( sscanf(argv[++i], "%d", &boardSize.height) != 1 || boardSize.height <= 0 ) 365 { 366 cout << "invalid board height" << endl; 367 return print_help(); 368 } 369 } 370 else if( string(argv[i]) == "-nr" ) 371 showRectified = false; 372 else if( string(argv[i]) == "--help" ) 373 return print_help(); 374 else if( argv[i][0] == '-' ) 375 { 376 cout << "invalid option " << argv[i] << endl; 377 return 0; 378 } 379 else 380 imagelistfn = argv[i]; 381 } 382 383 if( imagelistfn == "" ) 384 { 385 imagelistfn = "../data/stereo_calib.xml"; 386 boardSize = Size(9, 6); 387 } 388 else if( boardSize.width <= 0 || boardSize.height <= 0 ) 389 { 390 cout << "if you specified XML file with chessboards, you should also specify the board width and height (-w and -h options)" << endl; 391 return 0; 392 } 393 394 vector<string> imagelist; 395 bool ok = readStringList(imagelistfn, imagelist); 396 if(!ok || imagelist.empty()) 397 { 398 cout << "can not open " << imagelistfn << " or the string list is empty" << endl; 399 return print_help(); 400 } 401 402 StereoCalib(imagelist, boardSize, true, showRectified); 403 return 0; 404 } 405
