1 #include "opencv2/core.hpp" 2 #include <opencv2/core/utility.hpp> 3 #include "opencv2/imgproc.hpp" 4 #include "opencv2/calib3d.hpp" 5 #include "opencv2/imgcodecs.hpp" 6 #include "opencv2/videoio.hpp" 7 #include "opencv2/highgui.hpp" 8 9 #include <cctype> 10 #include <stdio.h> 11 #include <string.h> 12 #include <time.h> 13 14 using namespace cv; 15 using namespace std; 16 17 const char * usage = 18 " \nexample command line for calibration from a live feed.\n" 19 " calibration -w 4 -h 5 -s 0.025 -o camera.yml -op -oe\n" 20 " \n" 21 " example command line for calibration from a list of stored images:\n" 22 " imagelist_creator image_list.xml *.png\n" 23 " calibration -w 4 -h 5 -s 0.025 -o camera.yml -op -oe image_list.xml\n" 24 " where image_list.xml is the standard OpenCV XML/YAML\n" 25 " use imagelist_creator to create the xml or yaml list\n" 26 " file consisting of the list of strings, e.g.:\n" 27 " \n" 28 "<?xml version=\"1.0\"?>\n" 29 "<opencv_storage>\n" 30 "<images>\n" 31 "view000.png\n" 32 "view001.png\n" 33 "<!-- view002.png -->\n" 34 "view003.png\n" 35 "view010.png\n" 36 "one_extra_view.jpg\n" 37 "</images>\n" 38 "</opencv_storage>\n"; 39 40 41 42 43 const char* liveCaptureHelp = 44 "When the live video from camera is used as input, the following hot-keys may be used:\n" 45 " <ESC>, 'q' - quit the program\n" 46 " 'g' - start capturing images\n" 47 " 'u' - switch undistortion on/off\n"; 48 49 static void help() 50 { 51 printf( "This is a camera calibration sample.\n" 52 "Usage: calibration\n" 53 " -w <board_width> # the number of inner corners per one of board dimension\n" 54 " -h <board_height> # the number of inner corners per another board dimension\n" 55 " [-pt <pattern>] # the type of pattern: chessboard or circles' grid\n" 56 " [-n <number_of_frames>] # the number of frames to use for calibration\n" 57 " # (if not specified, it will be set to the number\n" 58 " # of board views actually available)\n" 59 " [-d <delay>] # a minimum delay in ms between subsequent attempts to capture a next view\n" 60 " # (used only for video capturing)\n" 61 " [-s <squareSize>] # square size in some user-defined units (1 by default)\n" 62 " [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n" 63 " [-op] # write detected feature points\n" 64 " [-oe] # write extrinsic parameters\n" 65 " [-zt] # assume zero tangential distortion\n" 66 " [-a <aspectRatio>] # fix aspect ratio (fx/fy)\n" 67 " [-p] # fix the principal point at the center\n" 68 " [-v] # flip the captured images around the horizontal axis\n" 69 " [-V] # use a video file, and not an image list, uses\n" 70 " # [input_data] string for the video file name\n" 71 " [-su] # show undistorted images after calibration\n" 72 " [input_data] # input data, one of the following:\n" 73 " # - text file with a list of the images of the board\n" 74 " # the text file can be generated with imagelist_creator\n" 75 " # - name of video file with a video of the board\n" 76 " # if input_data not specified, a live view from the camera is used\n" 77 "\n" ); 78 printf("\n%s",usage); 79 printf( "\n%s", liveCaptureHelp ); 80 } 81 82 enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; 83 enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; 84 85 static double computeReprojectionErrors( 86 const vector<vector<Point3f> >& objectPoints, 87 const vector<vector<Point2f> >& imagePoints, 88 const vector<Mat>& rvecs, const vector<Mat>& tvecs, 89 const Mat& cameraMatrix, const Mat& distCoeffs, 90 vector<float>& perViewErrors ) 91 { 92 vector<Point2f> imagePoints2; 93 int i, totalPoints = 0; 94 double totalErr = 0, err; 95 perViewErrors.resize(objectPoints.size()); 96 97 for( i = 0; i < (int)objectPoints.size(); i++ ) 98 { 99 projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i], 100 cameraMatrix, distCoeffs, imagePoints2); 101 err = norm(Mat(imagePoints[i]), Mat(imagePoints2), NORM_L2); 102 int n = (int)objectPoints[i].size(); 103 perViewErrors[i] = (float)std::sqrt(err*err/n); 104 totalErr += err*err; 105 totalPoints += n; 106 } 107 108 return std::sqrt(totalErr/totalPoints); 109 } 110 111 static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType = CHESSBOARD) 112 { 113 corners.resize(0); 114 115 switch(patternType) 116 { 117 case CHESSBOARD: 118 case CIRCLES_GRID: 119 for( int i = 0; i < boardSize.height; i++ ) 120 for( int j = 0; j < boardSize.width; j++ ) 121 corners.push_back(Point3f(float(j*squareSize), 122 float(i*squareSize), 0)); 123 break; 124 125 case ASYMMETRIC_CIRCLES_GRID: 126 for( int i = 0; i < boardSize.height; i++ ) 127 for( int j = 0; j < boardSize.width; j++ ) 128 corners.push_back(Point3f(float((2*j + i % 2)*squareSize), 129 float(i*squareSize), 0)); 130 break; 131 132 default: 133 CV_Error(Error::StsBadArg, "Unknown pattern type\n"); 134 } 135 } 136 137 static bool runCalibration( vector<vector<Point2f> > imagePoints, 138 Size imageSize, Size boardSize, Pattern patternType, 139 float squareSize, float aspectRatio, 140 int flags, Mat& cameraMatrix, Mat& distCoeffs, 141 vector<Mat>& rvecs, vector<Mat>& tvecs, 142 vector<float>& reprojErrs, 143 double& totalAvgErr) 144 { 145 cameraMatrix = Mat::eye(3, 3, CV_64F); 146 if( flags & CALIB_FIX_ASPECT_RATIO ) 147 cameraMatrix.at<double>(0,0) = aspectRatio; 148 149 distCoeffs = Mat::zeros(8, 1, CV_64F); 150 151 vector<vector<Point3f> > objectPoints(1); 152 calcChessboardCorners(boardSize, squareSize, objectPoints[0], patternType); 153 154 objectPoints.resize(imagePoints.size(),objectPoints[0]); 155 156 double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, 157 distCoeffs, rvecs, tvecs, flags|CALIB_FIX_K4|CALIB_FIX_K5); 158 ///*|CALIB_FIX_K3*/|CALIB_FIX_K4|CALIB_FIX_K5); 159 printf("RMS error reported by calibrateCamera: %g\n", rms); 160 161 bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs); 162 163 totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, 164 rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs); 165 166 return ok; 167 } 168 169 170 static void saveCameraParams( const string& filename, 171 Size imageSize, Size boardSize, 172 float squareSize, float aspectRatio, int flags, 173 const Mat& cameraMatrix, const Mat& distCoeffs, 174 const vector<Mat>& rvecs, const vector<Mat>& tvecs, 175 const vector<float>& reprojErrs, 176 const vector<vector<Point2f> >& imagePoints, 177 double totalAvgErr ) 178 { 179 FileStorage fs( filename, FileStorage::WRITE ); 180 181 time_t tt; 182 time( &tt ); 183 struct tm *t2 = localtime( &tt ); 184 char buf[1024]; 185 strftime( buf, sizeof(buf)-1, "%c", t2 ); 186 187 fs << "calibration_time" << buf; 188 189 if( !rvecs.empty() || !reprojErrs.empty() ) 190 fs << "nframes" << (int)std::max(rvecs.size(), reprojErrs.size()); 191 fs << "image_width" << imageSize.width; 192 fs << "image_height" << imageSize.height; 193 fs << "board_width" << boardSize.width; 194 fs << "board_height" << boardSize.height; 195 fs << "square_size" << squareSize; 196 197 if( flags & CALIB_FIX_ASPECT_RATIO ) 198 fs << "aspectRatio" << aspectRatio; 199 200 if( flags != 0 ) 201 { 202 sprintf( buf, "flags: %s%s%s%s", 203 flags & CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "", 204 flags & CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "", 205 flags & CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "", 206 flags & CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" ); 207 //cvWriteComment( *fs, buf, 0 ); 208 } 209 210 fs << "flags" << flags; 211 212 fs << "camera_matrix" << cameraMatrix; 213 fs << "distortion_coefficients" << distCoeffs; 214 215 fs << "avg_reprojection_error" << totalAvgErr; 216 if( !reprojErrs.empty() ) 217 fs << "per_view_reprojection_errors" << Mat(reprojErrs); 218 219 if( !rvecs.empty() && !tvecs.empty() ) 220 { 221 CV_Assert(rvecs[0].type() == tvecs[0].type()); 222 Mat bigmat((int)rvecs.size(), 6, rvecs[0].type()); 223 for( int i = 0; i < (int)rvecs.size(); i++ ) 224 { 225 Mat r = bigmat(Range(i, i+1), Range(0,3)); 226 Mat t = bigmat(Range(i, i+1), Range(3,6)); 227 228 CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1); 229 CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1); 230 //*.t() is MatExpr (not Mat) so we can use assignment operator 231 r = rvecs[i].t(); 232 t = tvecs[i].t(); 233 } 234 //cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 ); 235 fs << "extrinsic_parameters" << bigmat; 236 } 237 238 if( !imagePoints.empty() ) 239 { 240 Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2); 241 for( int i = 0; i < (int)imagePoints.size(); i++ ) 242 { 243 Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols); 244 Mat imgpti(imagePoints[i]); 245 imgpti.copyTo(r); 246 } 247 fs << "image_points" << imagePtMat; 248 } 249 } 250 251 static bool readStringList( const string& filename, vector<string>& l ) 252 { 253 l.resize(0); 254 FileStorage fs(filename, FileStorage::READ); 255 if( !fs.isOpened() ) 256 return false; 257 FileNode n = fs.getFirstTopLevelNode(); 258 if( n.type() != FileNode::SEQ ) 259 return false; 260 FileNodeIterator it = n.begin(), it_end = n.end(); 261 for( ; it != it_end; ++it ) 262 l.push_back((string)*it); 263 return true; 264 } 265 266 267 static bool runAndSave(const string& outputFilename, 268 const vector<vector<Point2f> >& imagePoints, 269 Size imageSize, Size boardSize, Pattern patternType, float squareSize, 270 float aspectRatio, int flags, Mat& cameraMatrix, 271 Mat& distCoeffs, bool writeExtrinsics, bool writePoints ) 272 { 273 vector<Mat> rvecs, tvecs; 274 vector<float> reprojErrs; 275 double totalAvgErr = 0; 276 277 bool ok = runCalibration(imagePoints, imageSize, boardSize, patternType, squareSize, 278 aspectRatio, flags, cameraMatrix, distCoeffs, 279 rvecs, tvecs, reprojErrs, totalAvgErr); 280 printf("%s. avg reprojection error = %.2f\n", 281 ok ? "Calibration succeeded" : "Calibration failed", 282 totalAvgErr); 283 284 if( ok ) 285 saveCameraParams( outputFilename, imageSize, 286 boardSize, squareSize, aspectRatio, 287 flags, cameraMatrix, distCoeffs, 288 writeExtrinsics ? rvecs : vector<Mat>(), 289 writeExtrinsics ? tvecs : vector<Mat>(), 290 writeExtrinsics ? reprojErrs : vector<float>(), 291 writePoints ? imagePoints : vector<vector<Point2f> >(), 292 totalAvgErr ); 293 return ok; 294 } 295 296 297 int main( int argc, char** argv ) 298 { 299 Size boardSize, imageSize; 300 float squareSize = 1.f, aspectRatio = 1.f; 301 Mat cameraMatrix, distCoeffs; 302 const char* outputFilename = "out_camera_data.yml"; 303 const char* inputFilename = 0; 304 305 int i, nframes = 10; 306 bool writeExtrinsics = false, writePoints = false; 307 bool undistortImage = false; 308 int flags = 0; 309 VideoCapture capture; 310 bool flipVertical = false; 311 bool showUndistorted = false; 312 bool videofile = false; 313 int delay = 1000; 314 clock_t prevTimestamp = 0; 315 int mode = DETECTION; 316 int cameraId = 0; 317 vector<vector<Point2f> > imagePoints; 318 vector<string> imageList; 319 Pattern pattern = CHESSBOARD; 320 321 if( argc < 2 ) 322 { 323 help(); 324 return 0; 325 } 326 327 for( i = 1; i < argc; i++ ) 328 { 329 const char* s = argv[i]; 330 if( strcmp( s, "-w" ) == 0 ) 331 { 332 if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 ) 333 return fprintf( stderr, "Invalid board width\n" ), -1; 334 } 335 else if( strcmp( s, "-h" ) == 0 ) 336 { 337 if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 ) 338 return fprintf( stderr, "Invalid board height\n" ), -1; 339 } 340 else if( strcmp( s, "-pt" ) == 0 ) 341 { 342 i++; 343 if( !strcmp( argv[i], "circles" ) ) 344 pattern = CIRCLES_GRID; 345 else if( !strcmp( argv[i], "acircles" ) ) 346 pattern = ASYMMETRIC_CIRCLES_GRID; 347 else if( !strcmp( argv[i], "chessboard" ) ) 348 pattern = CHESSBOARD; 349 else 350 return fprintf( stderr, "Invalid pattern type: must be chessboard or circles\n" ), -1; 351 } 352 else if( strcmp( s, "-s" ) == 0 ) 353 { 354 if( sscanf( argv[++i], "%f", &squareSize ) != 1 || squareSize <= 0 ) 355 return fprintf( stderr, "Invalid board square width\n" ), -1; 356 } 357 else if( strcmp( s, "-n" ) == 0 ) 358 { 359 if( sscanf( argv[++i], "%u", &nframes ) != 1 || nframes <= 3 ) 360 return printf("Invalid number of images\n" ), -1; 361 } 362 else if( strcmp( s, "-a" ) == 0 ) 363 { 364 if( sscanf( argv[++i], "%f", &aspectRatio ) != 1 || aspectRatio <= 0 ) 365 return printf("Invalid aspect ratio\n" ), -1; 366 flags |= CALIB_FIX_ASPECT_RATIO; 367 } 368 else if( strcmp( s, "-d" ) == 0 ) 369 { 370 if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 ) 371 return printf("Invalid delay\n" ), -1; 372 } 373 else if( strcmp( s, "-op" ) == 0 ) 374 { 375 writePoints = true; 376 } 377 else if( strcmp( s, "-oe" ) == 0 ) 378 { 379 writeExtrinsics = true; 380 } 381 else if( strcmp( s, "-zt" ) == 0 ) 382 { 383 flags |= CALIB_ZERO_TANGENT_DIST; 384 } 385 else if( strcmp( s, "-p" ) == 0 ) 386 { 387 flags |= CALIB_FIX_PRINCIPAL_POINT; 388 } 389 else if( strcmp( s, "-v" ) == 0 ) 390 { 391 flipVertical = true; 392 } 393 else if( strcmp( s, "-V" ) == 0 ) 394 { 395 videofile = true; 396 } 397 else if( strcmp( s, "-o" ) == 0 ) 398 { 399 outputFilename = argv[++i]; 400 } 401 else if( strcmp( s, "-su" ) == 0 ) 402 { 403 showUndistorted = true; 404 } 405 else if( s[0] != '-' ) 406 { 407 if( isdigit(s[0]) ) 408 sscanf(s, "%d", &cameraId); 409 else 410 inputFilename = s; 411 } 412 else 413 return fprintf( stderr, "Unknown option %s", s ), -1; 414 } 415 416 if( inputFilename ) 417 { 418 if( !videofile && readStringList(inputFilename, imageList) ) 419 mode = CAPTURING; 420 else 421 capture.open(inputFilename); 422 } 423 else 424 capture.open(cameraId); 425 426 if( !capture.isOpened() && imageList.empty() ) 427 return fprintf( stderr, "Could not initialize video (%d) capture\n",cameraId ), -2; 428 429 if( !imageList.empty() ) 430 nframes = (int)imageList.size(); 431 432 if( capture.isOpened() ) 433 printf( "%s", liveCaptureHelp ); 434 435 namedWindow( "Image View", 1 ); 436 437 for(i = 0;;i++) 438 { 439 Mat view, viewGray; 440 bool blink = false; 441 442 if( capture.isOpened() ) 443 { 444 Mat view0; 445 capture >> view0; 446 view0.copyTo(view); 447 } 448 else if( i < (int)imageList.size() ) 449 view = imread(imageList[i], 1); 450 451 if(view.empty()) 452 { 453 if( imagePoints.size() > 0 ) 454 runAndSave(outputFilename, imagePoints, imageSize, 455 boardSize, pattern, squareSize, aspectRatio, 456 flags, cameraMatrix, distCoeffs, 457 writeExtrinsics, writePoints); 458 break; 459 } 460 461 imageSize = view.size(); 462 463 if( flipVertical ) 464 flip( view, view, 0 ); 465 466 vector<Point2f> pointbuf; 467 cvtColor(view, viewGray, COLOR_BGR2GRAY); 468 469 bool found; 470 switch( pattern ) 471 { 472 case CHESSBOARD: 473 found = findChessboardCorners( view, boardSize, pointbuf, 474 CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE); 475 break; 476 case CIRCLES_GRID: 477 found = findCirclesGrid( view, boardSize, pointbuf ); 478 break; 479 case ASYMMETRIC_CIRCLES_GRID: 480 found = findCirclesGrid( view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID ); 481 break; 482 default: 483 return fprintf( stderr, "Unknown pattern type\n" ), -1; 484 } 485 486 // improve the found corners' coordinate accuracy 487 if( pattern == CHESSBOARD && found) cornerSubPix( viewGray, pointbuf, Size(11,11), 488 Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 )); 489 490 if( mode == CAPTURING && found && 491 (!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) ) 492 { 493 imagePoints.push_back(pointbuf); 494 prevTimestamp = clock(); 495 blink = capture.isOpened(); 496 } 497 498 if(found) 499 drawChessboardCorners( view, boardSize, Mat(pointbuf), found ); 500 501 string msg = mode == CAPTURING ? "100/100" : 502 mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; 503 int baseLine = 0; 504 Size textSize = getTextSize(msg, 1, 1, 1, &baseLine); 505 Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10); 506 507 if( mode == CAPTURING ) 508 { 509 if(undistortImage) 510 msg = format( "%d/%d Undist", (int)imagePoints.size(), nframes ); 511 else 512 msg = format( "%d/%d", (int)imagePoints.size(), nframes ); 513 } 514 515 putText( view, msg, textOrigin, 1, 1, 516 mode != CALIBRATED ? Scalar(0,0,255) : Scalar(0,255,0)); 517 518 if( blink ) 519 bitwise_not(view, view); 520 521 if( mode == CALIBRATED && undistortImage ) 522 { 523 Mat temp = view.clone(); 524 undistort(temp, view, cameraMatrix, distCoeffs); 525 } 526 527 imshow("Image View", view); 528 int key = 0xff & waitKey(capture.isOpened() ? 50 : 500); 529 530 if( (key & 255) == 27 ) 531 break; 532 533 if( key == 'u' && mode == CALIBRATED ) 534 undistortImage = !undistortImage; 535 536 if( capture.isOpened() && key == 'g' ) 537 { 538 mode = CAPTURING; 539 imagePoints.clear(); 540 } 541 542 if( mode == CAPTURING && imagePoints.size() >= (unsigned)nframes ) 543 { 544 if( runAndSave(outputFilename, imagePoints, imageSize, 545 boardSize, pattern, squareSize, aspectRatio, 546 flags, cameraMatrix, distCoeffs, 547 writeExtrinsics, writePoints)) 548 mode = CALIBRATED; 549 else 550 mode = DETECTION; 551 if( !capture.isOpened() ) 552 break; 553 } 554 } 555 556 if( !capture.isOpened() && showUndistorted ) 557 { 558 Mat view, rview, map1, map2; 559 initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), 560 getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), 561 imageSize, CV_16SC2, map1, map2); 562 563 for( i = 0; i < (int)imageList.size(); i++ ) 564 { 565 view = imread(imageList[i], 1); 566 if(view.empty()) 567 continue; 568 //undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix ); 569 remap(view, rview, map1, map2, INTER_LINEAR); 570 imshow("Image View", rview); 571 int c = 0xff & waitKey(); 572 if( (c & 255) == 27 || c == 'q' || c == 'Q' ) 573 break; 574 } 575 } 576 577 return 0; 578 } 579
