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 51e21a2..c84c816 100644 --- a/calibrationCharuco.cpp +++ b/calibrationCharuco.cpp @@ -1,89 +1,203 @@ -#include -#include -#include -#include +/* +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 "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::Ptr AruCoDict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + if(argc < 7) { + parser.printMessage(); + return 0; + } - int squaresX = 5; - int squaresY = 7; - float squareLength = 0.0336f; - float markerLength = 0.01682f; - std::string outputFile = "camera_calibration.yml"; + 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; + + 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; + } + } + + bool refindStrategy = parser.get("rs"); + int camId = parser.get("ci"); + String video; + + 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 - cv::Ptr charucoboard = - cv::aruco::CharucoBoard::create(squaresX, squaresY, squareLength, markerLength, AruCoDict); - cv::Ptr board = charucoboard.staticCast(); + Ptr charucoboard = + aruco::CharucoBoard::create(squaresX, squaresY, squareLength, markerLength, dictionary); + Ptr board = charucoboard.staticCast(); - lccv::PiCamera* cam = new lccv::PiCamera; - cam->options->video_width=1920; - cam->options->video_height=1080; - cam->options->framerate=5; - cam->options->verbose=true; - cv::namedWindow("Video",cv::WINDOW_NORMAL); - cam->startVideo(); + // collect data from each frame + vector< vector< vector< Point2f > > > allCorners; + vector< vector< int > > allIds; + vector< Mat > allImgs; + Size imgSize; - cv::Ptr detectorParams = cv::aruco::DetectorParameters::create(); + while(inputVideo.grab()) { + Mat image, imageCopy; + inputVideo.retrieve(image); - // cv::aruco::CharucoParameters charucoParams; + vector< int > ids; + vector< vector< Point2f > > corners, rejected; - // cv::aruco::CharucoDetector detector(board, charucoParams, detectorParams); + // detect markers + aruco::detectMarkers(image, dictionary, corners, ids, detectorParams, rejected); - // Collect data from each frame - // std::vector allCharucoCorners; - // std::vector allCharucoIds; - // - // std::vector> allImagePoints; - // std::vector> allObjectPoints; - // - // std::vector allImages; - // cv::Size imageSize; + // refind strategy to detect more markers + if(refindStrategy) aruco::refineDetectedMarkers(image, board, corners, ids, rejected); - std::vector< std::vector< std::vector< cv::Point2f > > > allCorners; - std::vector< std::vector< int > > allIds; - std::vector< cv::Mat > allImgs; - cv::Size imgSize; - - char key; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - while(key != 27) { - cv::Mat image, imageCopy, imgNotRotated; - if(!cam->getVideoFrame(imgNotRotated,1000)){ - std::cout<<"Timeout error"< ids; - std::vector< std::vector< cv::Point2f > > corners, rejected; - - cv::aruco::detectMarkers(image, AruCoDict, corners, ids, detectorParams, rejected); - - cv::Mat currentCharucoCorners, currentCharucoIds; - if(!ids.empty()) - cv::aruco::interpolateCornersCharuco(corners, ids, image, charucoboard, currentCharucoCorners, + // 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.empty()) cv::aruco::drawDetectedMarkers(imageCopy, corners); + if(ids.size() > 0) aruco::drawDetectedMarkers(imageCopy, corners); if(currentCharucoCorners.total() > 0) - cv::aruco::drawDetectedCornersCharuco(imageCopy, currentCharucoCorners, currentCharucoIds); + 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); + Point(10, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 0, 0), 2); - imshow("Video", imageCopy); - key = (char)cv::waitKey(10); - if(key == 'c' && !ids.empty()) { - std::cout << "Frame captured" << std::endl; + 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); @@ -91,29 +205,24 @@ int main(int argc, char *argv[]) { } } - - if(allIds.empty()) { - std::cerr << "Not enough captures for calibration" << std::endl; + if(allIds.size() < 1) { + cerr << "Not enough captures for calibration" << endl; return 0; } - cv::Mat cameraMatrix, distCoeffs; - std::vector rvecs, tvecs; + Mat cameraMatrix, distCoeffs; + vector< Mat > rvecs, tvecs; double repError; - int calibrationFlags = 0; + if(calibrationFlags & CALIB_FIX_ASPECT_RATIO) { + cameraMatrix = Mat::eye(3, 3, CV_64F); + cameraMatrix.at< double >(0, 0) = aspectRatio; + } - // Calibrate camera using ChArUco - // double repError = calibrateCamera( - // allObjectPoints, allImagePoints, imageSize, - // cameraMatrix, distCoeffs, cv::noArray(), cv::noArray(), cv::noArray(), - // cv::noArray(), cv::noArray(), calibrationFlags - // ); // prepare data for calibration - - std::vector< std::vector< cv::Point2f > > allCornersConcatenated; - std::vector< int > allIdsConcatenated; - std::vector< int > markerCounterPerFrame; + 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()); @@ -123,23 +232,24 @@ int main(int argc, char *argv[]) { } } + // calibrate camera using aruco markers double arucoRepErr; - arucoRepErr = cv::aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated, + arucoRepErr = aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated, markerCounterPerFrame, board, imgSize, cameraMatrix, - distCoeffs, cv::noArray(), cv::noArray(), calibrationFlags); - + distCoeffs, noArray(), noArray(), calibrationFlags); + // prepare data for charuco calibration int nFrames = (int)allCorners.size(); - std::vector< cv::Mat > allCharucoCorners; - std::vector< cv::Mat > allCharucoIds; - std::vector< cv::Mat > filteredImages; + 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 - cv::Mat currentCharucoCorners, currentCharucoIds; - cv::aruco::interpolateCornersCharuco(allCorners[i], allIds[i], allImgs[i], charucoboard, + Mat currentCharucoCorners, currentCharucoIds; + aruco::interpolateCornersCharuco(allCorners[i], allIds[i], allImgs[i], charucoboard, currentCharucoCorners, currentCharucoIds, cameraMatrix, distCoeffs); @@ -149,27 +259,43 @@ int main(int argc, char *argv[]) { } if(allCharucoCorners.size() < 4) { - std::cerr << "Not enough corners for calibration" << std::endl; + cerr << "Not enough corners for calibration" << endl; return 0; } // calibrate camera using charuco repError = - cv::aruco::calibrateCameraCharuco(allCharucoCorners, allCharucoIds, charucoboard, imgSize, + aruco::calibrateCameraCharuco(allCharucoCorners, allCharucoIds, charucoboard, imgSize, cameraMatrix, distCoeffs, rvecs, tvecs, calibrationFlags); - // std::cout << "Rep Error: " << repError << std::endl; - std::cout << "Rep Error Aruco: " << arucoRepErr << std::endl; - std::cout << "Calibration saved to " << outputFile << std::endl; + bool saveOk = saveCameraParams(outputFile, imgSize, aspectRatio, calibrationFlags, + cameraMatrix, distCoeffs, repError); + if(!saveOk) { + cerr << "Cannot save output file" << endl; + return 0; + } - std::cout << "Rep Error: " << arucoRepErr << std::endl; + cout << "Rep Error: " << repError << endl; + cout << "Rep Error Aruco: " << arucoRepErr << endl; + cout << "Calibration saved to " << outputFile << endl; - cv::FileStorage fs(outputFile, cv::FileStorage::WRITE); - fs << "cameraMatrix" << cameraMatrix; - fs << "distCoeffs" << distCoeffs; - fs.release(); // Release the file - cam->stopVideo(); - cv::destroyAllWindows(); + // 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