From 906e0537ff81e7993cb05e66ad58a60328cf6579 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 16:31:45 +0200 Subject: [PATCH] calibration --- aruco_samples_utility.hpp | 48 +++++ calibrationCharuco.cpp | 391 ++++++++++++++++++++++++++++---------- 2 files changed, 334 insertions(+), 105 deletions(-) create mode 100644 aruco_samples_utility.hpp diff --git a/aruco_samples_utility.hpp b/aruco_samples_utility.hpp new file mode 100644 index 0000000..ca95e8f --- /dev/null +++ b/aruco_samples_utility.hpp @@ -0,0 +1,48 @@ +#include +#include +#include +#include + +namespace { + inline static bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat &distCoeffs) { + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + return false; + fs["camera_matrix"] >> camMatrix; + fs["distortion_coefficients"] >> distCoeffs; + return true; + } + + inline static bool saveCameraParams(const std::string &filename, cv::Size imageSize, float aspectRatio, int flags, + const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, double totalAvgErr) { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) + return false; + + time_t tt; + time(&tt); + struct tm *t2 = localtime(&tt); + char buf[1024]; + strftime(buf, sizeof(buf) - 1, "%c", t2); + + fs << "calibration_time" << buf; + fs << "image_width" << imageSize.width; + fs << "image_height" << imageSize.height; + + if (flags & cv::CALIB_FIX_ASPECT_RATIO) fs << "aspectRatio" << aspectRatio; + + if (flags != 0) { + sprintf(buf, "flags: %s%s%s%s", + flags & cv::CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "", + flags & cv::CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "", + flags & cv::CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "", + flags & cv::CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : ""); + } + fs << "flags" << flags; + fs << "camera_matrix" << cameraMatrix; + fs << "distortion_coefficients" << distCoeffs; + fs << "avg_reprojection_error" << totalAvgErr; + return true; + } + +} \ No newline at end of file diff --git a/calibrationCharuco.cpp b/calibrationCharuco.cpp index 40de38a..c84c816 100644 --- a/calibrationCharuco.cpp +++ b/calibrationCharuco.cpp @@ -1,120 +1,301 @@ +/* +By downloading, copying, installing or using the software you agree to this +license. If you do not agree to this license, do not download, install, +copy or use the software. + + License Agreement + For Open Source Computer Vision Library + (3-clause BSD License) + +Copyright (C) 2013, OpenCV Foundation, all rights reserved. +Third party copyrights are property of their respective owners. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the names of the copyright holders nor the names of the contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +This software is provided by the copyright holders and contributors "as is" and +any express or implied warranties, including, but not limited to, the implied +warranties of merchantability and fitness for a particular purpose are +disclaimed. In no event shall copyright holders or contributors be liable for +any direct, indirect, incidental, special, exemplary, or consequential damages +(including, but not limited to, procurement of substitute goods or services; +loss of use, data, or profits; or business interruption) however caused +and on any theory of liability, whether in contract, strict liability, +or tort (including negligence or otherwise) arising in any way out of +the use of this software, even if advised of the possibility of such damage. +*/ + + +#include +#include +#include +#include +#include #include -#include +#include "aruco_samples_utility.hpp" + +using namespace std; +using namespace cv; + +namespace { +const char* about = + "Calibration using a ChArUco board\n" + " To capture a frame for calibration, press 'c',\n" + " If input comes from video, press any key for next frame\n" + " To finish capturing, press 'ESC' key and calibration starts.\n"; +const char* keys = + "{w | | Number of squares in X direction }" + "{h | | Number of squares in Y direction }" + "{sl | | Square side length (in meters) }" + "{ml | | Marker side length (in meters) }" + "{d | | dictionary: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2," + "DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, " + "DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12," + "DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16}" + "{cd | | Input file with custom dictionary }" + "{@outfile | | Output file with calibrated camera parameters }" + "{v | | Input from video file, if ommited, input comes from camera }" + "{ci | 0 | Camera id if input doesnt come from video (-v) }" + "{dp | | File of marker detector parameters }" + "{rs | false | Apply refind strategy }" + "{zt | false | Assume zero tangential distortion }" + "{a | | Fix aspect ratio (fx/fy) to this value }" + "{pc | false | Fix the principal point at the center }" + "{sc | false | Show detected chessboard corners after calibration }"; +} + int main(int argc, char *argv[]) { + CommandLineParser parser(argc, argv, keys); + parser.about(about); - cv::aruco::Dictionary AruCoDict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - - int squaresX = 1920; - int squaresY = 1080; - float squareLength = 20; - float markerLength = 10; - - cv::aruco::CharucoBoard board(cv::Size(squaresX, squaresY), squareLength, markerLength, AruCoDict); - - cv::VideoCapture cap(0); - if (!cap.isOpened()) { - std::cerr << "Error: Could not open the camera." << std::endl; - return -1; - } - - cv::Mat img, imgCopy; - - cv::aruco::DetectorParameters detectorParams = cv::aruco::DetectorParameters(); - - cv::aruco::CharucoParameters charucoParams; - - cv::aruco::CharucoDetector detector(board, charucoParams, detectorParams); - - // Collect data from each frame - std::vector allCharucoCorners; - std::vector allCharucoIds; - - std::vector> allImagePoints; - std::vector> allObjectPoints; - - std::vector allImages; - cv::Size imageSize; - - while(cap.grab()) { - cv::Mat image, imageCopy; - cap.retrieve(image); - - std::vector markerIds; - std::vector> markerCorners, rejectedMarkers; - cv::Mat currentCharucoCorners; - cv::Mat currentCharucoIds; - std::vector currentObjectPoints; - std::vector currentImagePoints; - - // Detect ChArUco board - detector.detectBoard(image, currentCharucoCorners, currentCharucoIds); - - // Draw results - image.copyTo(imageCopy); - if(!markerIds.empty()) { - cv::aruco::drawDetectedMarkers(imageCopy, markerCorners); - } - - if(currentCharucoCorners.total() > 3) { - cv::aruco::drawDetectedCornersCharuco(imageCopy, currentCharucoCorners, currentCharucoIds); - } - - putText(imageCopy, "Press 'c' to add current frame. 'ESC' to finish and calibrate", - cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 0, 0), 2); - - cv::imshow("out", imageCopy); - - // Wait for key pressed - char key = static_cast(cv::waitKey(10)); - - if(key == 27) { - break; - } - - if(key == 'c' && currentCharucoCorners.total() > 3) { - // Match image points - board.matchImagePoints(currentCharucoCorners, currentCharucoIds, currentObjectPoints, currentImagePoints); - - if(currentImagePoints.empty() || currentObjectPoints.empty()) { - std::cout << "Point matching failed, try again." << std::endl; - continue; - } - - std::cout << "Frame captured" << std::endl; - - allCharucoCorners.push_back(currentCharucoCorners); - allCharucoIds.push_back(currentCharucoIds); - allImagePoints.push_back(currentImagePoints); - allObjectPoints.push_back(currentObjectPoints); - allImages.push_back(image); - - imageSize = image.size(); - } - } - - if(allCharucoCorners.size() < 4) { - std::cerr << "Not enough corners for calibration" << std::endl; + if(argc < 7) { + parser.printMessage(); return 0; } - cv::Mat cameraMatrix, distCoeffs; + int squaresX = parser.get("w"); + int squaresY = parser.get("h"); + float squareLength = parser.get("sl"); + float markerLength = parser.get("ml"); + string outputFile = parser.get(0); + + bool showChessboardCorners = parser.get("sc"); int calibrationFlags = 0; + float aspectRatio = 1; + if(parser.has("a")) { + calibrationFlags |= CALIB_FIX_ASPECT_RATIO; + aspectRatio = parser.get("a"); + } + if(parser.get("zt")) calibrationFlags |= CALIB_ZERO_TANGENT_DIST; + if(parser.get("pc")) calibrationFlags |= CALIB_FIX_PRINCIPAL_POINT; - // Calibrate camera using ChArUco - double repError = calibrateCamera( - allObjectPoints, allImagePoints, imageSize, - cameraMatrix, distCoeffs, cv::noArray(), cv::noArray(), cv::noArray(), - cv::noArray(), cv::noArray(), calibrationFlags - ); + Ptr detectorParams; + if(parser.has("dp")) { + FileStorage fs(parser.get("dp"), FileStorage::READ); + bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams); + if(!readOk) { + cerr << "Invalid detector parameters file" << endl; + return 0; + } + } - std::cout << "Rep Error: " << repError << std::endl; + bool refindStrategy = parser.get("rs"); + int camId = parser.get("ci"); + String video; - cv::FileStorage fs("calibration_results.yaml", cv::FileStorage::WRITE); - fs << "cameraMatrix" << cameraMatrix; - fs << "distCoeffs" << distCoeffs; - fs.release(); // Release the file + if(parser.has("v")) { + video = parser.get("v"); + } + + if(!parser.check()) { + parser.printErrors(); + return 0; + } + + VideoCapture inputVideo; + int waitTime; + if(!video.empty()) { + inputVideo.open(video); + waitTime = 0; + } else { + inputVideo.open(camId); + waitTime = 10; + } + + Ptr dictionary; + if (parser.has("d")) { + int dictionaryId = parser.get("d"); + dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId)); + } + else if (parser.has("cd")) { + FileStorage fs(parser.get("cd"), FileStorage::READ); + bool readOk = aruco::Dictionary::readDictionary(fs.root(), dictionary); + if(!readOk) { + cerr << "Invalid dictionary file" << endl; + return 0; + } + } + else { + cerr << "Dictionary not specified" << endl; + return 0; + } + + // create charuco board object + Ptr charucoboard = + aruco::CharucoBoard::create(squaresX, squaresY, squareLength, markerLength, dictionary); + Ptr board = charucoboard.staticCast(); + + // collect data from each frame + vector< vector< vector< Point2f > > > allCorners; + vector< vector< int > > allIds; + vector< Mat > allImgs; + Size imgSize; + + while(inputVideo.grab()) { + Mat image, imageCopy; + inputVideo.retrieve(image); + + vector< int > ids; + vector< vector< Point2f > > corners, rejected; + + // detect markers + aruco::detectMarkers(image, dictionary, corners, ids, detectorParams, rejected); + + // refind strategy to detect more markers + if(refindStrategy) aruco::refineDetectedMarkers(image, board, corners, ids, rejected); + + // interpolate charuco corners + Mat currentCharucoCorners, currentCharucoIds; + if(ids.size() > 0) + aruco::interpolateCornersCharuco(corners, ids, image, charucoboard, currentCharucoCorners, + currentCharucoIds); + + // draw results + image.copyTo(imageCopy); + if(ids.size() > 0) aruco::drawDetectedMarkers(imageCopy, corners); + + if(currentCharucoCorners.total() > 0) + aruco::drawDetectedCornersCharuco(imageCopy, currentCharucoCorners, currentCharucoIds); + + putText(imageCopy, "Press 'c' to add current frame. 'ESC' to finish and calibrate", + Point(10, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 0, 0), 2); + + imshow("out", imageCopy); + char key = (char)waitKey(waitTime); + if(key == 27) break; + if(key == 'c' && ids.size() > 0) { + cout << "Frame captured" << endl; + allCorners.push_back(corners); + allIds.push_back(ids); + allImgs.push_back(image); + imgSize = image.size(); + } + } + + if(allIds.size() < 1) { + cerr << "Not enough captures for calibration" << endl; + return 0; + } + + Mat cameraMatrix, distCoeffs; + vector< Mat > rvecs, tvecs; + double repError; + + if(calibrationFlags & CALIB_FIX_ASPECT_RATIO) { + cameraMatrix = Mat::eye(3, 3, CV_64F); + cameraMatrix.at< double >(0, 0) = aspectRatio; + } + + // prepare data for calibration + vector< vector< Point2f > > allCornersConcatenated; + vector< int > allIdsConcatenated; + vector< int > markerCounterPerFrame; + markerCounterPerFrame.reserve(allCorners.size()); + for(unsigned int i = 0; i < allCorners.size(); i++) { + markerCounterPerFrame.push_back((int)allCorners[i].size()); + for(unsigned int j = 0; j < allCorners[i].size(); j++) { + allCornersConcatenated.push_back(allCorners[i][j]); + allIdsConcatenated.push_back(allIds[i][j]); + } + } + + // calibrate camera using aruco markers + double arucoRepErr; + arucoRepErr = aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated, + markerCounterPerFrame, board, imgSize, cameraMatrix, + distCoeffs, noArray(), noArray(), calibrationFlags); + + // prepare data for charuco calibration + int nFrames = (int)allCorners.size(); + vector< Mat > allCharucoCorners; + vector< Mat > allCharucoIds; + vector< Mat > filteredImages; + allCharucoCorners.reserve(nFrames); + allCharucoIds.reserve(nFrames); + + for(int i = 0; i < nFrames; i++) { + // interpolate using camera parameters + Mat currentCharucoCorners, currentCharucoIds; + aruco::interpolateCornersCharuco(allCorners[i], allIds[i], allImgs[i], charucoboard, + currentCharucoCorners, currentCharucoIds, cameraMatrix, + distCoeffs); + + allCharucoCorners.push_back(currentCharucoCorners); + allCharucoIds.push_back(currentCharucoIds); + filteredImages.push_back(allImgs[i]); + } + + if(allCharucoCorners.size() < 4) { + cerr << "Not enough corners for calibration" << endl; + return 0; + } + + // calibrate camera using charuco + repError = + aruco::calibrateCameraCharuco(allCharucoCorners, allCharucoIds, charucoboard, imgSize, + cameraMatrix, distCoeffs, rvecs, tvecs, calibrationFlags); + + bool saveOk = saveCameraParams(outputFile, imgSize, aspectRatio, calibrationFlags, + cameraMatrix, distCoeffs, repError); + if(!saveOk) { + cerr << "Cannot save output file" << endl; + return 0; + } + + cout << "Rep Error: " << repError << endl; + cout << "Rep Error Aruco: " << arucoRepErr << endl; + cout << "Calibration saved to " << outputFile << endl; + + // show interpolated charuco corners for debugging + if(showChessboardCorners) { + for(unsigned int frame = 0; frame < filteredImages.size(); frame++) { + Mat imageCopy = filteredImages[frame].clone(); + if(allIds[frame].size() > 0) { + + if(allCharucoCorners[frame].total() > 0) { + aruco::drawDetectedCornersCharuco( imageCopy, allCharucoCorners[frame], + allCharucoIds[frame]); + } + } + + imshow("out", imageCopy); + char key = (char)waitKey(0); + if(key == 27) break; + } + } return 0; } \ No newline at end of file