diff --git a/calibrationCharuco.cpp b/calibrationCharuco.cpp index 2ab61a5..dac360d 100644 --- a/calibrationCharuco.cpp +++ b/calibrationCharuco.cpp @@ -1,207 +1,89 @@ -/* -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" +#include #include -#include - -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 }"; -} +#include +#include int main(int argc, char *argv[]) { - CommandLineParser parser(argc, argv, keys); - parser.about(about); - if(argc < 7) { - parser.printMessage(); - return 0; - } + cv::Ptr AruCoDict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - 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 = cv::aruco::DetectorParameters::create(); - /*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; - } - - lccv::PiCamera cam; - cam.options->video_width=1024; - cam.options->video_height=768; - cam.options->framerate=5; - cam.options->verbose=true; - cam.startVideo(); - - usleep(500'000); - - int waitTime = 10; - - // VideoCapture inputVideo; - // 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)); - } + int squaresX = 5; + int squaresY = 7; + float squareLength = 0.04f; + float markerLength = 0.02f; + std::string outputFile = "camera_calibration.yml"; // create charuco board object - Ptr charucoboard = - aruco::CharucoBoard::create(squaresX, squaresY, squareLength, markerLength, dictionary); - Ptr board = charucoboard.staticCast(); + cv::Ptr charucoboard = + cv::aruco::CharucoBoard::create(squaresX, squaresY, squareLength, markerLength, AruCoDict); + cv::Ptr board = charucoboard.staticCast(); - // collect data from each frame - vector< vector< vector< Point2f > > > allCorners; - vector< vector< int > > allIds; - vector< Mat > allImgs; - Size imgSize; + 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(); - cv::namedWindow("out",cv::WINDOW_NORMAL); + cv::Ptr detectorParams = cv::aruco::DetectorParameters::create(); - char key = 0; + // 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; + + 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) { - Mat imageF, imageCopy, image; - cam.getVideoFrame(imageF,1000); - flip(imageF, image, -1); + cv::Mat image, imageCopy, imgNotRotated; + if(!cam->getVideoFrame(imgNotRotated,1000)){ + std::cout<<"Timeout error"< ids; - vector< vector< Point2f > > corners, rejected; + cv::flip(imgNotRotated, image, -1); - // detect markers - aruco::detectMarkers(image, dictionary, corners, ids, detectorParams, rejected); + std::vector< int > ids; + std::vector< std::vector< cv::Point2f > > corners, rejected; - // refind strategy to detect more markers - if(refindStrategy) aruco::refineDetectedMarkers(image, board, corners, ids, rejected); + cv::aruco::detectMarkers(image, AruCoDict, corners, ids, detectorParams, rejected); - // interpolate charuco corners - Mat currentCharucoCorners, currentCharucoIds; - if(ids.size() > 0) - aruco::interpolateCornersCharuco(corners, ids, image, charucoboard, currentCharucoCorners, + cv::Mat currentCharucoCorners, currentCharucoIds; + if(!ids.empty()) + cv::aruco::interpolateCornersCharuco(corners, ids, image, charucoboard, currentCharucoCorners, currentCharucoIds); - // draw results image.copyTo(imageCopy); - if(ids.size() > 0) aruco::drawDetectedMarkers(imageCopy, corners); + if(!ids.empty()) cv::aruco::drawDetectedMarkers(imageCopy, corners); if(currentCharucoCorners.total() > 0) - aruco::drawDetectedCornersCharuco(imageCopy, currentCharucoCorners, currentCharucoIds); + cv::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); + cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 0, 0), 2); - imshow("out", imageCopy); - key = static_cast(waitKey(waitTime)); - if(key == 'c' && ids.size() > 0) { - cout << "Frame captured" << endl; + imshow("Video", imageCopy); + key = (char)cv::waitKey(10); + if(key == 'c' && !ids.empty()) { + std::cout << "Frame captured" << std::endl; allCorners.push_back(corners); allIds.push_back(ids); allImgs.push_back(image); @@ -209,24 +91,29 @@ int main(int argc, char *argv[]) { } } - if(allIds.size() < 1) { - cerr << "Not enough captures for calibration" << endl; + + if(allIds.empty()) { + std::cerr << "Not enough captures for calibration" << std::endl; return 0; } - Mat cameraMatrix, distCoeffs; - vector< Mat > rvecs, tvecs; + cv::Mat cameraMatrix, distCoeffs; + std::vector rvecs, tvecs; double repError; - if(calibrationFlags & CALIB_FIX_ASPECT_RATIO) { - cameraMatrix = Mat::eye(3, 3, CV_64F); - cameraMatrix.at< double >(0, 0) = aspectRatio; - } + int calibrationFlags = 0; + // 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 - vector< vector< Point2f > > allCornersConcatenated; - vector< int > allIdsConcatenated; - vector< int > markerCounterPerFrame; + + std::vector< std::vector< cv::Point2f > > allCornersConcatenated; + std::vector< int > allIdsConcatenated; + std::vector< int > markerCounterPerFrame; markerCounterPerFrame.reserve(allCorners.size()); for(unsigned int i = 0; i < allCorners.size(); i++) { markerCounterPerFrame.push_back((int)allCorners[i].size()); @@ -236,24 +123,23 @@ int main(int argc, char *argv[]) { } } - // calibrate camera using aruco markers double arucoRepErr; - arucoRepErr = aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated, + arucoRepErr = cv::aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated, markerCounterPerFrame, board, imgSize, cameraMatrix, - distCoeffs, noArray(), noArray(), calibrationFlags); + distCoeffs, cv::noArray(), cv::noArray(), calibrationFlags); + - // prepare data for charuco calibration int nFrames = (int)allCorners.size(); - vector< Mat > allCharucoCorners; - vector< Mat > allCharucoIds; - vector< Mat > filteredImages; + std::vector< cv::Mat > allCharucoCorners; + std::vector< cv::Mat > allCharucoIds; + std::vector< cv::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, + cv::Mat currentCharucoCorners, currentCharucoIds; + cv::aruco::interpolateCornersCharuco(allCorners[i], allIds[i], allImgs[i], charucoboard, currentCharucoCorners, currentCharucoIds, cameraMatrix, distCoeffs); @@ -263,46 +149,27 @@ int main(int argc, char *argv[]) { } if(allCharucoCorners.size() < 4) { - cerr << "Not enough corners for calibration" << endl; + std::cerr << "Not enough corners for calibration" << std::endl; return 0; } // calibrate camera using charuco repError = - aruco::calibrateCameraCharuco(allCharucoCorners, allCharucoIds, charucoboard, imgSize, + cv::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; - } + // std::cout << "Rep Error: " << repError << std::endl; + std::cout << "Rep Error Aruco: " << arucoRepErr << std::endl; + std::cout << "Calibration saved to " << outputFile << std::endl; - cout << "Rep Error: " << repError << endl; - cout << "Rep Error Aruco: " << arucoRepErr << endl; - cout << "Calibration saved to " << outputFile << endl; + std::cout << "Rep Error: " << arucoRepErr << std::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; - } - } - - cam.stopVideo(); - cv::destroyWindow("out"); + cv::FileStorage fs(outputFile, cv::FileStorage::WRITE); + fs << "cameraMatrix" << cameraMatrix; + fs << "distCoeffs" << distCoeffs; + fs.release(); // Release the file + cam->stopVideo(); + cv::destroyAllWindows(); return 0; } \ No newline at end of file