1 #include <iostream> 2 #include <sstream> 3 #include <time.h> 4 #include <stdio.h> 5 6 #include <opencv2/core.hpp> 7 #include <opencv2/core/utility.hpp> 8 #include <opencv2/imgproc.hpp> 9 #include <opencv2/calib3d.hpp> 10 #include <opencv2/imgcodecs.hpp> 11 #include <opencv2/videoio.hpp> 12 #include <opencv2/highgui.hpp> 13 14 #ifndef _CRT_SECURE_NO_WARNINGS 15 # define _CRT_SECURE_NO_WARNINGS 16 #endif 17 18 using namespace cv; 19 using namespace std; 20 21 static void help() 22 { 23 cout << "This is a camera calibration sample." << endl 24 << "Usage: calibration configurationFile" << endl 25 << "Near the sample file you'll find the configuration file, which has detailed help of " 26 "how to edit it. It may be any OpenCV supported file format XML/YAML." << endl; 27 } 28 class Settings 29 { 30 public: 31 Settings() : goodInput(false) {} 32 enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; 33 enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST }; 34 35 void write(FileStorage& fs) const //Write serialization for this class 36 { 37 fs << "{" 38 << "BoardSize_Width" << boardSize.width 39 << "BoardSize_Height" << boardSize.height 40 << "Square_Size" << squareSize 41 << "Calibrate_Pattern" << patternToUse 42 << "Calibrate_NrOfFrameToUse" << nrFrames 43 << "Calibrate_FixAspectRatio" << aspectRatio 44 << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist 45 << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint 46 47 << "Write_DetectedFeaturePoints" << writePoints 48 << "Write_extrinsicParameters" << writeExtrinsics 49 << "Write_outputFileName" << outputFileName 50 51 << "Show_UndistortedImage" << showUndistorsed 52 53 << "Input_FlipAroundHorizontalAxis" << flipVertical 54 << "Input_Delay" << delay 55 << "Input" << input 56 << "}"; 57 } 58 void read(const FileNode& node) //Read serialization for this class 59 { 60 node["BoardSize_Width" ] >> boardSize.width; 61 node["BoardSize_Height"] >> boardSize.height; 62 node["Calibrate_Pattern"] >> patternToUse; 63 node["Square_Size"] >> squareSize; 64 node["Calibrate_NrOfFrameToUse"] >> nrFrames; 65 node["Calibrate_FixAspectRatio"] >> aspectRatio; 66 node["Write_DetectedFeaturePoints"] >> writePoints; 67 node["Write_extrinsicParameters"] >> writeExtrinsics; 68 node["Write_outputFileName"] >> outputFileName; 69 node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist; 70 node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint; 71 node["Input_FlipAroundHorizontalAxis"] >> flipVertical; 72 node["Show_UndistortedImage"] >> showUndistorsed; 73 node["Input"] >> input; 74 node["Input_Delay"] >> delay; 75 validate(); 76 } 77 void validate() 78 { 79 goodInput = true; 80 if (boardSize.width <= 0 || boardSize.height <= 0) 81 { 82 cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << endl; 83 goodInput = false; 84 } 85 if (squareSize <= 10e-6) 86 { 87 cerr << "Invalid square size " << squareSize << endl; 88 goodInput = false; 89 } 90 if (nrFrames <= 0) 91 { 92 cerr << "Invalid number of frames " << nrFrames << endl; 93 goodInput = false; 94 } 95 96 if (input.empty()) // Check for valid input 97 inputType = INVALID; 98 else 99 { 100 if (input[0] >= '0' && input[0] <= '9') 101 { 102 stringstream ss(input); 103 ss >> cameraID; 104 inputType = CAMERA; 105 } 106 else 107 { 108 if (readStringList(input, imageList)) 109 { 110 inputType = IMAGE_LIST; 111 nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames : (int)imageList.size(); 112 } 113 else 114 inputType = VIDEO_FILE; 115 } 116 if (inputType == CAMERA) 117 inputCapture.open(cameraID); 118 if (inputType == VIDEO_FILE) 119 inputCapture.open(input); 120 if (inputType != IMAGE_LIST && !inputCapture.isOpened()) 121 inputType = INVALID; 122 } 123 if (inputType == INVALID) 124 { 125 cerr << " Input does not exist: " << input; 126 goodInput = false; 127 } 128 129 flag = 0; 130 if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT; 131 if(calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST; 132 if(aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO; 133 134 135 calibrationPattern = NOT_EXISTING; 136 if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD; 137 if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID; 138 if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID; 139 if (calibrationPattern == NOT_EXISTING) 140 { 141 cerr << " Camera calibration mode does not exist: " << patternToUse << endl; 142 goodInput = false; 143 } 144 atImageList = 0; 145 146 } 147 Mat nextImage() 148 { 149 Mat result; 150 if( inputCapture.isOpened() ) 151 { 152 Mat view0; 153 inputCapture >> view0; 154 view0.copyTo(result); 155 } 156 else if( atImageList < imageList.size() ) 157 result = imread(imageList[atImageList++], IMREAD_COLOR); 158 159 return result; 160 } 161 162 static bool readStringList( const string& filename, vector<string>& l ) 163 { 164 l.clear(); 165 FileStorage fs(filename, FileStorage::READ); 166 if( !fs.isOpened() ) 167 return false; 168 FileNode n = fs.getFirstTopLevelNode(); 169 if( n.type() != FileNode::SEQ ) 170 return false; 171 FileNodeIterator it = n.begin(), it_end = n.end(); 172 for( ; it != it_end; ++it ) 173 l.push_back((string)*it); 174 return true; 175 } 176 public: 177 Size boardSize; // The size of the board -> Number of items by width and height 178 Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric circle pattern 179 float squareSize; // The size of a square in your defined unit (point, millimeter,etc). 180 int nrFrames; // The number of frames to use from the input for calibration 181 float aspectRatio; // The aspect ratio 182 int delay; // In case of a video input 183 bool writePoints; // Write detected feature points 184 bool writeExtrinsics; // Write extrinsic parameters 185 bool calibZeroTangentDist; // Assume zero tangential distortion 186 bool calibFixPrincipalPoint; // Fix the principal point at the center 187 bool flipVertical; // Flip the captured images around the horizontal axis 188 string outputFileName; // The name of the file where to write 189 bool showUndistorsed; // Show undistorted images after calibration 190 string input; // The input -> 191 192 int cameraID; 193 vector<string> imageList; 194 size_t atImageList; 195 VideoCapture inputCapture; 196 InputType inputType; 197 bool goodInput; 198 int flag; 199 200 private: 201 string patternToUse; 202 203 204 }; 205 206 static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings()) 207 { 208 if(node.empty()) 209 x = default_value; 210 else 211 x.read(node); 212 } 213 214 static inline void write(FileStorage& fs, const String&, const Settings& s ) 215 { 216 s.write(fs); 217 } 218 219 enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; 220 221 bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, 222 vector<vector<Point2f> > imagePoints ); 223 224 int main(int argc, char* argv[]) 225 { 226 help(); 227 228 //! [file_read] 229 Settings s; 230 const string inputSettingsFile = argc > 1 ? argv[1] : "default.xml"; 231 FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings 232 if (!fs.isOpened()) 233 { 234 cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl; 235 return -1; 236 } 237 fs["Settings"] >> s; 238 fs.release(); // close Settings file 239 //! [file_read] 240 241 //FileStorage fout("settings.yml", FileStorage::WRITE); // write config as YAML 242 //fout << "Settings" << s; 243 244 if (!s.goodInput) 245 { 246 cout << "Invalid input detected. Application stopping. " << endl; 247 return -1; 248 } 249 250 vector<vector<Point2f> > imagePoints; 251 Mat cameraMatrix, distCoeffs; 252 Size imageSize; 253 int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION; 254 clock_t prevTimestamp = 0; 255 const Scalar RED(0,0,255), GREEN(0,255,0); 256 const char ESC_KEY = 27; 257 258 //! [get_input] 259 for(;;) 260 { 261 Mat view; 262 bool blinkOutput = false; 263 264 view = s.nextImage(); 265 266 //----- If no more image, or got enough, then stop calibration and show result ------------- 267 if( mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames ) 268 { 269 if( runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints)) 270 mode = CALIBRATED; 271 else 272 mode = DETECTION; 273 } 274 if(view.empty()) // If there are no more images stop the loop 275 { 276 // if calibration threshold was not reached yet, calibrate now 277 if( mode != CALIBRATED && !imagePoints.empty() ) 278 runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints); 279 break; 280 } 281 //! [get_input] 282 283 imageSize = view.size(); // Format input image. 284 if( s.flipVertical ) flip( view, view, 0 ); 285 286 //! [find_pattern] 287 vector<Point2f> pointBuf; 288 289 bool found; 290 switch( s.calibrationPattern ) // Find feature points on the input format 291 { 292 case Settings::CHESSBOARD: 293 found = findChessboardCorners( view, s.boardSize, pointBuf, 294 CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE); 295 break; 296 case Settings::CIRCLES_GRID: 297 found = findCirclesGrid( view, s.boardSize, pointBuf ); 298 break; 299 case Settings::ASYMMETRIC_CIRCLES_GRID: 300 found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID ); 301 break; 302 default: 303 found = false; 304 break; 305 } 306 //! [find_pattern] 307 //! [pattern_found] 308 if ( found) // If done with success, 309 { 310 // improve the found corners' coordinate accuracy for chessboard 311 if( s.calibrationPattern == Settings::CHESSBOARD) 312 { 313 Mat viewGray; 314 cvtColor(view, viewGray, COLOR_BGR2GRAY); 315 cornerSubPix( viewGray, pointBuf, Size(11,11), 316 Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 )); 317 } 318 319 if( mode == CAPTURING && // For camera only take new samples after delay time 320 (!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) ) 321 { 322 imagePoints.push_back(pointBuf); 323 prevTimestamp = clock(); 324 blinkOutput = s.inputCapture.isOpened(); 325 } 326 327 // Draw the corners. 328 drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found ); 329 } 330 //! [pattern_found] 331 //----------------------------- Output Text ------------------------------------------------ 332 //! [output_text] 333 string msg = (mode == CAPTURING) ? "100/100" : 334 mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; 335 int baseLine = 0; 336 Size textSize = getTextSize(msg, 1, 1, 1, &baseLine); 337 Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10); 338 339 if( mode == CAPTURING ) 340 { 341 if(s.showUndistorsed) 342 msg = format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames ); 343 else 344 msg = format( "%d/%d", (int)imagePoints.size(), s.nrFrames ); 345 } 346 347 putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED); 348 349 if( blinkOutput ) 350 bitwise_not(view, view); 351 //! [output_text] 352 //------------------------- Video capture output undistorted ------------------------------ 353 //! [output_undistorted] 354 if( mode == CALIBRATED && s.showUndistorsed ) 355 { 356 Mat temp = view.clone(); 357 undistort(temp, view, cameraMatrix, distCoeffs); 358 } 359 //! [output_undistorted] 360 //------------------------------ Show image and check for input commands ------------------- 361 //! [await_input] 362 imshow("Image View", view); 363 char key = (char)waitKey(s.inputCapture.isOpened() ? 50 : s.delay); 364 365 if( key == ESC_KEY ) 366 break; 367 368 if( key == 'u' && mode == CALIBRATED ) 369 s.showUndistorsed = !s.showUndistorsed; 370 371 if( s.inputCapture.isOpened() && key == 'g' ) 372 { 373 mode = CAPTURING; 374 imagePoints.clear(); 375 } 376 //! [await_input] 377 } 378 379 // -----------------------Show the undistorted image for the image list ------------------------ 380 //! [show_results] 381 if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed ) 382 { 383 Mat view, rview, map1, map2; 384 initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), 385 getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), 386 imageSize, CV_16SC2, map1, map2); 387 388 for(size_t i = 0; i < s.imageList.size(); i++ ) 389 { 390 view = imread(s.imageList[i], 1); 391 if(view.empty()) 392 continue; 393 remap(view, rview, map1, map2, INTER_LINEAR); 394 imshow("Image View", rview); 395 char c = (char)waitKey(); 396 if( c == ESC_KEY || c == 'q' || c == 'Q' ) 397 break; 398 } 399 } 400 //! [show_results] 401 402 return 0; 403 } 404 405 //! [compute_errors] 406 static double computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints, 407 const vector<vector<Point2f> >& imagePoints, 408 const vector<Mat>& rvecs, const vector<Mat>& tvecs, 409 const Mat& cameraMatrix , const Mat& distCoeffs, 410 vector<float>& perViewErrors) 411 { 412 vector<Point2f> imagePoints2; 413 size_t totalPoints = 0; 414 double totalErr = 0, err; 415 perViewErrors.resize(objectPoints.size()); 416 417 for(size_t i = 0; i < objectPoints.size(); ++i ) 418 { 419 projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2); 420 err = norm(imagePoints[i], imagePoints2, NORM_L2); 421 422 size_t n = objectPoints[i].size(); 423 perViewErrors[i] = (float) std::sqrt(err*err/n); 424 totalErr += err*err; 425 totalPoints += n; 426 } 427 428 return std::sqrt(totalErr/totalPoints); 429 } 430 //! [compute_errors] 431 //! [board_corners] 432 static void calcBoardCornerPositions(Size boardSize, float squareSize, vector<Point3f>& corners, 433 Settings::Pattern patternType /*= Settings::CHESSBOARD*/) 434 { 435 corners.clear(); 436 437 switch(patternType) 438 { 439 case Settings::CHESSBOARD: 440 case Settings::CIRCLES_GRID: 441 for( int i = 0; i < boardSize.height; ++i ) 442 for( int j = 0; j < boardSize.width; ++j ) 443 corners.push_back(Point3f(j*squareSize, i*squareSize, 0)); 444 break; 445 446 case Settings::ASYMMETRIC_CIRCLES_GRID: 447 for( int i = 0; i < boardSize.height; i++ ) 448 for( int j = 0; j < boardSize.width; j++ ) 449 corners.push_back(Point3f((2*j + i % 2)*squareSize, i*squareSize, 0)); 450 break; 451 default: 452 break; 453 } 454 } 455 //! [board_corners] 456 static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs, 457 vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs, 458 vector<float>& reprojErrs, double& totalAvgErr) 459 { 460 //! [fixed_aspect] 461 cameraMatrix = Mat::eye(3, 3, CV_64F); 462 if( s.flag & CALIB_FIX_ASPECT_RATIO ) 463 cameraMatrix.at<double>(0,0) = s.aspectRatio; 464 //! [fixed_aspect] 465 distCoeffs = Mat::zeros(8, 1, CV_64F); 466 467 vector<vector<Point3f> > objectPoints(1); 468 calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern); 469 470 objectPoints.resize(imagePoints.size(),objectPoints[0]); 471 472 //Find intrinsic and extrinsic camera parameters 473 double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, 474 distCoeffs, rvecs, tvecs, s.flag|CALIB_FIX_K4|CALIB_FIX_K5); 475 476 cout << "Re-projection error reported by calibrateCamera: "<< rms << endl; 477 478 bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs); 479 480 totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, 481 rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs); 482 483 return ok; 484 } 485 486 // Print camera parameters to the output file 487 static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs, 488 const vector<Mat>& rvecs, const vector<Mat>& tvecs, 489 const vector<float>& reprojErrs, const vector<vector<Point2f> >& imagePoints, 490 double totalAvgErr ) 491 { 492 FileStorage fs( s.outputFileName, FileStorage::WRITE ); 493 494 time_t tm; 495 time( &tm ); 496 struct tm *t2 = localtime( &tm ); 497 char buf[1024]; 498 strftime( buf, sizeof(buf), "%c", t2 ); 499 500 fs << "calibration_time" << buf; 501 502 if( !rvecs.empty() || !reprojErrs.empty() ) 503 fs << "nr_of_frames" << (int)std::max(rvecs.size(), reprojErrs.size()); 504 fs << "image_width" << imageSize.width; 505 fs << "image_height" << imageSize.height; 506 fs << "board_width" << s.boardSize.width; 507 fs << "board_height" << s.boardSize.height; 508 fs << "square_size" << s.squareSize; 509 510 if( s.flag & CALIB_FIX_ASPECT_RATIO ) 511 fs << "fix_aspect_ratio" << s.aspectRatio; 512 513 if (s.flag) 514 { 515 sprintf(buf, "flags: %s%s%s%s", 516 s.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "", 517 s.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspect_ratio" : "", 518 s.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "", 519 s.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : ""); 520 cvWriteComment(*fs, buf, 0); 521 } 522 523 fs << "flags" << s.flag; 524 525 fs << "camera_matrix" << cameraMatrix; 526 fs << "distortion_coefficients" << distCoeffs; 527 528 fs << "avg_reprojection_error" << totalAvgErr; 529 if (s.writeExtrinsics && !reprojErrs.empty()) 530 fs << "per_view_reprojection_errors" << Mat(reprojErrs); 531 532 if(s.writeExtrinsics && !rvecs.empty() && !tvecs.empty() ) 533 { 534 CV_Assert(rvecs[0].type() == tvecs[0].type()); 535 Mat bigmat((int)rvecs.size(), 6, rvecs[0].type()); 536 for( size_t i = 0; i < rvecs.size(); i++ ) 537 { 538 Mat r = bigmat(Range(int(i), int(i+1)), Range(0,3)); 539 Mat t = bigmat(Range(int(i), int(i+1)), Range(3,6)); 540 541 CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1); 542 CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1); 543 //*.t() is MatExpr (not Mat) so we can use assignment operator 544 r = rvecs[i].t(); 545 t = tvecs[i].t(); 546 } 547 //cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 ); 548 fs << "extrinsic_parameters" << bigmat; 549 } 550 551 if(s.writePoints && !imagePoints.empty() ) 552 { 553 Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2); 554 for( size_t i = 0; i < imagePoints.size(); i++ ) 555 { 556 Mat r = imagePtMat.row(int(i)).reshape(2, imagePtMat.cols); 557 Mat imgpti(imagePoints[i]); 558 imgpti.copyTo(r); 559 } 560 fs << "image_points" << imagePtMat; 561 } 562 } 563 564 //! [run_and_save] 565 bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, 566 vector<vector<Point2f> > imagePoints) 567 { 568 vector<Mat> rvecs, tvecs; 569 vector<float> reprojErrs; 570 double totalAvgErr = 0; 571 572 bool ok = runCalibration(s, imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, reprojErrs, 573 totalAvgErr); 574 cout << (ok ? "Calibration succeeded" : "Calibration failed") 575 << ". avg re projection error = " << totalAvgErr << endl; 576 577 if (ok) 578 saveCameraParams(s, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, imagePoints, 579 totalAvgErr); 580 return ok; 581 } 582 //! [run_and_save] 583
