samples/cpp/calibration.cpp

   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.data)
 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.data)
 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 }