From 121034876ddfe9553babc868db644e2ebd74e318 Mon Sep 17 00:00:00 2001 From: Alexander Panov Date: Wed, 28 Dec 2022 17:28:59 +0300 Subject: [PATCH] Merge pull request #22986 from AleksandrPanov:move_contrib_charuco_to_main_objdetect merge with https://github.com/opencv/opencv_contrib/pull/3394 move Charuco API from contrib to main repo: - add CharucoDetector: ``` CharucoDetector::detectBoard(InputArray image, InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds, OutputArray charucoCorners, OutputArray charucoIds) const // detect charucoCorners and/or markerCorners CharucoDetector::detectDiamonds(InputArray image, InputOutputArrayOfArrays _markerCorners, InputOutputArrayOfArrays _markerIds, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds) const ``` - add `matchImagePoints()` for `CharucoBoard` - remove contrib aruco dependencies from interactive-calibration tool - move almost all aruco tests to objdetect ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- apps/interactive-calibration/CMakeLists.txt | 5 +- apps/interactive-calibration/calibController.cpp | 8 +- apps/interactive-calibration/frameProcessor.cpp | 62 +- apps/interactive-calibration/frameProcessor.hpp | 7 +- apps/interactive-calibration/main.cpp | 37 +- .../parametersController.cpp | 3 +- modules/objdetect/include/opencv2/objdetect.hpp | 21 + .../include/opencv2/objdetect/aruco_board.hpp | 139 ++--- .../include/opencv2/objdetect/aruco_detector.hpp | 29 +- .../include/opencv2/objdetect/aruco_dictionary.hpp | 2 +- .../include/opencv2/objdetect/charuco_detector.hpp | 154 +++++ modules/objdetect/misc/java/test/ArucoTest.java | 32 +- .../misc/python/test/test_objdetect_aruco.py | 62 +- modules/objdetect/perf/perf_aruco.cpp | 285 +++++++++ modules/objdetect/src/aruco/aruco_board.cpp | 536 +++++++++-------- modules/objdetect/src/aruco/aruco_detector.cpp | 78 ++- modules/objdetect/src/aruco/charuco_detector.cpp | 521 ++++++++++++++++ modules/objdetect/test/test_aruco_utils.cpp | 205 +++++++ modules/objdetect/test/test_aruco_utils.hpp | 42 ++ modules/objdetect/test/test_boarddetection.cpp | 321 ++++++++++ modules/objdetect/test/test_charucodetection.cpp | 659 +++++++++++++++++++++ 21 files changed, 2715 insertions(+), 493 deletions(-) create mode 100644 modules/objdetect/include/opencv2/objdetect/charuco_detector.hpp create mode 100644 modules/objdetect/perf/perf_aruco.cpp create mode 100644 modules/objdetect/src/aruco/charuco_detector.cpp create mode 100644 modules/objdetect/test/test_aruco_utils.cpp create mode 100644 modules/objdetect/test/test_aruco_utils.hpp create mode 100644 modules/objdetect/test/test_boarddetection.cpp create mode 100644 modules/objdetect/test/test_charucodetection.cpp diff --git a/apps/interactive-calibration/CMakeLists.txt b/apps/interactive-calibration/CMakeLists.txt index dacbb13..efac5fc 100644 --- a/apps/interactive-calibration/CMakeLists.txt +++ b/apps/interactive-calibration/CMakeLists.txt @@ -1,6 +1,3 @@ -set(DEPS opencv_core opencv_imgproc opencv_features2d opencv_highgui opencv_calib3d opencv_videoio) -if(${BUILD_opencv_aruco}) - list(APPEND DEPS opencv_aruco) -endif() +set(DEPS opencv_core opencv_imgproc opencv_features2d opencv_highgui opencv_calib3d opencv_videoio opencv_objdetect) file(GLOB SRCS *.cpp) ocv_add_application(opencv_interactive-calibration MODULES ${DEPS} SRCS ${SRCS}) diff --git a/apps/interactive-calibration/calibController.cpp b/apps/interactive-calibration/calibController.cpp index 9efb9d0..b2e5b87 100644 --- a/apps/interactive-calibration/calibController.cpp +++ b/apps/interactive-calibration/calibController.cpp @@ -219,10 +219,10 @@ void calib::calibDataController::filterFrames() if(mCalibData->imagePoints.size()) { mCalibData->imagePoints.erase(mCalibData->imagePoints.begin() + worstElemIndex); mCalibData->objectPoints.erase(mCalibData->objectPoints.begin() + worstElemIndex); - } - else { - mCalibData->allCharucoCorners.erase(mCalibData->allCharucoCorners.begin() + worstElemIndex); - mCalibData->allCharucoIds.erase(mCalibData->allCharucoIds.begin() + worstElemIndex); + if (mCalibData->allCharucoCorners.size()) { + mCalibData->allCharucoCorners.erase(mCalibData->allCharucoCorners.begin() + worstElemIndex); + mCalibData->allCharucoIds.erase(mCalibData->allCharucoIds.begin() + worstElemIndex); + } } cv::Mat newErrorsVec = cv::Mat((int)numberOfFrames - 1, 1, CV_64F); diff --git a/apps/interactive-calibration/frameProcessor.cpp b/apps/interactive-calibration/frameProcessor.cpp index 18d0d87..a3c17de 100644 --- a/apps/interactive-calibration/frameProcessor.cpp +++ b/apps/interactive-calibration/frameProcessor.cpp @@ -11,7 +11,6 @@ #include #include -#include #include using namespace calib; @@ -74,17 +73,12 @@ bool CalibProcessor::detectAndParseChessboard(const cv::Mat &frame) bool CalibProcessor::detectAndParseChAruco(const cv::Mat &frame) { -#ifdef HAVE_OPENCV_ARUCO cv::Ptr board = mCharucoBoard.staticCast(); std::vector > corners, rejected; std::vector ids; - cv::aruco::detectMarkers(frame, cv::makePtr(mArucoDictionary), corners, ids, cv::makePtr(), rejected); - cv::aruco::refineDetectedMarkers(frame, board, corners, ids, rejected); cv::Mat currentCharucoCorners, currentCharucoIds; - if(ids.size() > 0) - cv::aruco::interpolateCornersCharuco(corners, ids, frame, mCharucoBoard, currentCharucoCorners, - currentCharucoIds); + detector->detectBoard(frame, currentCharucoCorners, currentCharucoIds, corners, ids); if(ids.size() > 0) cv::aruco::drawDetectedMarkers(frame, corners); if(currentCharucoCorners.total() > 3) { @@ -102,9 +96,6 @@ bool CalibProcessor::detectAndParseChAruco(const cv::Mat &frame) mCurrentCharucoIds = currentCharucoIds; return true; } -#else - CV_UNUSED(frame); -#endif return false; } @@ -155,6 +146,7 @@ bool CalibProcessor::detectAndParseDualACircles(const cv::Mat &frame) void CalibProcessor::saveFrameData() { std::vector objectPoints; + std::vector imagePoints; switch(mBoardType) { @@ -169,6 +161,11 @@ void CalibProcessor::saveFrameData() case chAruco: mCalibData->allCharucoCorners.push_back(mCurrentCharucoCorners); mCalibData->allCharucoIds.push_back(mCurrentCharucoIds); + + mCharucoBoard->matchImagePoints(mCurrentCharucoCorners, mCurrentCharucoIds, objectPoints, imagePoints); + CV_Assert(mCurrentCharucoIds.total() == imagePoints.size()); + mCalibData->imagePoints.push_back(imagePoints); + mCalibData->objectPoints.push_back(objectPoints); break; case CirclesGrid: objectPoints.reserve(mBoardSize.height*mBoardSize.width); @@ -248,37 +245,17 @@ bool CalibProcessor::checkLastFrame() else mCalibData->cameraMatrix.copyTo(tmpCamMatrix); - if(mBoardType != chAruco) { - cv::Mat r, t, angles; - cv::solvePnP(mCalibData->objectPoints.back(), mCurrentImagePoints, tmpCamMatrix, mCalibData->distCoeffs, r, t); - RodriguesToEuler(r, angles, CALIB_DEGREES); - - if(fabs(angles.at(0)) > badAngleThresh || fabs(angles.at(1)) > badAngleThresh) { - mCalibData->objectPoints.pop_back(); - mCalibData->imagePoints.pop_back(); - isFrameBad = true; - } - } - else { -#ifdef HAVE_OPENCV_ARUCO - cv::Mat r, t, angles; - std::vector allObjPoints; - allObjPoints.reserve(mCurrentCharucoIds.total()); - for(size_t i = 0; i < mCurrentCharucoIds.total(); i++) { - int pointID = mCurrentCharucoIds.at((int)i); - CV_Assert(pointID >= 0 && pointID < (int)mCharucoBoard->getChessboardCorners().size()); - allObjPoints.push_back(mCharucoBoard->getChessboardCorners()[pointID]); - } - - cv::solvePnP(allObjPoints, mCurrentCharucoCorners, tmpCamMatrix, mCalibData->distCoeffs, r, t); - RodriguesToEuler(r, angles, CALIB_DEGREES); - - if(180.0 - fabs(angles.at(0)) > badAngleThresh || fabs(angles.at(1)) > badAngleThresh) { - isFrameBad = true; + cv::Mat r, t, angles; + cv::solvePnP(mCalibData->objectPoints.back(), mCalibData->imagePoints.back(), tmpCamMatrix, mCalibData->distCoeffs, r, t); + RodriguesToEuler(r, angles, CALIB_DEGREES); + if(fabs(angles.at(0)) > badAngleThresh || fabs(angles.at(1)) > badAngleThresh) { + mCalibData->objectPoints.pop_back(); + mCalibData->imagePoints.pop_back(); + if (mCalibData->allCharucoCorners.size()) { mCalibData->allCharucoCorners.pop_back(); mCalibData->allCharucoIds.pop_back(); } -#endif + isFrameBad = true; } return isFrameBad; } @@ -295,15 +272,16 @@ CalibProcessor::CalibProcessor(cv::Ptr data, captureParameters mTemplDist = capParams.templDst; mSaveFrames = capParams.saveFrames; mZoom = capParams.zoom; + cv::aruco::CharucoParameters charucoParameters; + charucoParameters.tryRefineMarkers = true; switch(mBoardType) { case chAruco: -#ifdef HAVE_OPENCV_ARUCO mArucoDictionary = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(capParams.charucoDictName)); - mCharucoBoard = cv::aruco::CharucoBoard::create(mBoardSize.width, mBoardSize.height, capParams.charucoSquareLength, - capParams.charucoMarkerSize, mArucoDictionary); -#endif + mCharucoBoard = cv::makePtr(cv::Size(mBoardSize.width + 1, mBoardSize.height + 1), capParams.charucoSquareLength, + capParams.charucoMarkerSize, mArucoDictionary); + detector = cv::makePtr(cv::aruco::CharucoDetector(*mCharucoBoard, charucoParameters)); break; case CirclesGrid: case AcirclesGrid: diff --git a/apps/interactive-calibration/frameProcessor.hpp b/apps/interactive-calibration/frameProcessor.hpp index 51c2906..8ecdca7 100644 --- a/apps/interactive-calibration/frameProcessor.hpp +++ b/apps/interactive-calibration/frameProcessor.hpp @@ -7,9 +7,7 @@ #include #include -#ifdef HAVE_OPENCV_ARUCO -#include -#endif +#include #include "calibCommon.hpp" #include "calibController.hpp" @@ -39,10 +37,9 @@ protected: cv::Mat mCurrentCharucoIds; cv::Ptr mBlobDetectorPtr; -#ifdef HAVE_OPENCV_ARUCO cv::aruco::Dictionary mArucoDictionary; cv::Ptr mCharucoBoard; -#endif + cv::Ptr detector; int mNeededFramesNum; unsigned mDelayBetweenCaptures; diff --git a/apps/interactive-calibration/main.cpp b/apps/interactive-calibration/main.cpp index d83ef95..4d665a6 100644 --- a/apps/interactive-calibration/main.cpp +++ b/apps/interactive-calibration/main.cpp @@ -7,9 +7,6 @@ #include #include -#ifdef HAVE_OPENCV_ARUCO -#include -#endif #include #include @@ -105,11 +102,6 @@ int main(int argc, char** argv) captureParameters capParams = paramsController.getCaptureParameters(); internalParameters intParams = paramsController.getInternalParameters(); -#ifndef HAVE_OPENCV_ARUCO - if(capParams.board == chAruco) - CV_Error(cv::Error::StsNotImplemented, "Aruco module is disabled in current build configuration." - " Consider usage of another calibration pattern\n"); -#endif cv::TermCriteria solverTermCrit = cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, intParams.solverMaxIters, intParams.solverEps); @@ -170,29 +162,12 @@ int main(int argc, char** argv) globalData->imageSize = pipeline->getImageSize(); calibrationFlags = controller->getNewFlags(); - if(capParams.board != chAruco) { - globalData->totalAvgErr = - cv::calibrateCamera(globalData->objectPoints, globalData->imagePoints, - globalData->imageSize, globalData->cameraMatrix, - globalData->distCoeffs, cv::noArray(), cv::noArray(), - globalData->stdDeviations, cv::noArray(), globalData->perViewErrors, - calibrationFlags, solverTermCrit); - } - else { -#ifdef HAVE_OPENCV_ARUCO - cv::aruco::Dictionary dictionary = - cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(capParams.charucoDictName)); - cv::Ptr charucoboard = - cv::aruco::CharucoBoard::create(capParams.boardSize.width, capParams.boardSize.height, - capParams.charucoSquareLength, capParams.charucoMarkerSize, dictionary); - globalData->totalAvgErr = - cv::aruco::calibrateCameraCharuco(globalData->allCharucoCorners, globalData->allCharucoIds, - charucoboard, globalData->imageSize, - globalData->cameraMatrix, globalData->distCoeffs, - cv::noArray(), cv::noArray(), globalData->stdDeviations, cv::noArray(), - globalData->perViewErrors, calibrationFlags, solverTermCrit); -#endif - } + globalData->totalAvgErr = + cv::calibrateCamera(globalData->objectPoints, globalData->imagePoints, + globalData->imageSize, globalData->cameraMatrix, + globalData->distCoeffs, cv::noArray(), cv::noArray(), + globalData->stdDeviations, cv::noArray(), globalData->perViewErrors, + calibrationFlags, solverTermCrit); dataController->updateUndistortMap(); dataController->printParametersToConsole(std::cout); controller->updateState(); diff --git a/apps/interactive-calibration/parametersController.cpp b/apps/interactive-calibration/parametersController.cpp index ebec850..eab183a 100644 --- a/apps/interactive-calibration/parametersController.cpp +++ b/apps/interactive-calibration/parametersController.cpp @@ -109,6 +109,7 @@ bool calib::parametersController::loadFromParser(cv::CommandLineParser &parser) std::string templateType = parser.get("t"); + if(templateType.find("symcircles", 0) == 0) { mCapParams.board = CirclesGrid; mCapParams.boardSize = cv::Size(4, 11); @@ -127,7 +128,7 @@ bool calib::parametersController::loadFromParser(cv::CommandLineParser &parser) } else if(templateType.find("charuco", 0) == 0) { mCapParams.board = chAruco; - mCapParams.boardSize = cv::Size(6, 8); + mCapParams.boardSize = cv::Size(5, 7); mCapParams.charucoDictName = 0; mCapParams.charucoSquareLength = 200; mCapParams.charucoMarkerSize = 100; diff --git a/modules/objdetect/include/opencv2/objdetect.hpp b/modules/objdetect/include/opencv2/objdetect.hpp index 58f1aee..c398f3a 100644 --- a/modules/objdetect/include/opencv2/objdetect.hpp +++ b/modules/objdetect/include/opencv2/objdetect.hpp @@ -105,6 +105,26 @@ using a Boosted Cascade of Simple Features. IEEE CVPR, 2001. The paper is availa @defgroup objdetect_dnn_face DNN-based face detection and recognition Check @ref tutorial_dnn_face "the corresponding tutorial" for more details. @defgroup objdetect_common Common functions and classes + @defgroup objdetect_aruco ArUco markers and boards detection for robust camera pose estimation + @{ + ArUco Marker Detection + Square fiducial markers (also known as Augmented Reality Markers) are useful for easy, + fast and robust camera pose estimation. + + The main functionality of ArucoDetector class is detection of markers in an image. If the markers are grouped + as a board, then you can try to recover the missing markers with ArucoDetector::refineDetectedMarkers(). + ArUco markers can also be used for advanced chessboard corner finding. To do this, group the markers in the + CharucoBoard and find the corners of the chessboard with the CharucoDetector::detectBoard(). + + The implementation is based on the ArUco Library by R. Muñoz-Salinas and S. Garrido-Jurado @cite Aruco2014. + + Markers can also be detected based on the AprilTag 2 @cite wang2016iros fiducial detection method. + + @sa @cite Aruco2014 + This code has been originally developed by Sergio Garrido-Jurado as a project + for Google Summer of Code 2015 (GSoC 15). + @} + @} */ @@ -852,5 +872,6 @@ protected: #include "opencv2/objdetect/detection_based_tracker.hpp" #include "opencv2/objdetect/face.hpp" #include "opencv2/objdetect/aruco_detector.hpp" +#include "opencv2/objdetect/charuco_detector.hpp" #endif diff --git a/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp b/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp index fb90c7b..90ad2b8 100644 --- a/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp +++ b/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp @@ -8,7 +8,7 @@ namespace cv { namespace aruco { -//! @addtogroup aruco +//! @addtogroup objdetect_aruco //! @{ class Dictionary; @@ -22,29 +22,15 @@ class Dictionary; * - The dictionary which indicates the type of markers of the board * - The identifier of all the markers in the board. */ -class CV_EXPORTS_W Board { -protected: - Board(); // use ::create() +class CV_EXPORTS_W_SIMPLE Board { public: - /** @brief Draw a planar board - * - * @param outSize size of the output image in pixels. - * @param img output image with the board. The size of this image will be outSize - * and the board will be on the center, keeping the board proportions. - * @param marginSize minimum margins (in pixels) of the board in the output image - * @param borderBits width of the marker borders. - * - * This function return the image of the GridBoard, ready to be printed. - */ - CV_WRAP virtual void generateImage(Size outSize, OutputArray img, int marginSize = 0, int borderBits = 1) const; - - /** @brief Provide way to create Board by passing necessary data. Specially needed in Python. + /** @brief Common Board constructor * * @param objPoints array of object points of all the marker corners in the board * @param dictionary the dictionary of markers employed for this board * @param ids vector of the identifiers of the markers in the board */ - CV_WRAP static Ptr create(InputArrayOfArrays objPoints, const Dictionary &dictionary, InputArray ids); + CV_WRAP Board(InputArrayOfArrays objPoints, const Dictionary& dictionary, InputArray ids); /** @brief return the Dictionary of markers employed for this board */ @@ -72,31 +58,19 @@ public: CV_WRAP const Point3f& getRightBottomCorner() const; /** @brief Given a board configuration and a set of detected markers, returns the corresponding - * image points and object points to call solvePnP + * image points and object points to call solvePnP() * * @param detectedCorners List of detected marker corners of the board. - * @param detectedIds List of identifiers for each marker. + * For CharucoBoard class you can set list of charuco corners. + * @param detectedIds List of identifiers for each marker or list of charuco identifiers for each corner. + * For CharucoBoard class you can set list of charuco identifiers for each corner. * @param objPoints Vector of vectors of board marker points in the board coordinate space. * @param imgPoints Vector of vectors of the projections of board marker corner points. */ CV_WRAP void matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) const; - virtual ~Board(); -protected: - struct BoardImpl; - Ptr boardImpl; -}; -/** @brief Planar board with grid arrangement of markers - * - * More common type of board. All markers are placed in the same plane in a grid arrangement. - * The board image can be drawn using generateImage() method. - */ -class CV_EXPORTS_W GridBoard : public Board { -protected: - GridBoard(); -public: - /** @brief Draw a GridBoard + /** @brief Draw a planar board * * @param outSize size of the output image in pixels. * @param img output image with the board. The size of this image will be outSize @@ -104,50 +78,44 @@ public: * @param marginSize minimum margins (in pixels) of the board in the output image * @param borderBits width of the marker borders. * - * This function return the image of the GridBoard, ready to be printed. + * This function return the image of the board, ready to be printed. */ - CV_WRAP void generateImage(Size outSize, OutputArray img, int marginSize = 0, int borderBits = 1) const CV_OVERRIDE; + CV_WRAP void generateImage(Size outSize, OutputArray img, int marginSize = 0, int borderBits = 1) const; - /** - * @brief Create a GridBoard object - * - * @param markersX number of markers in X direction - * @param markersY number of markers in Y direction - * @param markerLength marker side length (normally in meters) - * @param markerSeparation separation between two markers (same unit as markerLength) - * @param dictionary dictionary of markers indicating the type of markers - * @param ids set marker ids in dictionary to use on board. - * @return the output GridBoard object - * - * This functions creates a GridBoard object given the number of markers in each direction and - * the marker size and marker separation. - */ - CV_WRAP static Ptr create(int markersX, int markersY, float markerLength, float markerSeparation, - const Dictionary &dictionary, InputArray ids); + CV_DEPRECATED_EXTERNAL // avoid using in C++ code, will be moved to “protected” (need to fix bindings first) + Board(); + struct Impl; +protected: + Board(const Ptr& impl); + Ptr impl; +}; + +/** @brief Planar board with grid arrangement of markers + * + * More common type of board. All markers are placed in the same plane in a grid arrangement. + * The board image can be drawn using generateImage() method. + */ +class CV_EXPORTS_W_SIMPLE GridBoard : public Board { +public: /** - * @overload - * @brief Create a GridBoard object + * @brief GridBoard constructor * - * @param markersX number of markers in X direction - * @param markersY number of markers in Y direction + * @param size number of markers in x and y directions * @param markerLength marker side length (normally in meters) * @param markerSeparation separation between two markers (same unit as markerLength) * @param dictionary dictionary of markers indicating the type of markers - * @param firstMarker id of first marker in dictionary to use on board. - * @return the output GridBoard object + * @param ids set of marker ids in dictionary to use on board. */ - CV_WRAP static Ptr create(int markersX, int markersY, float markerLength, float markerSeparation, - const Dictionary &dictionary, int firstMarker = 0); + CV_WRAP GridBoard(const Size& size, float markerLength, float markerSeparation, + const Dictionary &dictionary, InputArray ids = noArray()); CV_WRAP Size getGridSize() const; CV_WRAP float getMarkerLength() const; CV_WRAP float getMarkerSeparation() const; -protected: - struct GridImpl; - Ptr gridImpl; - friend class CharucoBoard; + CV_DEPRECATED_EXTERNAL // avoid using in C++ code, will be moved to “protected” (need to fix bindings first) + GridBoard(); }; /** @@ -156,40 +124,19 @@ protected: * The benefits of ChArUco boards is that they provide both, ArUco markers versatility and chessboard corner precision, * which is important for calibration and pose estimation. The board image can be drawn using generateImage() method. */ -class CV_EXPORTS_W CharucoBoard : public Board { -protected: - CharucoBoard(); +class CV_EXPORTS_W_SIMPLE CharucoBoard : public Board { public: - - /** @brief Draw a ChArUco board + /** @brief CharucoBoard constructor * - * @param outSize size of the output image in pixels. - * @param img output image with the board. The size of this image will be outSize - * and the board will be on the center, keeping the board proportions. - * @param marginSize minimum margins (in pixels) of the board in the output image - * @param borderBits width of the marker borders. - * - * This function return the image of the ChArUco board, ready to be printed. - */ - CV_WRAP void generateImage(Size outSize, OutputArray img, int marginSize = 0, int borderBits = 1) const CV_OVERRIDE; - - - /** @brief Create a CharucoBoard object - * - * @param squaresX number of chessboard squares in X direction - * @param squaresY number of chessboard squares in Y direction - * @param squareLength chessboard square side length (normally in meters) + * @param size number of chessboard squares in x and y directions + * @param squareLength squareLength chessboard square side length (normally in meters) * @param markerLength marker side length (same unit than squareLength) - * @param dictionary dictionary of markers indicating the type of markers. + * @param dictionary dictionary of markers indicating the type of markers * @param ids array of id used markers * The first markers in the dictionary are used to fill the white chessboard squares. - * @return the output CharucoBoard object - * - * This functions creates a CharucoBoard object given the number of squares in each direction - * and the size of the markers and chessboard squares. */ - CV_WRAP static Ptr create(int squaresX, int squaresY, float squareLength, float markerLength, - const Dictionary &dictionary, InputArray ids = noArray()); + CV_WRAP CharucoBoard(const Size& size, float squareLength, float markerLength, + const Dictionary &dictionary, InputArray ids = noArray()); CV_WRAP Size getChessboardSize() const; CV_WRAP float getSquareLength() const; @@ -220,10 +167,8 @@ public: */ CV_WRAP bool checkCharucoCornersCollinear(InputArray charucoIds) const; -protected: - struct CharucoImpl; - friend struct CharucoImpl; - Ptr charucoImpl; + CV_DEPRECATED_EXTERNAL // avoid using in C++ code, will be moved to “protected” (need to fix bindings first) + CharucoBoard(); }; //! @} diff --git a/modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp b/modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp index 80f8433..c7338c3 100644 --- a/modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp +++ b/modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp @@ -10,28 +10,7 @@ namespace cv { namespace aruco { -/** @defgroup aruco ArUco Marker Detection - * Square fiducial markers (also known as Augmented Reality Markers) are useful for easy, - * fast and robust camera pose estimation. - * - * The main functionality of ArucoDetector class is detection of markers in an image. There are even more - * functionalities implemented in the aruco contrib module (files aruco.hpp, charuco.hpp, aruco_calib.hpp): - * - Pose estimation from a single marker or from a board/set of markers - * - Detection of ChArUco board for high subpixel accuracy - * - Camera calibration from both, ArUco boards and ChArUco boards. - * - Detection of ChArUco diamond markers - * The functionalities from the aruco contrib module is planned to be transferred to the main repository. - * - * The implementation is based on the ArUco Library by R. Muñoz-Salinas and S. Garrido-Jurado @cite Aruco2014. - * - * Markers can also be detected based on the AprilTag 2 @cite wang2016iros fiducial detection method. - * - * @sa @cite Aruco2014 - * This code has been originally developed by Sergio Garrido-Jurado as a project - * for Google Summer of Code 2015 (GSoC 15). - */ - -//! @addtogroup aruco +//! @addtogroup objdetect_aruco //! @{ enum CornerRefineMethod{ @@ -294,7 +273,7 @@ public: * @sa undistort, estimatePoseSingleMarkers, estimatePoseBoard */ CV_WRAP void detectMarkers(InputArray image, OutputArrayOfArrays corners, OutputArray ids, - OutputArrayOfArrays rejectedImgPoints = noArray()); + OutputArrayOfArrays rejectedImgPoints = noArray()) const; /** @brief Refind not detected markers based on the already detected and the board layout * @@ -318,11 +297,11 @@ public: * using projectPoint function. If not, missing marker projections are interpolated using global * homography, and all the marker corners in the board must have the same Z coordinate. */ - CV_WRAP void refineDetectedMarkers(InputArray image, const Ptr &board, + CV_WRAP void refineDetectedMarkers(InputArray image, const Board &board, InputOutputArrayOfArrays detectedCorners, InputOutputArray detectedIds, InputOutputArrayOfArrays rejectedCorners, InputArray cameraMatrix = noArray(), InputArray distCoeffs = noArray(), - OutputArray recoveredIdxs = noArray()); + OutputArray recoveredIdxs = noArray()) const; CV_WRAP const Dictionary& getDictionary() const; CV_WRAP void setDictionary(const Dictionary& dictionary); diff --git a/modules/objdetect/include/opencv2/objdetect/aruco_dictionary.hpp b/modules/objdetect/include/opencv2/objdetect/aruco_dictionary.hpp index 498bcf1..343d876 100644 --- a/modules/objdetect/include/opencv2/objdetect/aruco_dictionary.hpp +++ b/modules/objdetect/include/opencv2/objdetect/aruco_dictionary.hpp @@ -9,7 +9,7 @@ namespace cv { namespace aruco { -//! @addtogroup aruco +//! @addtogroup objdetect_aruco //! @{ diff --git a/modules/objdetect/include/opencv2/objdetect/charuco_detector.hpp b/modules/objdetect/include/opencv2/objdetect/charuco_detector.hpp new file mode 100644 index 0000000..f6ad677 --- /dev/null +++ b/modules/objdetect/include/opencv2/objdetect/charuco_detector.hpp @@ -0,0 +1,154 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html +#ifndef OPENCV_OBJDETECT_CHARUCO_DETECTOR_HPP +#define OPENCV_OBJDETECT_CHARUCO_DETECTOR_HPP + +#include "opencv2/objdetect/aruco_detector.hpp" + +namespace cv { +namespace aruco { + +//! @addtogroup objdetect_aruco +//! @{ + +struct CV_EXPORTS_W_SIMPLE CharucoParameters { + CharucoParameters() { + minMarkers = 2; + tryRefineMarkers = false; + } + /// cameraMatrix optional 3x3 floating-point camera matrix + CV_PROP_RW Mat cameraMatrix; + + /// distCoeffs optional vector of distortion coefficients + CV_PROP_RW Mat distCoeffs; + + /// minMarkers number of adjacent markers that must be detected to return a charuco corner, default = 2 + CV_PROP_RW int minMarkers; + + /// try to use refine board, default false + CV_PROP_RW bool tryRefineMarkers; +}; + +class CV_EXPORTS_W CharucoDetector : public Algorithm { +public: + /** @brief Basic CharucoDetector constructor + * + * @param board ChAruco board + * @param charucoParams charuco detection parameters + * @param detectorParams marker detection parameters + * @param refineParams marker refine detection parameters + */ + CV_WRAP CharucoDetector(const CharucoBoard& board, + const CharucoParameters& charucoParams = CharucoParameters(), + const DetectorParameters &detectorParams = DetectorParameters(), + const RefineParameters& refineParams = RefineParameters()); + + CV_WRAP const CharucoBoard& getBoard() const; + CV_WRAP void setBoard(const CharucoBoard& board); + + CV_WRAP const CharucoParameters& getCharucoParameters() const; + CV_WRAP void setCharucoParameters(CharucoParameters& charucoParameters); + + CV_WRAP const DetectorParameters& getDetectorParameters() const; + CV_WRAP void setDetectorParameters(const DetectorParameters& detectorParameters); + + CV_WRAP const RefineParameters& getRefineParameters() const; + CV_WRAP void setRefineParameters(const RefineParameters& refineParameters); + + /** + * @brief detect aruco markers and interpolate position of ChArUco board corners + * @param image input image necesary for corner refinement. Note that markers are not detected and + * should be sent in corners and ids parameters. + * @param charucoCorners interpolated chessboard corners. + * @param charucoIds interpolated chessboard corners identifiers. + * @param markerCorners vector of already detected markers corners. For each marker, its four + * corners are provided, (e.g std::vector > ). For N detected markers, the + * dimensions of this array should be Nx4. The order of the corners should be clockwise. + * If markerCorners and markerCorners are empty, the function detect aruco markers and ids. + * @param markerIds list of identifiers for each marker in corners. + * If markerCorners and markerCorners are empty, the function detect aruco markers and ids. + * + * This function receives the detected markers and returns the 2D position of the chessboard corners + * from a ChArUco board using the detected Aruco markers. + * + * If markerCorners and markerCorners are empty, the detectMarkers() will run and detect aruco markers and ids. + * + * If camera parameters are provided, the process is based in an approximated pose estimation, else it is based on local homography. + * Only visible corners are returned. For each corner, its corresponding identifier is also returned in charucoIds. + * @sa findChessboardCorners + */ + CV_WRAP void detectBoard(InputArray image, OutputArray charucoCorners, OutputArray charucoIds, + InputOutputArrayOfArrays markerCorners = noArray(), + InputOutputArray markerIds = noArray()) const; + + /** + * @brief Detect ChArUco Diamond markers + * + * @param image input image necessary for corner subpixel. + * @param diamondCorners output list of detected diamond corners (4 corners per diamond). The order + * is the same than in marker corners: top left, top right, bottom right and bottom left. Similar + * format than the corners returned by detectMarkers (e.g std::vector > ). + * @param diamondIds ids of the diamonds in diamondCorners. The id of each diamond is in fact of + * type Vec4i, so each diamond has 4 ids, which are the ids of the aruco markers composing the + * diamond. + * @param markerCorners list of detected marker corners from detectMarkers function. + * If markerCorners and markerCorners are empty, the function detect aruco markers and ids. + * @param markerIds list of marker ids in markerCorners. + * If markerCorners and markerCorners are empty, the function detect aruco markers and ids. + * + * This function detects Diamond markers from the previous detected ArUco markers. The diamonds + * are returned in the diamondCorners and diamondIds parameters. If camera calibration parameters + * are provided, the diamond search is based on reprojection. If not, diamond search is based on + * homography. Homography is faster than reprojection, but less accurate. + */ + CV_WRAP void detectDiamonds(InputArray image, OutputArrayOfArrays diamondCorners, OutputArray diamondIds, + InputOutputArrayOfArrays markerCorners = noArray(), + InputOutputArrayOfArrays markerIds = noArray()) const; +protected: + struct CharucoDetectorImpl; + Ptr charucoDetectorImpl; +}; + +/** + * @brief Draws a set of Charuco corners + * @param image input/output image. It must have 1 or 3 channels. The number of channels is not + * altered. + * @param charucoCorners vector of detected charuco corners + * @param charucoIds list of identifiers for each corner in charucoCorners + * @param cornerColor color of the square surrounding each corner + * + * This function draws a set of detected Charuco corners. If identifiers vector is provided, it also + * draws the id of each corner. + */ +CV_EXPORTS_W void drawDetectedCornersCharuco(InputOutputArray image, InputArray charucoCorners, + InputArray charucoIds = noArray(), Scalar cornerColor = Scalar(255, 0, 0)); + +/** + * @brief Draw a set of detected ChArUco Diamond markers + * + * @param image input/output image. It must have 1 or 3 channels. The number of channels is not + * altered. + * @param diamondCorners positions of diamond corners in the same format returned by + * detectCharucoDiamond(). (e.g std::vector > ). For N detected markers, + * the dimensions of this array should be Nx4. The order of the corners should be clockwise. + * @param diamondIds vector of identifiers for diamonds in diamondCorners, in the same format + * returned by detectCharucoDiamond() (e.g. std::vector). + * Optional, if not provided, ids are not painted. + * @param borderColor color of marker borders. Rest of colors (text color and first corner color) + * are calculated based on this one. + * + * Given an array of detected diamonds, this functions draws them in the image. The marker borders + * are painted and the markers identifiers if provided. + * Useful for debugging purposes. + */ +CV_EXPORTS_W void drawDetectedDiamonds(InputOutputArray image, InputArrayOfArrays diamondCorners, + InputArray diamondIds = noArray(), + Scalar borderColor = Scalar(0, 0, 255)); + +//! @} + +} +} + +#endif diff --git a/modules/objdetect/misc/java/test/ArucoTest.java b/modules/objdetect/misc/java/test/ArucoTest.java index 685f784..9321412 100644 --- a/modules/objdetect/misc/java/test/ArucoTest.java +++ b/modules/objdetect/misc/java/test/ArucoTest.java @@ -4,8 +4,10 @@ import java.util.ArrayList; import java.util.List; import org.opencv.test.OpenCVTestCase; +import org.junit.Assert; import org.opencv.core.Scalar; import org.opencv.core.Mat; +import org.opencv.core.MatOfInt; import org.opencv.core.Size; import org.opencv.core.CvType; import org.opencv.objdetect.*; @@ -29,7 +31,7 @@ public class ArucoTest extends OpenCVTestCase { Mat ids = new Mat(1, 1, CvType.CV_32SC1); ids.put(row, col, 0); - Board board = Board.create(objPoints, dictionary, ids); + Board board = new Board(objPoints, dictionary, ids); Mat image = new Mat(); board.generateImage(new Size(80, 80), image, 2); @@ -80,4 +82,32 @@ public class ArucoTest extends OpenCVTestCase { assertArrayEquals(new double[]{size + offset - 1, size + offset - 1}, res.get(0, 2), 0.0); assertArrayEquals(new double[]{offset, size + offset - 1}, res.get(0, 3), 0.0); } + + public void testCharucoDetector() { + Dictionary dictionary = Objdetect.getPredefinedDictionary(0); + int boardSizeX = 3, boardSizeY = 3; + CharucoBoard board = new CharucoBoard(new Size(boardSizeX, boardSizeY), 1.f, 0.8f, dictionary); + CharucoDetector charucoDetector = new CharucoDetector(board); + + int cellSize = 80; + Mat boardImage = new Mat(); + board.generateImage(new Size(cellSize*boardSizeX, cellSize*boardSizeY), boardImage); + + assertTrue(boardImage.total() > 0); + + Mat charucoCorners = new Mat(); + Mat charucoIds = new Mat(); + charucoDetector.detectBoard(boardImage, charucoCorners, charucoIds); + + assertEquals(4, charucoIds.total()); + int[] intCharucoIds = (new MatOfInt(charucoIds)).toArray(); + Assert.assertArrayEquals(new int[]{0, 1, 2, 3}, intCharucoIds); + + double eps = 0.2; + assertArrayEquals(new double[]{cellSize, cellSize}, charucoCorners.get(0, 0), eps); + assertArrayEquals(new double[]{2*cellSize, cellSize}, charucoCorners.get(1, 0), eps); + assertArrayEquals(new double[]{cellSize, 2*cellSize}, charucoCorners.get(2, 0), eps); + assertArrayEquals(new double[]{2*cellSize, 2*cellSize}, charucoCorners.get(3, 0), eps); + } + } diff --git a/modules/objdetect/misc/python/test/test_objdetect_aruco.py b/modules/objdetect/misc/python/test/test_objdetect_aruco.py index 318159b..97d4fcb 100644 --- a/modules/objdetect/misc/python/test/test_objdetect_aruco.py +++ b/modules/objdetect/misc/python/test/test_objdetect_aruco.py @@ -3,7 +3,7 @@ # Python 2/3 compatibility from __future__ import print_function -import os, numpy as np +import os, tempfile, numpy as np import cv2 as cv @@ -17,14 +17,14 @@ class aruco_objdetect_test(NewOpenCVTests): rev_ids = ids[::-1] aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_5X5_250) - board = cv.aruco.CharucoBoard_create(7, 5, 1, 0.5, aruco_dict) + board = cv.aruco.CharucoBoard((7, 5), 1, 0.5, aruco_dict) np.testing.assert_array_equal(board.getIds().squeeze(), ids) - board = cv.aruco.CharucoBoard_create(7, 5, 1, 0.5, aruco_dict, rev_ids) + board = cv.aruco.CharucoBoard((7, 5), 1, 0.5, aruco_dict, rev_ids) np.testing.assert_array_equal(board.getIds().squeeze(), rev_ids) - board = cv.aruco.CharucoBoard_create(7, 5, 1, 0.5, aruco_dict, ids) + board = cv.aruco.CharucoBoard((7, 5), 1, 0.5, aruco_dict, ids) np.testing.assert_array_equal(board.getIds().squeeze(), ids) def test_identify(self): @@ -72,7 +72,7 @@ class aruco_objdetect_test(NewOpenCVTests): aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_250) aruco_detector = cv.aruco.ArucoDetector(aruco_dict, aruco_params) board_size = (3, 4) - board = cv.aruco.GridBoard_create(board_size[0], board_size[1], 5.0, 1.0, aruco_dict) + board = cv.aruco.GridBoard(board_size, 5.0, 1.0, aruco_dict) board_image = board.generateImage((board_size[0]*50, board_size[1]*50), marginSize=10) corners, ids, rejected = aruco_detector.detectMarkers(board_image) @@ -90,5 +90,57 @@ class aruco_objdetect_test(NewOpenCVTests): self.assertEqual((1, 4, 2), refine_corners[0].shape) np.testing.assert_array_equal(corners, refine_corners) + def test_write_read_dictionary(self): + try: + aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_5X5_50) + markers_gold = aruco_dict.bytesList + + # write aruco_dict + fd, filename = tempfile.mkstemp(prefix="opencv_python_aruco_dict_", suffix=".yml") + os.close(fd) + + fs_write = cv.FileStorage(filename, cv.FileStorage_WRITE) + aruco_dict.writeDictionary(fs_write) + fs_write.release() + + # reset aruco_dict + aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_6X6_250) + + # read aruco_dict + fs_read = cv.FileStorage(filename, cv.FileStorage_READ) + aruco_dict.readDictionary(fs_read.root()) + fs_read.release() + + # check equal + self.assertEqual(aruco_dict.markerSize, 5) + self.assertEqual(aruco_dict.maxCorrectionBits, 3) + np.testing.assert_array_equal(aruco_dict.bytesList, markers_gold) + + finally: + if os.path.exists(filename): + os.remove(filename) + + def test_charuco_detector(self): + aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_250) + board_size = (3, 3) + board = cv.aruco.CharucoBoard(board_size, 1.0, .8, aruco_dict) + charuco_detector = cv.aruco.CharucoDetector(board) + cell_size = 100 + + image = board.generateImage((cell_size*board_size[0], cell_size*board_size[1])) + + list_gold_corners = [] + for i in range(1, board_size[0]): + for j in range(1, board_size[1]): + list_gold_corners.append((j*cell_size, i*cell_size)) + gold_corners = np.array(list_gold_corners, dtype=np.float32) + + charucoCorners, charucoIds, markerCorners, markerIds = charuco_detector.detectBoard(image) + + self.assertEqual(len(charucoIds), 4) + for i in range(0, 4): + self.assertEqual(charucoIds[i], i) + np.testing.assert_allclose(gold_corners, charucoCorners.reshape(-1, 2), 0.01, 0.1) + if __name__ == '__main__': NewOpenCVTests.bootstrap() diff --git a/modules/objdetect/perf/perf_aruco.cpp b/modules/objdetect/perf/perf_aruco.cpp new file mode 100644 index 0000000..d3f9141 --- /dev/null +++ b/modules/objdetect/perf/perf_aruco.cpp @@ -0,0 +1,285 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html +#include "perf_precomp.hpp" +#include "opencv2/calib3d.hpp" + +namespace opencv_test { +using namespace perf; + +typedef tuple UseArucoParams; +typedef TestBaseWithParam EstimateAruco; +#define ESTIMATE_PARAMS Combine(Values(false, true), Values(-1)) + +static double deg2rad(double deg) { return deg * CV_PI / 180.; } + +class MarkerPainter +{ +private: + int imgMarkerSize = 0; + Mat cameraMatrix; +public: + MarkerPainter(const int size) { + setImgMarkerSize(size); + } + + void setImgMarkerSize(const int size) { + imgMarkerSize = size; + cameraMatrix = Mat::eye(3, 3, CV_64FC1); + cameraMatrix.at(0, 0) = cameraMatrix.at(1, 1) = imgMarkerSize; + cameraMatrix.at(0, 2) = imgMarkerSize / 2.0; + cameraMatrix.at(1, 2) = imgMarkerSize / 2.0; + } + + static std::pair getSyntheticRT(double yaw, double pitch, double distance) { + auto rvec_tvec = std::make_pair(Mat(3, 1, CV_64FC1), Mat(3, 1, CV_64FC1)); + Mat& rvec = rvec_tvec.first; + Mat& tvec = rvec_tvec.second; + + // Rvec + // first put the Z axis aiming to -X (like the camera axis system) + Mat rotZ(3, 1, CV_64FC1); + rotZ.ptr(0)[0] = 0; + rotZ.ptr(0)[1] = 0; + rotZ.ptr(0)[2] = -0.5 * CV_PI; + + Mat rotX(3, 1, CV_64FC1); + rotX.ptr(0)[0] = 0.5 * CV_PI; + rotX.ptr(0)[1] = 0; + rotX.ptr(0)[2] = 0; + + Mat camRvec, camTvec; + composeRT(rotZ, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotX, Mat(3, 1, CV_64FC1, Scalar::all(0)), + camRvec, camTvec); + + // now pitch and yaw angles + Mat rotPitch(3, 1, CV_64FC1); + rotPitch.ptr(0)[0] = 0; + rotPitch.ptr(0)[1] = pitch; + rotPitch.ptr(0)[2] = 0; + + Mat rotYaw(3, 1, CV_64FC1); + rotYaw.ptr(0)[0] = yaw; + rotYaw.ptr(0)[1] = 0; + rotYaw.ptr(0)[2] = 0; + + composeRT(rotPitch, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotYaw, + Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec); + + // compose both rotations + composeRT(camRvec, Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, + Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec); + + // Tvec, just move in z (camera) direction the specific distance + tvec.ptr(0)[0] = 0.; + tvec.ptr(0)[1] = 0.; + tvec.ptr(0)[2] = distance; + return rvec_tvec; + } + + std::pair > getProjectMarker(int id, double yaw, double pitch, + const aruco::DetectorParameters& parameters, + const aruco::Dictionary& dictionary) { + auto marker_corners = std::make_pair(Mat(imgMarkerSize, imgMarkerSize, CV_8UC1, Scalar::all(255)), vector()); + Mat& img = marker_corners.first; + vector& corners = marker_corners.second; + + // canonical image + const int markerSizePixels = static_cast(imgMarkerSize/sqrt(2.f)); + aruco::generateImageMarker(dictionary, id, markerSizePixels, img, parameters.markerBorderBits); + + // get rvec and tvec for the perspective + const double distance = 0.1; + auto rvec_tvec = MarkerPainter::getSyntheticRT(yaw, pitch, distance); + Mat& rvec = rvec_tvec.first; + Mat& tvec = rvec_tvec.second; + + const float markerLength = 0.05f; + vector markerObjPoints; + markerObjPoints.emplace_back(Point3f(-markerLength / 2.f, +markerLength / 2.f, 0)); + markerObjPoints.emplace_back(markerObjPoints[0] + Point3f(markerLength, 0, 0)); + markerObjPoints.emplace_back(markerObjPoints[0] + Point3f(markerLength, -markerLength, 0)); + markerObjPoints.emplace_back(markerObjPoints[0] + Point3f(0, -markerLength, 0)); + + // project markers and draw them + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + projectPoints(markerObjPoints, rvec, tvec, cameraMatrix, distCoeffs, corners); + + vector originalCorners; + originalCorners.emplace_back(Point2f(0.f, 0.f)); + originalCorners.emplace_back(originalCorners[0]+Point2f((float)markerSizePixels, 0)); + originalCorners.emplace_back(originalCorners[0]+Point2f((float)markerSizePixels, (float)markerSizePixels)); + originalCorners.emplace_back(originalCorners[0]+Point2f(0, (float)markerSizePixels)); + + Mat transformation = getPerspectiveTransform(originalCorners, corners); + + warpPerspective(img, img, transformation, Size(imgMarkerSize, imgMarkerSize), INTER_NEAREST, BORDER_CONSTANT, + Scalar::all(255)); + return marker_corners; + } + + std::pair > > getProjectMarkersTile(const int numMarkers, + const aruco::DetectorParameters& params, + const aruco::Dictionary& dictionary) { + Mat tileImage(imgMarkerSize*numMarkers, imgMarkerSize*numMarkers, CV_8UC1, Scalar::all(255)); + map > idCorners; + + int iter = 0, pitch = 0, yaw = 0; + for (int i = 0; i < numMarkers; i++) { + for (int j = 0; j < numMarkers; j++) { + int currentId = iter; + auto marker_corners = getProjectMarker(currentId, deg2rad(70+yaw), deg2rad(pitch), params, dictionary); + Point2i startPoint(j*imgMarkerSize, i*imgMarkerSize); + Mat tmp_roi = tileImage(Rect(startPoint.x, startPoint.y, imgMarkerSize, imgMarkerSize)); + marker_corners.first.copyTo(tmp_roi); + + for (Point2f& point: marker_corners.second) + point += static_cast(startPoint); + idCorners[currentId] = marker_corners.second; + auto test = idCorners[currentId]; + yaw = (yaw + 10) % 51; // 70+yaw >= 70 && 70+yaw <= 120 + iter++; + } + pitch = (pitch + 60) % 360; + } + return std::make_pair(tileImage, idCorners); + } +}; + +static inline double getMaxDistance(map > &golds, const vector& ids, + const vector >& corners) { + std::map mapDist; + for (const auto& el : golds) + mapDist[el.first] = std::numeric_limits::max(); + for (size_t i = 0; i < ids.size(); i++) { + int id = ids[i]; + const auto gold_corners = golds.find(id); + if (gold_corners != golds.end()) { + double distance = 0.; + for (int c = 0; c < 4; c++) + distance = std::max(distance, cv::norm(gold_corners->second[c] - corners[i][c])); + mapDist[id] = distance; + } + } + return std::max_element(std::begin(mapDist), std::end(mapDist), + [](const pair& p1, const pair& p2){return p1.second < p2.second;})->second; +} + +PERF_TEST_P(EstimateAruco, ArucoFirst, ESTIMATE_PARAMS) { + UseArucoParams testParams = GetParam(); + aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250); + aruco::DetectorParameters detectorParams; + detectorParams.minDistanceToBorder = 1; + detectorParams.markerBorderBits = 1; + detectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + + const int markerSize = 100; + const int numMarkersInRow = 9; + //USE_ARUCO3 + detectorParams.useAruco3Detection = get<0>(testParams); + if (detectorParams.useAruco3Detection) { + detectorParams.minSideLengthCanonicalImg = 32; + detectorParams.minMarkerLengthRatioOriginalImg = 0.04f / numMarkersInRow; + } + aruco::ArucoDetector detector(dictionary, detectorParams); + MarkerPainter painter(markerSize); + auto image_map = painter.getProjectMarkersTile(numMarkersInRow, detectorParams, dictionary); + + // detect markers + vector > corners; + vector ids; + TEST_CYCLE() { + detector.detectMarkers(image_map.first, corners, ids); + } + ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast(ids.size())); + double maxDistance = getMaxDistance(image_map.second, ids, corners); + ASSERT_LT(maxDistance, 3.); + SANITY_CHECK_NOTHING(); +} + +PERF_TEST_P(EstimateAruco, ArucoSecond, ESTIMATE_PARAMS) { + UseArucoParams testParams = GetParam(); + aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250); + aruco::DetectorParameters detectorParams; + detectorParams.minDistanceToBorder = 1; + detectorParams.markerBorderBits = 1; + detectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + + //USE_ARUCO3 + detectorParams.useAruco3Detection = get<0>(testParams); + if (detectorParams.useAruco3Detection) { + detectorParams.minSideLengthCanonicalImg = 64; + detectorParams.minMarkerLengthRatioOriginalImg = 0.f; + } + aruco::ArucoDetector detector(dictionary, detectorParams); + const int markerSize = 200; + const int numMarkersInRow = 11; + MarkerPainter painter(markerSize); + auto image_map = painter.getProjectMarkersTile(numMarkersInRow, detectorParams, dictionary); + + // detect markers + vector > corners; + vector ids; + TEST_CYCLE() { + detector.detectMarkers(image_map.first, corners, ids); + } + ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast(ids.size())); + double maxDistance = getMaxDistance(image_map.second, ids, corners); + ASSERT_LT(maxDistance, 3.); + SANITY_CHECK_NOTHING(); +} + +struct Aruco3Params { + bool useAruco3Detection = false; + float minMarkerLengthRatioOriginalImg = 0.f; + int minSideLengthCanonicalImg = 0; + + Aruco3Params(bool useAruco3, float minMarkerLen, int minSideLen): useAruco3Detection(useAruco3), + minMarkerLengthRatioOriginalImg(minMarkerLen), + minSideLengthCanonicalImg(minSideLen) {} + friend std::ostream& operator<<(std::ostream& os, const Aruco3Params& d) { + os << d.useAruco3Detection << " " << d.minMarkerLengthRatioOriginalImg << " " << d.minSideLengthCanonicalImg; + return os; + } +}; +typedef tuple> ArucoTestParams; + +typedef TestBaseWithParam EstimateLargeAruco; +#define ESTIMATE_FHD_PARAMS Combine(Values(Aruco3Params(false, 0.f, 0), Aruco3Params(true, 0.f, 32), \ +Aruco3Params(true, 0.015f, 32), Aruco3Params(true, 0.f, 16), Aruco3Params(true, 0.0069f, 16)), \ +Values(std::make_pair(1440, 1), std::make_pair(480, 3), std::make_pair(144, 10))) + +PERF_TEST_P(EstimateLargeAruco, ArucoFHD, ESTIMATE_FHD_PARAMS) { + ArucoTestParams testParams = GetParam(); + aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250); + aruco::DetectorParameters detectorParams; + detectorParams.minDistanceToBorder = 1; + detectorParams.markerBorderBits = 1; + detectorParams.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + + //USE_ARUCO3 + detectorParams.useAruco3Detection = get<0>(testParams).useAruco3Detection; + if (detectorParams.useAruco3Detection) { + detectorParams.minSideLengthCanonicalImg = get<0>(testParams).minSideLengthCanonicalImg; + detectorParams.minMarkerLengthRatioOriginalImg = get<0>(testParams).minMarkerLengthRatioOriginalImg; + } + aruco::ArucoDetector detector(dictionary, detectorParams); + const int markerSize = get<1>(testParams).first; // 1440 or 480 or 144 + const int numMarkersInRow = get<1>(testParams).second; // 1 or 3 or 144 + MarkerPainter painter(markerSize); // num pixels is 1440x1440 as in FHD 1920x1080 + auto image_map = painter.getProjectMarkersTile(numMarkersInRow, detectorParams, dictionary); + + // detect markers + vector > corners; + vector ids; + TEST_CYCLE() + { + detector.detectMarkers(image_map.first, corners, ids); + } + ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast(ids.size())); + double maxDistance = getMaxDistance(image_map.second, ids, corners); + ASSERT_LT(maxDistance, 3.); + SANITY_CHECK_NOTHING(); +} + +} diff --git a/modules/objdetect/src/aruco/aruco_board.cpp b/modules/objdetect/src/aruco/aruco_board.cpp index 23bf4cf..370d50d 100644 --- a/modules/objdetect/src/aruco/aruco_board.cpp +++ b/modules/objdetect/src/aruco/aruco_board.cpp @@ -3,6 +3,8 @@ // of this distribution and at http://opencv.org/license.html #include "../precomp.hpp" +#include "opencv2/objdetect/aruco_board.hpp" + #include #include @@ -10,72 +12,60 @@ namespace cv { namespace aruco { using namespace std; -struct Board::BoardImpl { - std::vector > objPoints; +struct Board::Impl { Dictionary dictionary; - Point3f rightBottomBorder; std::vector ids; + std::vector > objPoints; + Point3f rightBottomBorder; - BoardImpl() { - dictionary = Dictionary(getPredefinedDictionary(PredefinedDictionaryType::DICT_4X4_50)); - } -}; + explicit Impl(const Dictionary& _dictionary): + dictionary(_dictionary) + {} -Board::Board(): boardImpl(makePtr()) {} + virtual ~Impl() {} -Board::~Board() {} + Impl(const Impl&) = delete; + Impl& operator=(const Impl&) = delete; -Ptr Board::create(InputArrayOfArrays objPoints, const Dictionary &dictionary, InputArray ids) { - CV_Assert(objPoints.total() == ids.total()); - CV_Assert(objPoints.type() == CV_32FC3 || objPoints.type() == CV_32FC1); + virtual void matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray _objPoints, + OutputArray imgPoints) const; - vector > obj_points_vector; - Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f); - for (unsigned int i = 0; i < objPoints.total(); i++) { - vector corners; - Mat corners_mat = objPoints.getMat(i); + virtual void generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const; +}; - if (corners_mat.type() == CV_32FC1) - corners_mat = corners_mat.reshape(3); - CV_Assert(corners_mat.total() == 4); +void Board::Impl::matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray _objPoints, + OutputArray imgPoints) const { - for (int j = 0; j < 4; j++) { - const Point3f &corner = corners_mat.at(j); - corners.push_back(corner); - rightBottomBorder.x = std::max(rightBottomBorder.x, corner.x); - rightBottomBorder.y = std::max(rightBottomBorder.y, corner.y); - rightBottomBorder.z = std::max(rightBottomBorder.z, corner.z); - } - obj_points_vector.push_back(corners); - } - Board board; - Ptr res = makePtr(board); - ids.copyTo(res->boardImpl->ids); - res->boardImpl->objPoints = obj_points_vector; - res->boardImpl->dictionary = dictionary; - res->boardImpl->rightBottomBorder = rightBottomBorder; - return res; -} + CV_Assert(ids.size() == objPoints.size()); + CV_Assert(detectedIds.total() == detectedCorners.total()); -const Dictionary& Board::getDictionary() const { - return this->boardImpl->dictionary; -} + size_t nDetectedMarkers = detectedIds.total(); -const vector >& Board::getObjPoints() const { - return this->boardImpl->objPoints; -} + vector objPnts; + objPnts.reserve(nDetectedMarkers); -const Point3f& Board::getRightBottomCorner() const { - return this->boardImpl->rightBottomBorder; -} + vector imgPnts; + imgPnts.reserve(nDetectedMarkers); -const vector& Board::getIds() const { - return this->boardImpl->ids; + // look for detected markers that belong to the board and get their information + for(unsigned int i = 0; i < nDetectedMarkers; i++) { + int currentId = detectedIds.getMat().ptr< int >(0)[i]; + for(unsigned int j = 0; j < ids.size(); j++) { + if(currentId == ids[j]) { + for(int p = 0; p < 4; p++) { + objPnts.push_back(objPoints[j][p]); + imgPnts.push_back(detectedCorners.getMat(i).ptr(0)[p]); + } + } + } + } + + // create output + Mat(objPnts).copyTo(_objPoints); + Mat(imgPnts).copyTo(imgPoints); } -/** @brief Implementation of draw planar board that accepts a raw Board pointer. - */ -void Board::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const { +void Board::Impl::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const { CV_Assert(!outSize.empty()); CV_Assert(marginSize >= 0); @@ -85,17 +75,17 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize); // calculate max and min values in XY plane - CV_Assert(this->getObjPoints().size() > 0); + CV_Assert(objPoints.size() > 0); float minX, maxX, minY, maxY; - minX = maxX = this->getObjPoints()[0][0].x; - minY = maxY = this->getObjPoints()[0][0].y; + minX = maxX = objPoints[0][0].x; + minY = maxY = objPoints[0][0].y; - for(unsigned int i = 0; i < this->getObjPoints().size(); i++) { + for(unsigned int i = 0; i < objPoints.size(); i++) { for(int j = 0; j < 4; j++) { - minX = min(minX, this->getObjPoints()[i][j].x); - maxX = max(maxX, this->getObjPoints()[i][j].x); - minY = min(minY, this->getObjPoints()[i][j].y); - maxY = max(maxY, this->getObjPoints()[i][j].y); + minX = min(minX, objPoints[i][j].x); + maxX = max(maxX, objPoints[i][j].x); + minY = min(minY, objPoints[i][j].y); + maxY = max(maxY, objPoints[i][j].y); } } @@ -121,10 +111,10 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor Mat marker; Point2f outCorners[3]; Point2f inCorners[3]; - for(unsigned int m = 0; m < this->getObjPoints().size(); m++) { + for(unsigned int m = 0; m < objPoints.size(); m++) { // transform corners to markerZone coordinates for(int j = 0; j < 3; j++) { - Point2f pf = Point2f(this->getObjPoints()[m][j].x, this->getObjPoints()[m][j].y); + Point2f pf = Point2f(objPoints[m][j].x, objPoints[m][j].y); // move top left to 0, 0 pf -= Point2f(minX, minY); pf.x = pf.x / sizeX * float(out.cols); @@ -135,7 +125,7 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor // get marker Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square - getDictionary().generateImageMarker(this->getIds()[m], dst_sz.width, marker, borderBits); + dictionary.generateImageMarker(ids[m], dst_sz.width, marker, borderBits); if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) { // marker is aligned to image axes @@ -155,70 +145,119 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor } } -void Board::matchImagePoints(InputArray detectedCorners, InputArray detectedIds, - OutputArray _objPoints, OutputArray imgPoints) const { - CV_Assert(getIds().size() == getObjPoints().size()); - CV_Assert(detectedIds.total() == detectedCorners.total()); +Board::Board(const Ptr& _impl): + impl(_impl) +{ + CV_Assert(impl); +} - size_t nDetectedMarkers = detectedIds.total(); +Board::Board(): + impl(nullptr) +{} - vector objPnts; - objPnts.reserve(nDetectedMarkers); +Board::Board(InputArrayOfArrays objPoints, const Dictionary &dictionary, InputArray ids): + Board(new Board::Impl(dictionary)) { + CV_Assert(ids.size() == objPoints.size()); + CV_Assert(objPoints.total() == ids.total()); + CV_Assert(objPoints.type() == CV_32FC3 || objPoints.type() == CV_32FC1); - vector imgPnts; - imgPnts.reserve(nDetectedMarkers); + vector > obj_points_vector; + Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f); + for (unsigned int i = 0; i < objPoints.total(); i++) { + vector corners; + Mat corners_mat = objPoints.getMat(i); - // look for detected markers that belong to the board and get their information - for(unsigned int i = 0; i < nDetectedMarkers; i++) { - int currentId = detectedIds.getMat().ptr< int >(0)[i]; - for(unsigned int j = 0; j < getIds().size(); j++) { - if(currentId == getIds()[j]) { - for(int p = 0; p < 4; p++) { - objPnts.push_back(getObjPoints()[j][p]); - imgPnts.push_back(detectedCorners.getMat(i).ptr(0)[p]); - } - } + if (corners_mat.type() == CV_32FC1) + corners_mat = corners_mat.reshape(3); + CV_Assert(corners_mat.total() == 4); + + for (int j = 0; j < 4; j++) { + const Point3f &corner = corners_mat.at(j); + corners.push_back(corner); + rightBottomBorder.x = std::max(rightBottomBorder.x, corner.x); + rightBottomBorder.y = std::max(rightBottomBorder.y, corner.y); + rightBottomBorder.z = std::max(rightBottomBorder.z, corner.z); } + obj_points_vector.push_back(corners); } - // create output - Mat(objPnts).copyTo(_objPoints); - Mat(imgPnts).copyTo(imgPoints); + ids.copyTo(impl->ids); + impl->objPoints = obj_points_vector; + impl->rightBottomBorder = rightBottomBorder; } -struct GridBoard::GridImpl { - GridImpl(){}; - // number of markers in X and Y directions - int sizeX = 3, sizeY = 3; +const Dictionary& Board::getDictionary() const { + CV_Assert(this->impl); + return this->impl->dictionary; +} - // marker side length (normally in meters) - float markerLength = 1.f; +const vector >& Board::getObjPoints() const { + CV_Assert(this->impl); + return this->impl->objPoints; +} +const Point3f& Board::getRightBottomCorner() const { + CV_Assert(this->impl); + return this->impl->rightBottomBorder; +} + +const vector& Board::getIds() const { + CV_Assert(this->impl); + return this->impl->ids; +} + +/** @brief Implementation of draw planar board that accepts a raw Board pointer. + */ +void Board::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const { + CV_Assert(this->impl); + impl->generateImage(outSize, img, marginSize, borderBits); +} + +void Board::matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray objPoints, + OutputArray imgPoints) const { + CV_Assert(this->impl); + impl->matchImagePoints(detectedCorners, detectedIds, objPoints, imgPoints); +} + +struct GridBoardImpl : public Board::Impl { + GridBoardImpl(const Dictionary& _dictionary, const Size& _size, float _markerLength, float _markerSeparation): + Board::Impl(_dictionary), + size(_size), + markerLength(_markerLength), + markerSeparation(_markerSeparation) + { + CV_Assert(size.width*size.height > 0 && markerLength > 0 && markerSeparation > 0); + } + + // number of markers in X and Y directions + const Size size; + // marker side length (normally in meters) + float markerLength; // separation between markers in the grid - float markerSeparation = .5f; + float markerSeparation; }; -GridBoard::GridBoard(): gridImpl(makePtr()) {} - -Ptr GridBoard::create(int markersX, int markersY, float markerLength, float markerSeparation, - const Dictionary &dictionary, InputArray ids) { - CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparation > 0); - GridBoard board; - Ptr res = makePtr(board); - res->gridImpl->sizeX = markersX; - res->gridImpl->sizeY = markersY; - res->gridImpl->markerLength = markerLength; - res->gridImpl->markerSeparation = markerSeparation; - res->boardImpl->dictionary = dictionary; - - size_t totalMarkers = (size_t) markersX * markersY; - CV_Assert(totalMarkers == ids.total()); +GridBoard::GridBoard() {} + +GridBoard::GridBoard(const Size& size, float markerLength, float markerSeparation, + const Dictionary &dictionary, InputArray ids): + Board(new GridBoardImpl(dictionary, size, markerLength, markerSeparation)) { + + size_t totalMarkers = (size_t) size.width*size.height; + CV_Assert(ids.empty() || totalMarkers == ids.total()); vector > objPoints; objPoints.reserve(totalMarkers); - ids.copyTo(res->boardImpl->ids); + + if(!ids.empty()) { + ids.copyTo(impl->ids); + } else { + impl->ids = std::vector(totalMarkers); + std::iota(impl->ids.begin(), impl->ids.end(), 0); + } + // calculate Board objPoints - for (int y = 0; y < markersY; y++) { - for (int x = 0; x < markersX; x++) { + for (int y = 0; y < size.height; y++) { + for (int x = 0; x < size.width; x++) { vector corners(4); corners[0] = Point3f(x * (markerLength + markerSeparation), y * (markerLength + markerSeparation), 0); @@ -228,67 +267,141 @@ Ptr GridBoard::create(int markersX, int markersY, float markerLength, objPoints.push_back(corners); } } - res->boardImpl->objPoints = objPoints; - res->boardImpl->rightBottomBorder = Point3f(markersX * markerLength + markerSeparation * (markersX - 1), - markersY * markerLength + markerSeparation * (markersY - 1), 0.f); - return res; -} - -Ptr GridBoard::create(int markersX, int markersY, float markerLength, float markerSeparation, - const Dictionary &dictionary, int firstMarker) { - vector ids(markersX*markersY); - std::iota(ids.begin(), ids.end(), firstMarker); - return GridBoard::create(markersX, markersY, markerLength, markerSeparation, dictionary, ids); -} - -void GridBoard::generateImage(Size outSize, OutputArray _img, int marginSize, int borderBits) const { - Board::generateImage(outSize, _img, marginSize, borderBits); + impl->objPoints = objPoints; + impl->rightBottomBorder = Point3f(size.width * markerLength + markerSeparation * (size.width - 1), + size.height * markerLength + markerSeparation * (size.height - 1), 0.f); } Size GridBoard::getGridSize() const { - return Size(gridImpl->sizeX, gridImpl->sizeY); + CV_Assert(impl); + return static_pointer_cast(impl)->size; } float GridBoard::getMarkerLength() const { - return gridImpl->markerLength; + CV_Assert(impl); + return static_pointer_cast(impl)->markerLength; } float GridBoard::getMarkerSeparation() const { - return gridImpl->markerSeparation; + CV_Assert(impl); + return static_pointer_cast(impl)->markerSeparation; } -struct CharucoBoard::CharucoImpl : GridBoard::GridImpl { - // size of chessboard squares side (normally in meters) +struct CharucoBoardImpl : Board::Impl { + CharucoBoardImpl(const Dictionary& _dictionary, const Size& _size, float _squareLength, float _markerLength): + Board::Impl(_dictionary), + size(_size), + squareLength(_squareLength), + markerLength(_markerLength) + {} + + // chessboard size + Size size; + + // Physical size of chessboard squares side (normally in meters) float squareLength; - // marker side length (normally in meters) + // Physical marker side length (normally in meters) float markerLength; - static void _getNearestMarkerCorners(CharucoBoard &board, float squareLength); - // vector of chessboard 3D corners precalculated std::vector chessboardCorners; // for each charuco corner, nearest marker id and nearest marker corner id of each marker std::vector > nearestMarkerIdx; std::vector > nearestMarkerCorners; + + void calcNearestMarkerCorners(); + + void matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds, + OutputArray objPoints, OutputArray imgPoints) const override; + + void generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const override; }; -CharucoBoard::CharucoBoard(): charucoImpl(makePtr()) {} +/** Fill nearestMarkerIdx and nearestMarkerCorners arrays */ +void CharucoBoardImpl::calcNearestMarkerCorners() { + nearestMarkerIdx.resize(chessboardCorners.size()); + nearestMarkerCorners.resize(chessboardCorners.size()); + unsigned int nMarkers = (unsigned int)objPoints.size(); + unsigned int nCharucoCorners = (unsigned int)chessboardCorners.size(); + for(unsigned int i = 0; i < nCharucoCorners; i++) { + double minDist = -1; // distance of closest markers + Point3f charucoCorner = chessboardCorners[i]; + for(unsigned int j = 0; j < nMarkers; j++) { + // calculate distance from marker center to charuco corner + Point3f center = Point3f(0, 0, 0); + for(unsigned int k = 0; k < 4; k++) + center += objPoints[j][k]; + center /= 4.; + double sqDistance; + Point3f distVector = charucoCorner - center; + sqDistance = distVector.x * distVector.x + distVector.y * distVector.y; + if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * squareLength, 2)) { + // if same minimum distance (or first iteration), add to nearestMarkerIdx vector + nearestMarkerIdx[i].push_back(j); + minDist = sqDistance; + } else if(sqDistance < minDist) { + // if finding a closest marker to the charuco corner + nearestMarkerIdx[i].clear(); // remove any previous added marker + nearestMarkerIdx[i].push_back(j); // add the new closest marker index + minDist = sqDistance; + } + } + // for each of the closest markers, search the marker corner index closer + // to the charuco corner + for(unsigned int j = 0; j < nearestMarkerIdx[i].size(); j++) { + nearestMarkerCorners[i].resize(nearestMarkerIdx[i].size()); + double minDistCorner = -1; + for(unsigned int k = 0; k < 4; k++) { + double sqDistance; + Point3f distVector = charucoCorner - objPoints[nearestMarkerIdx[i][j]][k]; + sqDistance = distVector.x * distVector.x + distVector.y * distVector.y; + if(k == 0 || sqDistance < minDistCorner) { + // if this corner is closer to the charuco corner, assing its index + // to nearestMarkerCorners + minDistCorner = sqDistance; + nearestMarkerCorners[i][j] = k; + } + } + } + } +} -void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize, int borderBits) const { +void CharucoBoardImpl::matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds, + OutputArray _objPoints, OutputArray imgPoints) const { + if (detectedCorners.kind() == _InputArray::STD_VECTOR_VECTOR || + detectedCorners.isMatVector() || detectedCorners.isUMatVector()) + Board::Impl::matchImagePoints(detectedCorners, detectedIds, _objPoints, imgPoints); + else { + CV_Assert(detectedCorners.isMat() || detectedCorners.isVector()); + size_t nDetected = detectedCorners.total(); + vector objPnts(nDetected); + vector imgPnts(nDetected); + for(size_t i = 0ull; i < nDetected; i++) { + int pointId = detectedIds.getMat().at((int)i); + CV_Assert(pointId >= 0 && pointId < (int)chessboardCorners.size()); + objPnts[i] = chessboardCorners[pointId]; + imgPnts[i] = detectedCorners.getMat().at((int)i); + } + Mat(objPnts).copyTo(_objPoints); + Mat(imgPnts).copyTo(imgPoints); + } +} + +void CharucoBoardImpl::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const { CV_Assert(!outSize.empty()); CV_Assert(marginSize >= 0); - _img.create(outSize, CV_8UC1); - _img.setTo(255); - Mat out = _img.getMat(); + img.create(outSize, CV_8UC1); + img.setTo(255); + Mat out = img.getMat(); Mat noMarginsImg = out.colRange(marginSize, out.cols - marginSize).rowRange(marginSize, out.rows - marginSize); double totalLengthX, totalLengthY; - totalLengthX = charucoImpl->squareLength * charucoImpl->sizeX; - totalLengthY = charucoImpl->squareLength * charucoImpl->sizeY; + totalLengthX = squareLength * size.width; + totalLengthY = squareLength * size.height; // proportional transformation double xReduction = totalLengthX / double(noMarginsImg.cols); @@ -308,21 +421,21 @@ void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize, // determine the margins to draw only the markers // take the minimum just to be sure - double squareSizePixels = min(double(chessboardZoneImg.cols) / double(charucoImpl->sizeX), - double(chessboardZoneImg.rows) / double(charucoImpl->sizeY)); + double squareSizePixels = min(double(chessboardZoneImg.cols) / double(size.width), + double(chessboardZoneImg.rows) / double(size.height)); - double diffSquareMarkerLength = (charucoImpl->squareLength - charucoImpl->markerLength) / 2; + double diffSquareMarkerLength = (squareLength - markerLength) / 2; int diffSquareMarkerLengthPixels = - int(diffSquareMarkerLength * squareSizePixels / charucoImpl->squareLength); + int(diffSquareMarkerLength * squareSizePixels / squareLength); // draw markers Mat markersImg; - Board::generateImage(chessboardZoneImg.size(), markersImg, diffSquareMarkerLengthPixels, borderBits); + Board::Impl::generateImage(chessboardZoneImg.size(), markersImg, diffSquareMarkerLengthPixels, borderBits); markersImg.copyTo(chessboardZoneImg); // now draw black squares - for(int y = 0; y < charucoImpl->sizeY; y++) { - for(int x = 0; x < charucoImpl->sizeX; x++) { + for(int y = 0; y < size.height; y++) { + for(int x = 0; x < size.width; x++) { if(y % 2 != x % 2) continue; // white corner, dont do anything @@ -338,78 +451,22 @@ void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize, } } -/** - * Fill nearestMarkerIdx and nearestMarkerCorners arrays - */ -void CharucoBoard::CharucoImpl::_getNearestMarkerCorners(CharucoBoard &board, float squareLength) { - board.charucoImpl->nearestMarkerIdx.resize(board.charucoImpl->chessboardCorners.size()); - board.charucoImpl->nearestMarkerCorners.resize(board.charucoImpl->chessboardCorners.size()); +CharucoBoard::CharucoBoard(){} - unsigned int nMarkers = (unsigned int)board.getIds().size(); - unsigned int nCharucoCorners = (unsigned int)board.charucoImpl->chessboardCorners.size(); - for(unsigned int i = 0; i < nCharucoCorners; i++) { - double minDist = -1; // distance of closest markers - Point3f charucoCorner = board.charucoImpl->chessboardCorners[i]; - for(unsigned int j = 0; j < nMarkers; j++) { - // calculate distance from marker center to charuco corner - Point3f center = Point3f(0, 0, 0); - for(unsigned int k = 0; k < 4; k++) - center += board.getObjPoints()[j][k]; - center /= 4.; - double sqDistance; - Point3f distVector = charucoCorner - center; - sqDistance = distVector.x * distVector.x + distVector.y * distVector.y; - if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * squareLength, 2)) { - // if same minimum distance (or first iteration), add to nearestMarkerIdx vector - board.charucoImpl->nearestMarkerIdx[i].push_back(j); - minDist = sqDistance; - } else if(sqDistance < minDist) { - // if finding a closest marker to the charuco corner - board.charucoImpl->nearestMarkerIdx[i].clear(); // remove any previous added marker - board.charucoImpl->nearestMarkerIdx[i].push_back(j); // add the new closest marker index - minDist = sqDistance; - } - } - // for each of the closest markers, search the marker corner index closer - // to the charuco corner - for(unsigned int j = 0; j < board.charucoImpl->nearestMarkerIdx[i].size(); j++) { - board.charucoImpl->nearestMarkerCorners[i].resize(board.charucoImpl->nearestMarkerIdx[i].size()); - double minDistCorner = -1; - for(unsigned int k = 0; k < 4; k++) { - double sqDistance; - Point3f distVector = charucoCorner - board.getObjPoints()[board.charucoImpl->nearestMarkerIdx[i][j]][k]; - sqDistance = distVector.x * distVector.x + distVector.y * distVector.y; - if(k == 0 || sqDistance < minDistCorner) { - // if this corner is closer to the charuco corner, assing its index - // to nearestMarkerCorners - minDistCorner = sqDistance; - board.charucoImpl->nearestMarkerCorners[i][j] = k; - } - } - } - } -} +CharucoBoard::CharucoBoard(const Size& size, float squareLength, float markerLength, + const Dictionary &dictionary, InputArray ids): + Board(new CharucoBoardImpl(dictionary, size, squareLength, markerLength)) { -Ptr CharucoBoard::create(int squaresX, int squaresY, float squareLength, float markerLength, - const Dictionary &dictionary, InputArray ids) { - CV_Assert(squaresX > 1 && squaresY > 1 && markerLength > 0 && squareLength > markerLength); - CharucoBoard board; - Ptr res = makePtr(board); - - res->charucoImpl->sizeX = squaresX; - res->charucoImpl->sizeY = squaresY; - res->charucoImpl->squareLength = squareLength; - res->charucoImpl->markerLength = markerLength; - res->boardImpl->dictionary = dictionary; - vector > objPoints; + CV_Assert(size.width > 1 && size.height > 1 && markerLength > 0 && squareLength > markerLength); + vector > objPoints; float diffSquareMarkerLength = (squareLength - markerLength) / 2; int totalMarkers = (int)(ids.total()); - ids.copyTo(res->boardImpl->ids); + ids.copyTo(impl->ids); // calculate Board objPoints int nextId = 0; - for(int y = 0; y < squaresY; y++) { - for(int x = 0; x < squaresX; x++) { + for(int y = 0; y < size.height; y++) { + for(int x = 0; x < size.width; x++) { if(y % 2 == x % 2) continue; // black corner, no marker here @@ -422,48 +479,60 @@ Ptr CharucoBoard::create(int squaresX, int squaresY, float squareL objPoints.push_back(corners); // first ids in dictionary if (totalMarkers == 0) - res->boardImpl->ids.push_back(nextId); + impl->ids.push_back(nextId); nextId++; } } if (totalMarkers > 0 && nextId != totalMarkers) CV_Error(cv::Error::StsBadSize, "Size of ids must be equal to the number of markers: "+std::to_string(nextId)); - res->boardImpl->objPoints = objPoints; + impl->objPoints = objPoints; // now fill chessboardCorners - for(int y = 0; y < squaresY - 1; y++) { - for(int x = 0; x < squaresX - 1; x++) { + std::vector & c = static_pointer_cast(impl)->chessboardCorners; + for(int y = 0; y < size.height - 1; y++) { + for(int x = 0; x < size.width - 1; x++) { Point3f corner; corner.x = (x + 1) * squareLength; corner.y = (y + 1) * squareLength; corner.z = 0; - res->charucoImpl->chessboardCorners.push_back(corner); + c.push_back(corner); } } - res->boardImpl->rightBottomBorder = Point3f(squaresX * squareLength, squaresY * squareLength, 0.f); - CharucoBoard::CharucoImpl::_getNearestMarkerCorners(*res, res->charucoImpl->squareLength); - return res; + impl->rightBottomBorder = Point3f(size.width * squareLength, size.height * squareLength, 0.f); + static_pointer_cast(impl)->calcNearestMarkerCorners(); } -Size CharucoBoard::getChessboardSize() const { return Size(charucoImpl->sizeX, charucoImpl->sizeY); } +Size CharucoBoard::getChessboardSize() const { + CV_Assert(impl); + return static_pointer_cast(impl)->size; +} -float CharucoBoard::getSquareLength() const { return charucoImpl->squareLength; } +float CharucoBoard::getSquareLength() const { + CV_Assert(impl); + return static_pointer_cast(impl)->squareLength; +} -float CharucoBoard::getMarkerLength() const { return charucoImpl->markerLength; } +float CharucoBoard::getMarkerLength() const { + CV_Assert(impl); + return static_pointer_cast(impl)->markerLength; +} bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const { + CV_Assert(impl); + unsigned int nCharucoCorners = (unsigned int)charucoIds.getMat().total(); if (nCharucoCorners <= 2) return true; // only test if there are 3 or more corners - CV_Assert(charucoImpl->chessboardCorners.size() >= charucoIds.getMat().total()); + auto board = static_pointer_cast(impl); + CV_Assert(board->chessboardCorners.size() >= charucoIds.getMat().total()); - Vec point0(charucoImpl->chessboardCorners[charucoIds.getMat().at(0)].x, - charucoImpl->chessboardCorners[charucoIds.getMat().at(0)].y, 1); + Vec point0(board->chessboardCorners[charucoIds.getMat().at(0)].x, + board->chessboardCorners[charucoIds.getMat().at(0)].y, 1); - Vec point1(charucoImpl->chessboardCorners[charucoIds.getMat().at(1)].x, - charucoImpl->chessboardCorners[charucoIds.getMat().at(1)].y, 1); + Vec point1(board->chessboardCorners[charucoIds.getMat().at(1)].x, + board->chessboardCorners[charucoIds.getMat().at(1)].y, 1); // create a line from the first two points. Vec testLine = point0.cross(point1); @@ -477,8 +546,8 @@ bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const { double dotProduct; for (unsigned int i = 2; i < nCharucoCorners; i++){ - testPoint(0) = charucoImpl->chessboardCorners[charucoIds.getMat().at(i)].x; - testPoint(1) = charucoImpl->chessboardCorners[charucoIds.getMat().at(i)].y; + testPoint(0) = board->chessboardCorners[charucoIds.getMat().at(i)].x; + testPoint(1) = board->chessboardCorners[charucoIds.getMat().at(i)].y; // if testPoint is on testLine, dotProduct will be zero (or very, very close) dotProduct = testPoint.dot(testLine); @@ -492,15 +561,18 @@ bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const { } std::vector CharucoBoard::getChessboardCorners() const { - return charucoImpl->chessboardCorners; + CV_Assert(impl); + return static_pointer_cast(impl)->chessboardCorners; } std::vector > CharucoBoard::getNearestMarkerIdx() const { - return charucoImpl->nearestMarkerIdx; + CV_Assert(impl); + return static_pointer_cast(impl)->nearestMarkerIdx; } std::vector > CharucoBoard::getNearestMarkerCorners() const { - return charucoImpl->nearestMarkerCorners; + CV_Assert(impl); + return static_pointer_cast(impl)->nearestMarkerCorners; } } diff --git a/modules/objdetect/src/aruco/aruco_detector.cpp b/modules/objdetect/src/aruco/aruco_detector.cpp index 4ef4710..bd9dcfb 100644 --- a/modules/objdetect/src/aruco/aruco_detector.cpp +++ b/modules/objdetect/src/aruco/aruco_detector.cpp @@ -856,7 +856,7 @@ ArucoDetector::ArucoDetector(const Dictionary &_dictionary, } void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corners, OutputArray _ids, - OutputArrayOfArrays _rejectedImgPoints) { + OutputArrayOfArrays _rejectedImgPoints) const { CV_Assert(!_image.empty()); DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams; const Dictionary& dictionary = arucoDetectorImpl->dictionary; @@ -994,37 +994,25 @@ void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corner /** * Project board markers that are not included in the list of detected markers */ -static void _projectUndetectedMarkers(const Ptr &_board, InputOutputArrayOfArrays _detectedCorners, - InputOutputArray _detectedIds, InputArray _cameraMatrix, InputArray _distCoeffs, - vector >& _undetectedMarkersProjectedCorners, - OutputArray _undetectedMarkersIds) { - // first estimate board pose with the current avaible markers - Mat rvec, tvec; - int boardDetectedMarkers = 0; - { - CV_Assert(_detectedCorners.total() == _detectedIds.total()); - // get object and image points for the solvePnP function - Mat detectedObjPoints, imgPoints; - _board->matchImagePoints(_detectedCorners, _detectedIds, detectedObjPoints, imgPoints); - CV_Assert(imgPoints.total() == detectedObjPoints.total()); - if(detectedObjPoints.total() > 0) // 0 of the detected markers in board - { - solvePnP(detectedObjPoints, imgPoints, _cameraMatrix, _distCoeffs, rvec, tvec); - // divide by four since all the four corners are concatenated in the array for each marker - boardDetectedMarkers = static_cast(detectedObjPoints.total()) / 4; - } - } - - // at least one marker from board so rvec and tvec are valid - if(boardDetectedMarkers == 0) return; +static inline void _projectUndetectedMarkers(const Board &board, InputOutputArrayOfArrays detectedCorners, + InputOutputArray detectedIds, InputArray cameraMatrix, InputArray distCoeffs, + vector >& undetectedMarkersProjectedCorners, + OutputArray undetectedMarkersIds) { + Mat rvec, tvec; // first estimate board pose with the current avaible markers + Mat objPoints, imgPoints; // object and image points for the solvePnP function + board.matchImagePoints(detectedCorners, detectedIds, objPoints, imgPoints); + if (objPoints.total() < 4ull) // at least one marker from board so rvec and tvec are valid + return; + solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec); // search undetected markers and project them using the previous pose vector > undetectedCorners; + const std::vector& ids = board.getIds(); vector undetectedIds; - for(unsigned int i = 0; i < _board->getIds().size(); i++) { + for(unsigned int i = 0; i < ids.size(); i++) { int foundIdx = -1; - for(unsigned int j = 0; j < _detectedIds.total(); j++) { - if(_board->getIds()[i] == _detectedIds.getMat().ptr()[j]) { + for(unsigned int j = 0; j < detectedIds.total(); j++) { + if(ids[i] == detectedIds.getMat().ptr()[j]) { foundIdx = j; break; } @@ -1033,31 +1021,31 @@ static void _projectUndetectedMarkers(const Ptr &_board, InputOutputArray // not detected if(foundIdx == -1) { undetectedCorners.push_back(vector()); - undetectedIds.push_back(_board->getIds()[i]); - projectPoints(_board->getObjPoints()[i], rvec, tvec, _cameraMatrix, _distCoeffs, + undetectedIds.push_back(ids[i]); + projectPoints(board.getObjPoints()[i], rvec, tvec, cameraMatrix, distCoeffs, undetectedCorners.back()); } } // parse output - Mat(undetectedIds).copyTo(_undetectedMarkersIds); - _undetectedMarkersProjectedCorners = undetectedCorners; + Mat(undetectedIds).copyTo(undetectedMarkersIds); + undetectedMarkersProjectedCorners = undetectedCorners; } /** * Interpolate board markers that are not included in the list of detected markers using * global homography */ -static void _projectUndetectedMarkers(const Ptr &_board, InputOutputArrayOfArrays _detectedCorners, +static void _projectUndetectedMarkers(const Board &_board, InputOutputArrayOfArrays _detectedCorners, InputOutputArray _detectedIds, vector >& _undetectedMarkersProjectedCorners, OutputArray _undetectedMarkersIds) { // check board points are in the same plane, if not, global homography cannot be applied - CV_Assert(_board->getObjPoints().size() > 0); - CV_Assert(_board->getObjPoints()[0].size() > 0); - float boardZ = _board->getObjPoints()[0][0].z; - for(unsigned int i = 0; i < _board->getObjPoints().size(); i++) { - for(unsigned int j = 0; j < _board->getObjPoints()[i].size(); j++) - CV_Assert(boardZ == _board->getObjPoints()[i][j].z); + CV_Assert(_board.getObjPoints().size() > 0); + CV_Assert(_board.getObjPoints()[0].size() > 0); + float boardZ = _board.getObjPoints()[0][0].z; + for(unsigned int i = 0; i < _board.getObjPoints().size(); i++) { + for(unsigned int j = 0; j < _board.getObjPoints()[i].size(); j++) + CV_Assert(boardZ == _board.getObjPoints()[i][j].z); } vector detectedMarkersObj2DAll; // Object coordinates (without Z) of all the detected @@ -1067,14 +1055,14 @@ static void _projectUndetectedMarkers(const Ptr &_board, InputOutputArray // missing markers in different vectors vector undetectedMarkersIds; // ids of missing markers // find markers included in board, and missing markers from board. Fill the previous vectors - for(unsigned int j = 0; j < _board->getIds().size(); j++) { + for(unsigned int j = 0; j < _board.getIds().size(); j++) { bool found = false; for(unsigned int i = 0; i < _detectedIds.total(); i++) { - if(_detectedIds.getMat().ptr()[i] == _board->getIds()[j]) { + if(_detectedIds.getMat().ptr()[i] == _board.getIds()[j]) { for(int c = 0; c < 4; c++) { imageCornersAll.push_back(_detectedCorners.getMat(i).ptr()[c]); detectedMarkersObj2DAll.push_back( - Point2f(_board->getObjPoints()[j][c].x, _board->getObjPoints()[j][c].y)); + Point2f(_board.getObjPoints()[j][c].x, _board.getObjPoints()[j][c].y)); } found = true; break; @@ -1084,9 +1072,9 @@ static void _projectUndetectedMarkers(const Ptr &_board, InputOutputArray undetectedMarkersObj2D.push_back(vector()); for(int c = 0; c < 4; c++) { undetectedMarkersObj2D.back().push_back( - Point2f(_board->getObjPoints()[j][c].x, _board->getObjPoints()[j][c].y)); + Point2f(_board.getObjPoints()[j][c].x, _board.getObjPoints()[j][c].y)); } - undetectedMarkersIds.push_back(_board->getIds()[j]); + undetectedMarkersIds.push_back(_board.getIds()[j]); } } if(imageCornersAll.size() == 0) return; @@ -1103,10 +1091,10 @@ static void _projectUndetectedMarkers(const Ptr &_board, InputOutputArray Mat(undetectedMarkersIds).copyTo(_undetectedMarkersIds); } -void ArucoDetector::refineDetectedMarkers(InputArray _image, const Ptr &_board, +void ArucoDetector::refineDetectedMarkers(InputArray _image, const Board& _board, InputOutputArrayOfArrays _detectedCorners, InputOutputArray _detectedIds, InputOutputArrayOfArrays _rejectedCorners, InputArray _cameraMatrix, - InputArray _distCoeffs, OutputArray _recoveredIdxs) { + InputArray _distCoeffs, OutputArray _recoveredIdxs) const { DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams; const Dictionary& dictionary = arucoDetectorImpl->dictionary; RefineParameters& refineParams = arucoDetectorImpl->refineParams; diff --git a/modules/objdetect/src/aruco/charuco_detector.cpp b/modules/objdetect/src/aruco/charuco_detector.cpp new file mode 100644 index 0000000..3955a8f --- /dev/null +++ b/modules/objdetect/src/aruco/charuco_detector.cpp @@ -0,0 +1,521 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +#include "../precomp.hpp" + +#include +#include "opencv2/objdetect/charuco_detector.hpp" +#include "aruco_utils.hpp" + +namespace cv { +namespace aruco { + +using namespace std; + +struct CharucoDetector::CharucoDetectorImpl { + CharucoBoard board; + CharucoParameters charucoParameters; + ArucoDetector arucoDetector; + + CharucoDetectorImpl(const CharucoBoard& _board, const CharucoParameters _charucoParameters, + const ArucoDetector& _arucoDetector): board(_board), charucoParameters(_charucoParameters), + arucoDetector(_arucoDetector) + {} + + /** Calculate the maximum window sizes for corner refinement for each charuco corner based on the distance + * to their closest markers */ + vector getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, InputArray markerIds, + InputArray charucoCorners) { + size_t nCharucoCorners = charucoCorners.getMat().total(); + + CV_Assert(board.getNearestMarkerIdx().size() == nCharucoCorners); + + vector winSizes(nCharucoCorners, Size(-1, -1)); + for(size_t i = 0ull; i < nCharucoCorners; i++) { + if(charucoCorners.getMat().at((int)i) == Point2f(-1.f, -1.f)) continue; + if(board.getNearestMarkerIdx()[i].empty()) continue; + double minDist = -1; + int counter = 0; + // calculate the distance to each of the closest corner of each closest marker + for(size_t j = 0; j < board.getNearestMarkerIdx()[i].size(); j++) { + // find marker + int markerId = board.getIds()[board.getNearestMarkerIdx()[i][j]]; + int markerIdx = -1; + for(size_t k = 0; k < markerIds.getMat().total(); k++) { + if(markerIds.getMat().at((int)k) == markerId) { + markerIdx = (int)k; + break; + } + } + if(markerIdx == -1) continue; + Point2f markerCorner = + markerCorners.getMat(markerIdx).at(board.getNearestMarkerCorners()[i][j]); + Point2f charucoCorner = charucoCorners.getMat().at((int)i); + double dist = norm(markerCorner - charucoCorner); + if(minDist == -1) minDist = dist; // if first distance, just assign it + minDist = min(dist, minDist); + counter++; + } + // if this is the first closest marker, dont do anything + if(counter == 0) + continue; + else { + // else, calculate the maximum window size + int winSizeInt = int(minDist - 2); // remove 2 pixels for safety + if(winSizeInt < 1) winSizeInt = 1; // minimum size is 1 + if(winSizeInt > 10) winSizeInt = 10; // maximum size is 10 + winSizes[i] = Size(winSizeInt, winSizeInt); + } + } + return winSizes; + } + + /** @brief From all projected chessboard corners, select those inside the image and apply subpixel refinement */ + void selectAndRefineChessboardCorners(InputArray allCorners, InputArray image, OutputArray selectedCorners, + OutputArray selectedIds, const vector &winSizes) { + const int minDistToBorder = 2; // minimum distance of the corner to the image border + // remaining corners, ids and window refinement sizes after removing corners outside the image + vector filteredChessboardImgPoints; + vector filteredWinSizes; + vector filteredIds; + // filter corners outside the image + Rect innerRect(minDistToBorder, minDistToBorder, image.getMat().cols - 2 * minDistToBorder, + image.getMat().rows - 2 * minDistToBorder); + for(unsigned int i = 0; i < allCorners.getMat().total(); i++) { + if(innerRect.contains(allCorners.getMat().at(i))) { + filteredChessboardImgPoints.push_back(allCorners.getMat().at(i)); + filteredIds.push_back(i); + filteredWinSizes.push_back(winSizes[i]); + } + } + // if none valid, return 0 + if(filteredChessboardImgPoints.empty()) return; + // corner refinement, first convert input image to grey + Mat grey; + if(image.type() == CV_8UC3) + cvtColor(image, grey, COLOR_BGR2GRAY); + else + grey = image.getMat(); + //// For each of the charuco corners, apply subpixel refinement using its correspondind winSize + parallel_for_(Range(0, (int)filteredChessboardImgPoints.size()), [&](const Range& range) { + const int begin = range.start; + const int end = range.end; + for (int i = begin; i < end; i++) { + vector in; + in.push_back(filteredChessboardImgPoints[i] - Point2f(0.5, 0.5)); // adjust sub-pixel coordinates for cornerSubPix + Size winSize = filteredWinSizes[i]; + if (winSize.height == -1 || winSize.width == -1) + winSize = Size(arucoDetector.getDetectorParameters().cornerRefinementWinSize, + arucoDetector.getDetectorParameters().cornerRefinementWinSize); + cornerSubPix(grey, in, winSize, Size(), + TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS, + arucoDetector.getDetectorParameters().cornerRefinementMaxIterations, + arucoDetector.getDetectorParameters().cornerRefinementMinAccuracy)); + filteredChessboardImgPoints[i] = in[0] + Point2f(0.5, 0.5); + } + }); + // parse output + Mat(filteredChessboardImgPoints).copyTo(selectedCorners); + Mat(filteredIds).copyTo(selectedIds); + } + + /** Interpolate charuco corners using approximated pose estimation */ + void interpolateCornersCharucoApproxCalib(InputArrayOfArrays markerCorners, InputArray markerIds, + InputArray image, OutputArray charucoCorners, OutputArray charucoIds) { + CV_Assert(image.getMat().channels() == 1 || image.getMat().channels() == 3); + CV_Assert(markerCorners.total() == markerIds.getMat().total()); + + // approximated pose estimation using marker corners + Mat approximatedRvec, approximatedTvec; + Mat objPoints, imgPoints; // object and image points for the solvePnP function + printf("before board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);\n"); + board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints); + printf("after board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);\n"); + if (objPoints.total() < 4ull) // need, at least, 4 corners + return; + + solvePnP(objPoints, imgPoints, charucoParameters.cameraMatrix, charucoParameters.distCoeffs, approximatedRvec, approximatedTvec); + printf("after solvePnP\n"); + + // project chessboard corners + vector allChessboardImgPoints; + projectPoints(board.getChessboardCorners(), approximatedRvec, approximatedTvec, charucoParameters.cameraMatrix, + charucoParameters.distCoeffs, allChessboardImgPoints); + printf("after projectPoints\n"); + // calculate maximum window sizes for subpixel refinement. The size is limited by the distance + // to the closes marker corner to avoid erroneous displacements to marker corners + vector subPixWinSizes = getMaximumSubPixWindowSizes(markerCorners, markerIds, allChessboardImgPoints); + // filter corners outside the image and subpixel-refine charuco corners + printf("before selectAndRefineChessboardCorners\n"); + selectAndRefineChessboardCorners(allChessboardImgPoints, image, charucoCorners, charucoIds, subPixWinSizes); + } + + /** Interpolate charuco corners using local homography */ + void interpolateCornersCharucoLocalHom(InputArrayOfArrays markerCorners, InputArray markerIds, InputArray image, + OutputArray charucoCorners, OutputArray charucoIds) { + CV_Assert(image.getMat().channels() == 1 || image.getMat().channels() == 3); + CV_Assert(markerCorners.total() == markerIds.getMat().total()); + size_t nMarkers = markerIds.getMat().total(); + // calculate local homographies for each marker + vector transformations(nMarkers); + vector validTransform(nMarkers, false); + const auto& ids = board.getIds(); + for(size_t i = 0ull; i < nMarkers; i++) { + vector markerObjPoints2D; + int markerId = markerIds.getMat().at((int)i); + auto it = find(ids.begin(), ids.end(), markerId); + if(it == ids.end()) continue; + auto boardIdx = it - ids.begin(); + markerObjPoints2D.resize(4ull); + for(size_t j = 0ull; j < 4ull; j++) + markerObjPoints2D[j] = + Point2f(board.getObjPoints()[boardIdx][j].x, board.getObjPoints()[boardIdx][j].y); + transformations[i] = getPerspectiveTransform(markerObjPoints2D, markerCorners.getMat((int)i)); + // set transform as valid if transformation is non-singular + double det = determinant(transformations[i]); + validTransform[i] = std::abs(det) > 1e-6; + } + size_t nCharucoCorners = (size_t)board.getChessboardCorners().size(); + vector allChessboardImgPoints(nCharucoCorners, Point2f(-1, -1)); + // for each charuco corner, calculate its interpolation position based on the closest markers + // homographies + for(size_t i = 0ull; i < nCharucoCorners; i++) { + Point2f objPoint2D = Point2f(board.getChessboardCorners()[i].x, board.getChessboardCorners()[i].y); + vector interpolatedPositions; + for(size_t j = 0ull; j < board.getNearestMarkerIdx()[i].size(); j++) { + int markerId = board.getIds()[board.getNearestMarkerIdx()[i][j]]; + int markerIdx = -1; + for(size_t k = 0ull; k < markerIds.getMat().total(); k++) { + if(markerIds.getMat().at((int)k) == markerId) { + markerIdx = (int)k; + break; + } + } + if (markerIdx != -1 && + validTransform[markerIdx]) + { + vector in, out; + in.push_back(objPoint2D); + perspectiveTransform(in, out, transformations[markerIdx]); + interpolatedPositions.push_back(out[0]); + } + } + // none of the closest markers detected + if(interpolatedPositions.empty()) continue; + // more than one closest marker detected, take middle point + if(interpolatedPositions.size() > 1ull) { + allChessboardImgPoints[i] = (interpolatedPositions[0] + interpolatedPositions[1]) / 2.; + } + // a single closest marker detected + else allChessboardImgPoints[i] = interpolatedPositions[0]; + } + // calculate maximum window sizes for subpixel refinement. The size is limited by the distance + // to the closes marker corner to avoid erroneous displacements to marker corners + vector subPixWinSizes = getMaximumSubPixWindowSizes(markerCorners, markerIds, allChessboardImgPoints); + // filter corners outside the image and subpixel-refine charuco corners + selectAndRefineChessboardCorners(allChessboardImgPoints, image, charucoCorners, charucoIds, subPixWinSizes); + } + + /** Remove charuco corners if any of their minMarkers closest markers has not been detected */ + int filterCornersWithoutMinMarkers(InputArray _allCharucoCorners, InputArray allCharucoIds, InputArray allArucoIds, + OutputArray _filteredCharucoCorners, OutputArray _filteredCharucoIds) { + CV_Assert(charucoParameters.minMarkers >= 0 && charucoParameters.minMarkers <= 2); + vector filteredCharucoCorners; + vector filteredCharucoIds; + // for each charuco corner + for(unsigned int i = 0; i < allCharucoIds.getMat().total(); i++) { + int currentCharucoId = allCharucoIds.getMat().at(i); + int totalMarkers = 0; // nomber of closest marker detected + // look for closest markers + for(unsigned int m = 0; m < board.getNearestMarkerIdx()[currentCharucoId].size(); m++) { + int markerId = board.getIds()[board.getNearestMarkerIdx()[currentCharucoId][m]]; + bool found = false; + for(unsigned int k = 0; k < allArucoIds.getMat().total(); k++) { + if(allArucoIds.getMat().at(k) == markerId) { + found = true; + break; + } + } + if(found) totalMarkers++; + } + // if enough markers detected, add the charuco corner to the final list + if(totalMarkers >= charucoParameters.minMarkers) { + filteredCharucoIds.push_back(currentCharucoId); + filteredCharucoCorners.push_back(_allCharucoCorners.getMat().at(i)); + } + } + // parse output + Mat(filteredCharucoCorners).copyTo(_filteredCharucoCorners); + Mat(filteredCharucoIds).copyTo(_filteredCharucoIds); + return (int)_filteredCharucoIds.total(); + } +}; + +CharucoDetector::CharucoDetector(const CharucoBoard &board, const CharucoParameters &charucoParams, + const DetectorParameters &detectorParams, const RefineParameters& refineParams) { + this->charucoDetectorImpl = makePtr(board, charucoParams, ArucoDetector(board.getDictionary(), detectorParams, refineParams)); +} + +const CharucoBoard& CharucoDetector::getBoard() const { + return charucoDetectorImpl->board; +} + +void CharucoDetector::setBoard(const CharucoBoard& board) { + this->charucoDetectorImpl->board = board; + charucoDetectorImpl->arucoDetector.setDictionary(board.getDictionary()); +} + +const CharucoParameters &CharucoDetector::getCharucoParameters() const { + return charucoDetectorImpl->charucoParameters; +} + +void CharucoDetector::setCharucoParameters(CharucoParameters &charucoParameters) { + charucoDetectorImpl->charucoParameters = charucoParameters; +} + +const DetectorParameters& CharucoDetector::getDetectorParameters() const { + return charucoDetectorImpl->arucoDetector.getDetectorParameters(); +} + +void CharucoDetector::setDetectorParameters(const DetectorParameters& detectorParameters) { + charucoDetectorImpl->arucoDetector.setDetectorParameters(detectorParameters); +} + +const RefineParameters& CharucoDetector::getRefineParameters() const { + return charucoDetectorImpl->arucoDetector.getRefineParameters(); +} + +void CharucoDetector::setRefineParameters(const RefineParameters& refineParameters) { + charucoDetectorImpl->arucoDetector.setRefineParameters(refineParameters); +} + +void CharucoDetector::detectBoard(InputArray image, OutputArray charucoCorners, OutputArray charucoIds, + InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds) const { + CV_Assert((markerCorners.empty() && markerIds.empty() && !image.empty()) || (markerCorners.size() == markerIds.size())); + vector> tmpMarkerCorners; + vector tmpMarkerIds; + InputOutputArrayOfArrays _markerCorners = markerCorners.needed() ? markerCorners : tmpMarkerCorners; + InputOutputArray _markerIds = markerIds.needed() ? markerIds : tmpMarkerIds; + + if (markerCorners.empty() && markerIds.empty()) { + vector > rejectedMarkers; + charucoDetectorImpl->arucoDetector.detectMarkers(image, _markerCorners, _markerIds, rejectedMarkers); + if (charucoDetectorImpl->charucoParameters.tryRefineMarkers) + charucoDetectorImpl->arucoDetector.refineDetectedMarkers(image, charucoDetectorImpl->board, _markerCorners, + _markerIds, rejectedMarkers); + } + // if camera parameters are avaible, use approximated calibration + if(!charucoDetectorImpl->charucoParameters.cameraMatrix.empty()) + charucoDetectorImpl->interpolateCornersCharucoApproxCalib(_markerCorners, _markerIds, image, charucoCorners, + charucoIds); + // else use local homography + else + charucoDetectorImpl->interpolateCornersCharucoLocalHom(_markerCorners, _markerIds, image, charucoCorners, + charucoIds); + // to return a charuco corner, its closest aruco markers should have been detected + charucoDetectorImpl->filterCornersWithoutMinMarkers(charucoCorners, charucoIds, _markerIds, charucoCorners, + charucoIds); +} + +void CharucoDetector::detectDiamonds(InputArray image, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds, + InputOutputArrayOfArrays inMarkerCorners, InputOutputArrayOfArrays inMarkerIds) const { + CV_Assert(getBoard().getChessboardSize() == Size(3, 3)); + CV_Assert((inMarkerCorners.empty() && inMarkerIds.empty() && !image.empty()) || (inMarkerCorners.size() == inMarkerIds.size())); + + vector> tmpMarkerCorners; + vector tmpMarkerIds; + InputOutputArrayOfArrays _markerCorners = inMarkerCorners.needed() ? inMarkerCorners : tmpMarkerCorners; + InputOutputArray _markerIds = inMarkerIds.needed() ? inMarkerIds : tmpMarkerIds; + if (_markerCorners.empty() && _markerIds.empty()) { + charucoDetectorImpl->arucoDetector.detectMarkers(image, _markerCorners, _markerIds); + } + + const float minRepDistanceRate = 1.302455f; + vector> diamondCorners; + vector diamondIds; + + // stores if the detected markers have been assigned or not to a diamond + vector assigned(_markerIds.total(), false); + if(_markerIds.total() < 4ull) return; // a diamond need at least 4 markers + + // convert input image to grey + Mat grey; + if(image.type() == CV_8UC3) + cvtColor(image, grey, COLOR_BGR2GRAY); + else + grey = image.getMat(); + auto board = getBoard(); + + // for each of the detected markers, try to find a diamond + for(unsigned int i = 0; i < (unsigned int)_markerIds.total(); i++) { + if(assigned[i]) continue; + + // calculate marker perimeter + float perimeterSq = 0; + Mat corners = _markerCorners.getMat(i); + for(int c = 0; c < 4; c++) { + Point2f edge = corners.at(c) - corners.at((c + 1) % 4); + perimeterSq += edge.x*edge.x + edge.y*edge.y; + } + // maximum reprojection error relative to perimeter + float minRepDistance = sqrt(perimeterSq) * minRepDistanceRate; + + int currentId = _markerIds.getMat().at(i); + + // prepare data to call refineDetectedMarkers() + // detected markers (only the current one) + vector currentMarker; + vector currentMarkerId; + currentMarker.push_back(_markerCorners.getMat(i)); + currentMarkerId.push_back(currentId); + + // marker candidates (the rest of markers if they have not been assigned) + vector candidates; + vector candidatesIdxs; + for(unsigned int k = 0; k < assigned.size(); k++) { + if(k == i) continue; + if(!assigned[k]) { + candidates.push_back(_markerCorners.getMat(k)); + candidatesIdxs.push_back(k); + } + } + if(candidates.size() < 3ull) break; // we need at least 3 free markers + // modify charuco layout id to make sure all the ids are different than current id + vector tmpIds(4ull); + for(int k = 1; k < 4; k++) + tmpIds[k] = currentId + 1 + k; + // current id is assigned to [0], so it is the marker on the top + tmpIds[0] = currentId; + + // create Charuco board layout for diamond (3x3 layout) + charucoDetectorImpl->board = CharucoBoard(Size(3, 3), board.getSquareLength(), + board.getMarkerLength(), board.getDictionary(), tmpIds); + + // try to find the rest of markers in the diamond + vector acceptedIdxs; + if (currentMarker.size() != 4ull) + { + RefineParameters refineParameters(minRepDistance, -1.f, false); + RefineParameters tmp = charucoDetectorImpl->arucoDetector.getRefineParameters(); + charucoDetectorImpl->arucoDetector.setRefineParameters(refineParameters); + charucoDetectorImpl->arucoDetector.refineDetectedMarkers(grey, getBoard(), currentMarker, currentMarkerId, + candidates, + noArray(), noArray(), acceptedIdxs); + charucoDetectorImpl->arucoDetector.setRefineParameters(tmp); + } + + // if found, we have a diamond + if(currentMarker.size() == 4ull) { + assigned[i] = true; + // calculate diamond id, acceptedIdxs array indicates the markers taken from candidates array + Vec4i markerId; + markerId[0] = currentId; + for(int k = 1; k < 4; k++) { + int currentMarkerIdx = candidatesIdxs[acceptedIdxs[k - 1]]; + markerId[k] = _markerIds.getMat().at(currentMarkerIdx); + assigned[currentMarkerIdx] = true; + } + + // interpolate the charuco corners of the diamond + vector currentMarkerCorners; + Mat aux; + detectBoard(grey, currentMarkerCorners, aux, currentMarker, currentMarkerId); + + // if everything is ok, save the diamond + if(currentMarkerCorners.size() > 0ull) { + // reorder corners + vector currentMarkerCornersReorder; + currentMarkerCornersReorder.resize(4); + currentMarkerCornersReorder[0] = currentMarkerCorners[0]; + currentMarkerCornersReorder[1] = currentMarkerCorners[1]; + currentMarkerCornersReorder[2] = currentMarkerCorners[3]; + currentMarkerCornersReorder[3] = currentMarkerCorners[2]; + + diamondCorners.push_back(currentMarkerCornersReorder); + diamondIds.push_back(markerId); + } + } + } + charucoDetectorImpl->board = board; + + if(diamondIds.size() > 0ull) { + // parse output + Mat(diamondIds).copyTo(_diamondIds); + + _diamondCorners.create((int)diamondCorners.size(), 1, CV_32FC2); + for(unsigned int i = 0; i < diamondCorners.size(); i++) { + _diamondCorners.create(4, 1, CV_32FC2, i, true); + for(int j = 0; j < 4; j++) { + _diamondCorners.getMat(i).at(j) = diamondCorners[i][j]; + } + } + } +} + +void drawDetectedCornersCharuco(InputOutputArray _image, InputArray _charucoCorners, + InputArray _charucoIds, Scalar cornerColor) { + CV_Assert(!_image.getMat().empty() && + (_image.getMat().channels() == 1 || _image.getMat().channels() == 3)); + CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total()) || + _charucoIds.getMat().total() == 0); + + size_t nCorners = _charucoCorners.getMat().total(); + for(size_t i = 0; i < nCorners; i++) { + Point2f corner = _charucoCorners.getMat().at((int)i); + // draw first corner mark + rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 1, LINE_AA); + // draw ID + if(!_charucoIds.empty()) { + int id = _charucoIds.getMat().at((int)i); + stringstream s; + s << "id=" << id; + putText(_image, s.str(), corner + Point2f(5, -5), FONT_HERSHEY_SIMPLEX, 0.5, + cornerColor, 2); + } + } +} + +void drawDetectedDiamonds(InputOutputArray _image, InputArrayOfArrays _corners, InputArray _ids, Scalar borderColor) { + CV_Assert(_image.getMat().total() != 0 && + (_image.getMat().channels() == 1 || _image.getMat().channels() == 3)); + CV_Assert((_corners.total() == _ids.total()) || _ids.total() == 0); + + // calculate colors + Scalar textColor, cornerColor; + textColor = cornerColor = borderColor; + swap(textColor.val[0], textColor.val[1]); // text color just sawp G and R + swap(cornerColor.val[1], cornerColor.val[2]); // corner color just sawp G and B + + int nMarkers = (int)_corners.total(); + for(int i = 0; i < nMarkers; i++) { + Mat currentMarker = _corners.getMat(i); + CV_Assert(currentMarker.total() == 4 && currentMarker.type() == CV_32FC2); + + // draw marker sides + for(int j = 0; j < 4; j++) { + Point2f p0, p1; + p0 = currentMarker.at< Point2f >(j); + p1 = currentMarker.at< Point2f >((j + 1) % 4); + line(_image, p0, p1, borderColor, 1); + } + + // draw first corner mark + rectangle(_image, currentMarker.at< Point2f >(0) - Point2f(3, 3), + currentMarker.at< Point2f >(0) + Point2f(3, 3), cornerColor, 1, LINE_AA); + + // draw id composed by four numbers + if(_ids.total() != 0) { + Point2f cent(0, 0); + for(int p = 0; p < 4; p++) + cent += currentMarker.at< Point2f >(p); + cent = cent / 4.; + stringstream s; + s << "id=" << _ids.getMat().at< Vec4i >(i); + putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2); + } + } +} + +} +} diff --git a/modules/objdetect/test/test_aruco_utils.cpp b/modules/objdetect/test/test_aruco_utils.cpp new file mode 100644 index 0000000..b301622 --- /dev/null +++ b/modules/objdetect/test/test_aruco_utils.cpp @@ -0,0 +1,205 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +#include "test_precomp.hpp" + +#include "test_aruco_utils.hpp" + +namespace opencv_test { + +vector getAxis(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec, + InputArray _tvec, float length, const float offset) { + vector axis; + axis.push_back(Point3f(offset, offset, 0.f)); + axis.push_back(Point3f(length+offset, offset, 0.f)); + axis.push_back(Point3f(offset, length+offset, 0.f)); + axis.push_back(Point3f(offset, offset, length)); + vector axis_to_img; + projectPoints(axis, _rvec, _tvec, _cameraMatrix, _distCoeffs, axis_to_img); + return axis_to_img; +} + +vector getMarkerById(int id, const vector >& corners, const vector& ids) { + for (size_t i = 0ull; i < ids.size(); i++) + if (ids[i] == id) + return corners[i]; + return vector(); +} + +void getSyntheticRT(double yaw, double pitch, double distance, Mat& rvec, Mat& tvec) { + rvec = Mat::zeros(3, 1, CV_64FC1); + tvec = Mat::zeros(3, 1, CV_64FC1); + + // rotate "scene" in pitch axis (x-axis) + Mat rotPitch(3, 1, CV_64FC1); + rotPitch.at(0) = -pitch; + rotPitch.at(1) = 0; + rotPitch.at(2) = 0; + + // rotate "scene" in yaw (y-axis) + Mat rotYaw(3, 1, CV_64FC1); + rotYaw.at(0) = 0; + rotYaw.at(1) = yaw; + rotYaw.at(2) = 0; + + // compose both rotations + composeRT(rotPitch, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotYaw, + Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec); + + // Tvec, just move in z (camera) direction the specific distance + tvec.at(0) = 0.; + tvec.at(1) = 0.; + tvec.at(2) = distance; +} + +void projectMarker(Mat& img, const aruco::Board& board, int markerIndex, Mat cameraMatrix, Mat rvec, Mat tvec, + int markerBorder) { + // canonical image + Mat markerImg; + const int markerSizePixels = 100; + aruco::generateImageMarker(board.getDictionary(), board.getIds()[markerIndex], markerSizePixels, markerImg, markerBorder); + + // projected corners + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + vector corners; + + // get max coordinate of board + Point3f maxCoord = board.getRightBottomCorner(); + // copy objPoints + vector objPoints = board.getObjPoints()[markerIndex]; + // move the marker to the origin + for (size_t i = 0; i < objPoints.size(); i++) + objPoints[i] -= (maxCoord / 2.f); + + projectPoints(objPoints, rvec, tvec, cameraMatrix, distCoeffs, corners); + + // get perspective transform + vector originalCorners; + originalCorners.push_back(Point2f(0, 0)); + originalCorners.push_back(Point2f((float)markerSizePixels, 0)); + originalCorners.push_back(Point2f((float)markerSizePixels, (float)markerSizePixels)); + originalCorners.push_back(Point2f(0, (float)markerSizePixels)); + Mat transformation = getPerspectiveTransform(originalCorners, corners); + + // apply transformation + Mat aux; + const char borderValue = 127; + warpPerspective(markerImg, aux, transformation, img.size(), INTER_NEAREST, BORDER_CONSTANT, + Scalar::all(borderValue)); + + // copy only not-border pixels + for (int y = 0; y < aux.rows; y++) { + for (int x = 0; x < aux.cols; x++) { + if (aux.at< unsigned char >(y, x) == borderValue) continue; + img.at< unsigned char >(y, x) = aux.at< unsigned char >(y, x); + } + } +} + +Mat projectBoard(const aruco::GridBoard& board, Mat cameraMatrix, double yaw, double pitch, double distance, + Size imageSize, int markerBorder) { + Mat rvec, tvec; + getSyntheticRT(yaw, pitch, distance, rvec, tvec); + + Mat img = Mat(imageSize, CV_8UC1, Scalar::all(255)); + for (unsigned int index = 0; index < board.getIds().size(); index++) + projectMarker(img, board, index, cameraMatrix, rvec, tvec, markerBorder); + return img; +} + +/** Check if a set of 3d points are enough for calibration. Z coordinate is ignored. + * Only axis parallel lines are considered */ +static bool _arePointsEnoughForPoseEstimation(const std::vector &points) { + if(points.size() < 4) return false; + + std::vector sameXValue; // different x values in points + std::vector sameXCounter; // number of points with the x value in sameXValue + for(unsigned int i = 0; i < points.size(); i++) { + bool found = false; + for(unsigned int j = 0; j < sameXValue.size(); j++) { + if(sameXValue[j] == points[i].x) { + found = true; + sameXCounter[j]++; + } + } + if(!found) { + sameXValue.push_back(points[i].x); + sameXCounter.push_back(1); + } + } + + // count how many x values has more than 2 points + int moreThan2 = 0; + for(unsigned int i = 0; i < sameXCounter.size(); i++) { + if(sameXCounter[i] >= 2) moreThan2++; + } + + // if we have more than 1 two xvalues with more than 2 points, calibration is ok + if(moreThan2 > 1) + return true; + return false; +} + +bool getCharucoBoardPose(InputArray charucoCorners, InputArray charucoIds, const aruco::CharucoBoard &board, + InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec, InputOutputArray tvec, + bool useExtrinsicGuess) { + CV_Assert((charucoCorners.getMat().total() == charucoIds.getMat().total())); + if(charucoIds.getMat().total() < 4) return false; // need, at least, 4 corners + + std::vector objPoints; + objPoints.reserve(charucoIds.getMat().total()); + for(unsigned int i = 0; i < charucoIds.getMat().total(); i++) { + int currId = charucoIds.getMat().at< int >(i); + CV_Assert(currId >= 0 && currId < (int)board.getChessboardCorners().size()); + objPoints.push_back(board.getChessboardCorners()[currId]); + } + + // points need to be in different lines, check if detected points are enough + if(!_arePointsEnoughForPoseEstimation(objPoints)) return false; + + solvePnP(objPoints, charucoCorners, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess); + return true; +} + +/** + * @brief Return object points for the system centered in a middle (by default) or in a top left corner of single + * marker, given the marker length + */ +static Mat _getSingleMarkerObjectPoints(float markerLength, bool use_aruco_ccw_center) { + CV_Assert(markerLength > 0); + Mat objPoints(4, 1, CV_32FC3); + // set coordinate system in the top-left corner of the marker, with Z pointing out + if (use_aruco_ccw_center) { + objPoints.ptr(0)[0] = Vec3f(-markerLength/2.f, markerLength/2.f, 0); + objPoints.ptr(0)[1] = Vec3f(markerLength/2.f, markerLength/2.f, 0); + objPoints.ptr(0)[2] = Vec3f(markerLength/2.f, -markerLength/2.f, 0); + objPoints.ptr(0)[3] = Vec3f(-markerLength/2.f, -markerLength/2.f, 0); + } + else { + objPoints.ptr(0)[0] = Vec3f(0.f, 0.f, 0); + objPoints.ptr(0)[1] = Vec3f(markerLength, 0.f, 0); + objPoints.ptr(0)[2] = Vec3f(markerLength, markerLength, 0); + objPoints.ptr(0)[3] = Vec3f(0.f, markerLength, 0); + } + return objPoints; +} + +void getMarkersPoses(InputArrayOfArrays corners, float markerLength, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray _rvecs, OutputArray _tvecs, OutputArray objPoints, + bool use_aruco_ccw_center, SolvePnPMethod solvePnPMethod) { + CV_Assert(markerLength > 0); + Mat markerObjPoints = _getSingleMarkerObjectPoints(markerLength, use_aruco_ccw_center); + int nMarkers = (int)corners.total(); + _rvecs.create(nMarkers, 1, CV_64FC3); + _tvecs.create(nMarkers, 1, CV_64FC3); + + Mat rvecs = _rvecs.getMat(), tvecs = _tvecs.getMat(); + for (int i = 0; i < nMarkers; i++) + solvePnP(markerObjPoints, corners.getMat(i), cameraMatrix, distCoeffs, rvecs.at(i), tvecs.at(i), + false, solvePnPMethod); + + if(objPoints.needed()) + markerObjPoints.convertTo(objPoints, -1); +} + +} diff --git a/modules/objdetect/test/test_aruco_utils.hpp b/modules/objdetect/test/test_aruco_utils.hpp new file mode 100644 index 0000000..9b79fa5 --- /dev/null +++ b/modules/objdetect/test/test_aruco_utils.hpp @@ -0,0 +1,42 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "test_precomp.hpp" +#include "opencv2/calib3d.hpp" + +namespace opencv_test { + +static inline double deg2rad(double deg) { return deg * CV_PI / 180.; } + +vector getAxis(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec, InputArray _tvec, + float length, const float offset = 0.f); + +vector getMarkerById(int id, const vector >& corners, const vector& ids); + +/** + * @brief Get rvec and tvec from yaw, pitch and distance + */ +void getSyntheticRT(double yaw, double pitch, double distance, Mat& rvec, Mat& tvec); + +/** + * @brief Project a synthetic marker + */ +void projectMarker(Mat& img, const aruco::Board& board, int markerIndex, Mat cameraMatrix, Mat rvec, Mat tvec, + int markerBorder); + +/** + * @brief Get a synthetic image of GridBoard in perspective + */ +Mat projectBoard(const aruco::GridBoard& board, Mat cameraMatrix, double yaw, double pitch, double distance, + Size imageSize, int markerBorder); + +bool getCharucoBoardPose(InputArray charucoCorners, InputArray charucoIds, const aruco::CharucoBoard &board, + InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec, + InputOutputArray tvec, bool useExtrinsicGuess = false); + +void getMarkersPoses(InputArrayOfArrays corners, float markerLength, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray _rvecs, OutputArray _tvecs, OutputArray objPoints = noArray(), + bool use_aruco_ccw_center = true, SolvePnPMethod solvePnPMethod = SolvePnPMethod::SOLVEPNP_ITERATIVE); + +} diff --git a/modules/objdetect/test/test_boarddetection.cpp b/modules/objdetect/test/test_boarddetection.cpp new file mode 100644 index 0000000..d385992 --- /dev/null +++ b/modules/objdetect/test/test_boarddetection.cpp @@ -0,0 +1,321 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#include "test_precomp.hpp" +#include "test_aruco_utils.hpp" + +namespace opencv_test { namespace { + +enum class ArucoAlgParams +{ + USE_DEFAULT = 0, + USE_ARUCO3 = 1 +}; + +/** + * @brief Check pose estimation of aruco board + */ +class CV_ArucoBoardPose : public cvtest::BaseTest { + public: + CV_ArucoBoardPose(ArucoAlgParams arucoAlgParams) + { + aruco::DetectorParameters params; + aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250); + params.minDistanceToBorder = 3; + if (arucoAlgParams == ArucoAlgParams::USE_ARUCO3) { + params.useAruco3Detection = true; + params.cornerRefinementMethod = aruco::CORNER_REFINE_SUBPIX; + params.minSideLengthCanonicalImg = 16; + params.errorCorrectionRate = 0.8; + } + detector = aruco::ArucoDetector(dictionary, params); + } + + protected: + aruco::ArucoDetector detector; + void run(int); +}; + + +void CV_ArucoBoardPose::run(int) { + int iter = 0; + Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1); + Size imgSize(500, 500); + cameraMatrix.at< double >(0, 0) = cameraMatrix.at< double >(1, 1) = 650; + cameraMatrix.at< double >(0, 2) = imgSize.width / 2; + cameraMatrix.at< double >(1, 2) = imgSize.height / 2; + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + const int sizeX = 3, sizeY = 3; + aruco::DetectorParameters detectorParameters = detector.getDetectorParameters(); + + // for different perspectives + for(double distance = 0.2; distance <= 0.4; distance += 0.15) { + for(int yaw = -55; yaw <= 50; yaw += 25) { + for(int pitch = -55; pitch <= 50; pitch += 25) { + vector tmpIds; + for(int i = 0; i < sizeX*sizeY; i++) + tmpIds.push_back((iter + int(i)) % 250); + aruco::GridBoard gridboard(Size(sizeX, sizeY), 0.02f, 0.005f, detector.getDictionary(), tmpIds); + int markerBorder = iter % 2 + 1; + iter++; + // create synthetic image + Mat img = projectBoard(gridboard, cameraMatrix, deg2rad(yaw), deg2rad(pitch), distance, + imgSize, markerBorder); + vector > corners; + vector ids; + detectorParameters.markerBorderBits = markerBorder; + detector.setDetectorParameters(detectorParameters); + detector.detectMarkers(img, corners, ids); + + ASSERT_EQ(ids.size(), gridboard.getIds().size()); + + // estimate pose + Mat rvec, tvec; + { + Mat objPoints, imgPoints; // get object and image points for the solvePnP function + gridboard.matchImagePoints(corners, ids, objPoints, imgPoints); + solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec); + } + + // check axes + vector axes = getAxis(cameraMatrix, distCoeffs, rvec, tvec, gridboard.getRightBottomCorner().x); + vector topLeft = getMarkerById(gridboard.getIds()[0], corners, ids); + ASSERT_NEAR(topLeft[0].x, axes[0].x, 2.f); + ASSERT_NEAR(topLeft[0].y, axes[0].y, 2.f); + vector topRight = getMarkerById(gridboard.getIds()[2], corners, ids); + ASSERT_NEAR(topRight[1].x, axes[1].x, 2.f); + ASSERT_NEAR(topRight[1].y, axes[1].y, 2.f); + vector bottomLeft = getMarkerById(gridboard.getIds()[6], corners, ids); + ASSERT_NEAR(bottomLeft[3].x, axes[2].x, 2.f); + ASSERT_NEAR(bottomLeft[3].y, axes[2].y, 2.f); + + // check estimate result + for(unsigned int i = 0; i < ids.size(); i++) { + int foundIdx = -1; + for(unsigned int j = 0; j < gridboard.getIds().size(); j++) { + if(gridboard.getIds()[j] == ids[i]) { + foundIdx = int(j); + break; + } + } + + if(foundIdx == -1) { + ts->printf(cvtest::TS::LOG, "Marker detected with wrong ID in Board test"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + + vector< Point2f > projectedCorners; + projectPoints(gridboard.getObjPoints()[foundIdx], rvec, tvec, cameraMatrix, distCoeffs, + projectedCorners); + + for(int c = 0; c < 4; c++) { + double repError = cv::norm(projectedCorners[c] - corners[i][c]); // TODO cvtest + if(repError > 5.) { + ts->printf(cvtest::TS::LOG, "Corner reprojection error too high"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + } + } + } + } + } +} + + + +/** + * @brief Check refine strategy + */ +class CV_ArucoRefine : public cvtest::BaseTest { + public: + CV_ArucoRefine(ArucoAlgParams arucoAlgParams) + { + aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250); + aruco::DetectorParameters params; + params.minDistanceToBorder = 3; + params.cornerRefinementMethod = aruco::CORNER_REFINE_SUBPIX; + if (arucoAlgParams == ArucoAlgParams::USE_ARUCO3) + params.useAruco3Detection = true; + aruco::RefineParameters refineParams(10.f, 3.f, true); + detector = aruco::ArucoDetector(dictionary, params, refineParams); + } + + protected: + aruco::ArucoDetector detector; + void run(int); +}; + + +void CV_ArucoRefine::run(int) { + + int iter = 0; + Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1); + Size imgSize(500, 500); + cameraMatrix.at< double >(0, 0) = cameraMatrix.at< double >(1, 1) = 650; + cameraMatrix.at< double >(0, 2) = imgSize.width / 2; + cameraMatrix.at< double >(1, 2) = imgSize.height / 2; + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + aruco::DetectorParameters detectorParameters = detector.getDetectorParameters(); + + // for different perspectives + for(double distance = 0.2; distance <= 0.4; distance += 0.2) { + for(int yaw = -60; yaw < 60; yaw += 30) { + for(int pitch = -60; pitch <= 60; pitch += 30) { + aruco::GridBoard gridboard(Size(3, 3), 0.02f, 0.005f, detector.getDictionary()); + int markerBorder = iter % 2 + 1; + iter++; + + // create synthetic image + Mat img = projectBoard(gridboard, cameraMatrix, deg2rad(yaw), deg2rad(pitch), distance, + imgSize, markerBorder); + // detect markers + vector > corners, rejected; + vector ids; + detectorParameters.markerBorderBits = markerBorder; + detector.setDetectorParameters(detectorParameters); + detector.detectMarkers(img, corners, ids, rejected); + + // remove a marker from detection + int markersBeforeDelete = (int)ids.size(); + if(markersBeforeDelete < 2) continue; + + rejected.push_back(corners[0]); + corners.erase(corners.begin(), corners.begin() + 1); + ids.erase(ids.begin(), ids.begin() + 1); + + // try to refind the erased marker + detector.refineDetectedMarkers(img, gridboard, corners, ids, rejected, cameraMatrix, + distCoeffs, noArray()); + + // check result + if((int)ids.size() < markersBeforeDelete) { + ts->printf(cvtest::TS::LOG, "Error in refine detected markers"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + } + } + } +} + +TEST(CV_ArucoBoardPose, accuracy) { + CV_ArucoBoardPose test(ArucoAlgParams::USE_DEFAULT); + test.safe_run(); +} + +typedef CV_ArucoBoardPose CV_Aruco3BoardPose; +TEST(CV_Aruco3BoardPose, accuracy) { + CV_Aruco3BoardPose test(ArucoAlgParams::USE_ARUCO3); + test.safe_run(); +} + +typedef CV_ArucoRefine CV_Aruco3Refine; + +TEST(CV_ArucoRefine, accuracy) { + CV_ArucoRefine test(ArucoAlgParams::USE_DEFAULT); + test.safe_run(); +} + +TEST(CV_Aruco3Refine, accuracy) { + CV_Aruco3Refine test(ArucoAlgParams::USE_ARUCO3); + test.safe_run(); +} + +TEST(CV_ArucoBoardPose, CheckNegativeZ) +{ + double matrixData[9] = { -3.9062571886921410e+02, 0., 4.2350000000000000e+02, + 0., 3.9062571886921410e+02, 2.3950000000000000e+02, + 0., 0., 1 }; + cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F, matrixData); + + vector pts3d1, pts3d2; + pts3d1.push_back(cv::Point3f(0.326198f, -0.030621f, 0.303620f)); + pts3d1.push_back(cv::Point3f(0.325340f, -0.100594f, 0.301862f)); + pts3d1.push_back(cv::Point3f(0.255859f, -0.099530f, 0.293416f)); + pts3d1.push_back(cv::Point3f(0.256717f, -0.029557f, 0.295174f)); + + pts3d2.push_back(cv::Point3f(-0.033144f, -0.034819f, 0.245216f)); + pts3d2.push_back(cv::Point3f(-0.035507f, -0.104705f, 0.241987f)); + pts3d2.push_back(cv::Point3f(-0.105289f, -0.102120f, 0.237120f)); + pts3d2.push_back(cv::Point3f(-0.102926f, -0.032235f, 0.240349f)); + + vector tmpIds = {0, 1}; + vector > tmpObjectPoints = {pts3d1, pts3d2}; + aruco::Board board(tmpObjectPoints, aruco::getPredefinedDictionary(0), tmpIds); + + vector > corners; + vector pts2d; + pts2d.push_back(cv::Point2f(37.7f, 203.3f)); + pts2d.push_back(cv::Point2f(38.5f, 120.5f)); + pts2d.push_back(cv::Point2f(105.5f, 115.8f)); + pts2d.push_back(cv::Point2f(104.2f, 202.7f)); + corners.push_back(pts2d); + pts2d.clear(); + pts2d.push_back(cv::Point2f(476.0f, 184.2f)); + pts2d.push_back(cv::Point2f(479.6f, 73.8f)); + pts2d.push_back(cv::Point2f(590.9f, 77.0f)); + pts2d.push_back(cv::Point2f(587.5f, 188.1f)); + corners.push_back(pts2d); + + Vec3d rvec, tvec; + int nUsed = 0; + { + Mat objPoints, imgPoints; // get object and image points for the solvePnP function + board.matchImagePoints(corners, board.getIds(), objPoints, imgPoints); + nUsed = (int)objPoints.total()/4; + solvePnP(objPoints, imgPoints, cameraMatrix, Mat(), rvec, tvec); + } + ASSERT_EQ(nUsed, 2); + + cv::Matx33d rotm; cv::Point3d out; + cv::Rodrigues(rvec, rotm); + out = cv::Point3d(tvec) + rotm*Point3d(board.getObjPoints()[0][0]); + ASSERT_GT(out.z, 0); + + corners.clear(); pts2d.clear(); + pts2d.push_back(cv::Point2f(38.4f, 204.5f)); + pts2d.push_back(cv::Point2f(40.0f, 124.7f)); + pts2d.push_back(cv::Point2f(102.0f, 119.1f)); + pts2d.push_back(cv::Point2f(99.9f, 203.6f)); + corners.push_back(pts2d); + pts2d.clear(); + pts2d.push_back(cv::Point2f(476.0f, 184.3f)); + pts2d.push_back(cv::Point2f(479.2f, 75.1f)); + pts2d.push_back(cv::Point2f(588.7f, 79.2f)); + pts2d.push_back(cv::Point2f(586.3f, 188.5f)); + corners.push_back(pts2d); + + nUsed = 0; + { + Mat objPoints, imgPoints; // get object and image points for the solvePnP function + board.matchImagePoints(corners, board.getIds(), objPoints, imgPoints); + nUsed = (int)objPoints.total()/4; + solvePnP(objPoints, imgPoints, cameraMatrix, Mat(), rvec, tvec, true); + } + ASSERT_EQ(nUsed, 2); + + cv::Rodrigues(rvec, rotm); + out = cv::Point3d(tvec) + rotm*Point3d(board.getObjPoints()[0][0]); + ASSERT_GT(out.z, 0); +} + +TEST(CV_ArucoGenerateBoard, regression_1226) { + int bwidth = 1600; + int bheight = 1200; + + cv::aruco::Dictionary dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + cv::aruco::CharucoBoard board(Size(7, 5), 1.0, 0.75, dict); + cv::Size sz(bwidth, bheight); + cv::Mat mat; + + ASSERT_NO_THROW( + { + board.generateImage(sz, mat, 0, 1); + }); +} + +}} // namespace diff --git a/modules/objdetect/test/test_charucodetection.cpp b/modules/objdetect/test/test_charucodetection.cpp new file mode 100644 index 0000000..ef044c8 --- /dev/null +++ b/modules/objdetect/test/test_charucodetection.cpp @@ -0,0 +1,659 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#include "test_precomp.hpp" +#include "test_aruco_utils.hpp" + +namespace opencv_test { namespace { + +/** + * @brief Get a synthetic image of Chessboard in perspective + */ +static Mat projectChessboard(int squaresX, int squaresY, float squareSize, Size imageSize, + Mat cameraMatrix, Mat rvec, Mat tvec) { + + Mat img(imageSize, CV_8UC1, Scalar::all(255)); + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + + for(int y = 0; y < squaresY; y++) { + float startY = float(y) * squareSize; + for(int x = 0; x < squaresX; x++) { + if(y % 2 != x % 2) continue; + float startX = float(x) * squareSize; + + vector< Point3f > squareCorners; + squareCorners.push_back(Point3f(startX, startY, 0) - Point3f(squaresX*squareSize/2.f, squaresY*squareSize/2.f, 0.f)); + squareCorners.push_back(squareCorners[0] + Point3f(squareSize, 0, 0)); + squareCorners.push_back(squareCorners[0] + Point3f(squareSize, squareSize, 0)); + squareCorners.push_back(squareCorners[0] + Point3f(0, squareSize, 0)); + + vector< vector< Point2f > > projectedCorners; + projectedCorners.push_back(vector< Point2f >()); + projectPoints(squareCorners, rvec, tvec, cameraMatrix, distCoeffs, projectedCorners[0]); + + vector< vector< Point > > projectedCornersInt; + projectedCornersInt.push_back(vector< Point >()); + + for(int k = 0; k < 4; k++) + projectedCornersInt[0] + .push_back(Point((int)projectedCorners[0][k].x, (int)projectedCorners[0][k].y)); + + fillPoly(img, projectedCornersInt, Scalar::all(0)); + } + } + + return img; +} + + +/** + * @brief Check pose estimation of charuco board + */ +static Mat projectCharucoBoard(aruco::CharucoBoard& board, Mat cameraMatrix, double yaw, + double pitch, double distance, Size imageSize, int markerBorder, + Mat &rvec, Mat &tvec) { + + getSyntheticRT(yaw, pitch, distance, rvec, tvec); + + // project markers + Mat img = Mat(imageSize, CV_8UC1, Scalar::all(255)); + for(unsigned int indexMarker = 0; indexMarker < board.getIds().size(); indexMarker++) { + projectMarker(img, board, indexMarker, cameraMatrix, rvec, tvec, markerBorder); + } + + // project chessboard + Mat chessboard = + projectChessboard(board.getChessboardSize().width, board.getChessboardSize().height, + board.getSquareLength(), imageSize, cameraMatrix, rvec, tvec); + + for(unsigned int i = 0; i < chessboard.total(); i++) { + if(chessboard.ptr< unsigned char >()[i] == 0) { + img.ptr< unsigned char >()[i] = 0; + } + } + + return img; +} + +/** + * @brief Check Charuco detection + */ +class CV_CharucoDetection : public cvtest::BaseTest { + public: + CV_CharucoDetection(); + + protected: + void run(int); +}; + + +CV_CharucoDetection::CV_CharucoDetection() {} + + +void CV_CharucoDetection::run(int) { + + int iter = 0; + Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1); + Size imgSize(500, 500); + aruco::DetectorParameters params; + params.minDistanceToBorder = 3; + aruco::CharucoBoard board(Size(4, 4), 0.03f, 0.015f, aruco::getPredefinedDictionary(aruco::DICT_6X6_250)); + aruco::CharucoDetector detector(board, aruco::CharucoParameters(), params); + + cameraMatrix.at(0, 0) = cameraMatrix.at(1, 1) = 600; + cameraMatrix.at(0, 2) = imgSize.width / 2; + cameraMatrix.at(1, 2) = imgSize.height / 2; + + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + + // for different perspectives + for(double distance = 0.2; distance <= 0.4; distance += 0.2) { + for(int yaw = -55; yaw <= 50; yaw += 25) { + for(int pitch = -55; pitch <= 50; pitch += 25) { + + int markerBorder = iter % 2 + 1; + iter++; + + // create synthetic image + Mat rvec, tvec; + Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(yaw), deg2rad(pitch), + distance, imgSize, markerBorder, rvec, tvec); + + // detect markers and interpolate charuco corners + vector > corners; + vector charucoCorners; + vector ids, charucoIds; + + params.markerBorderBits = markerBorder; + detector.setDetectorParameters(params); + + //detector.detectMarkers(img, corners, ids); + if(iter % 2 == 0) { + detector.detectBoard(img, charucoCorners, charucoIds, corners, ids); + } else { + aruco::CharucoParameters charucoParameters; + charucoParameters.cameraMatrix = cameraMatrix; + charucoParameters.distCoeffs = distCoeffs; + detector.setCharucoParameters(charucoParameters); + detector.detectBoard(img, charucoCorners, charucoIds, corners, ids); + } + + if(ids.size() == 0) { + ts->printf(cvtest::TS::LOG, "Marker detection failed"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + + // check results + vector< Point2f > projectedCharucoCorners; + + // copy chessboardCorners + vector copyChessboardCorners = board.getChessboardCorners(); + // move copyChessboardCorners points + for (size_t i = 0; i < copyChessboardCorners.size(); i++) + copyChessboardCorners[i] -= board.getRightBottomCorner() / 2.f; + projectPoints(copyChessboardCorners, rvec, tvec, cameraMatrix, distCoeffs, + projectedCharucoCorners); + + for(unsigned int i = 0; i < charucoIds.size(); i++) { + + int currentId = charucoIds[i]; + + if(currentId >= (int)board.getChessboardCorners().size()) { + ts->printf(cvtest::TS::LOG, "Invalid Charuco corner id"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + + double repError = cv::norm(charucoCorners[i] - projectedCharucoCorners[currentId]); // TODO cvtest + + + if(repError > 5.) { + ts->printf(cvtest::TS::LOG, "Charuco corner reprojection error too high"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + } + } + } + } +} + + + +/** + * @brief Check charuco pose estimation + */ +class CV_CharucoPoseEstimation : public cvtest::BaseTest { + public: + CV_CharucoPoseEstimation(); + + protected: + void run(int); +}; + + +CV_CharucoPoseEstimation::CV_CharucoPoseEstimation() {} + + +void CV_CharucoPoseEstimation::run(int) { + int iter = 0; + Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1); + Size imgSize(500, 500); + aruco::DetectorParameters params; + params.minDistanceToBorder = 3; + aruco::CharucoBoard board(Size(4, 4), 0.03f, 0.015f, aruco::getPredefinedDictionary(aruco::DICT_6X6_250)); + aruco::CharucoDetector detector(board, aruco::CharucoParameters(), params); + + cameraMatrix.at(0, 0) = cameraMatrix.at< double >(1, 1) = 650; + cameraMatrix.at(0, 2) = imgSize.width / 2; + cameraMatrix.at(1, 2) = imgSize.height / 2; + + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + // for different perspectives + for(double distance = 0.2; distance <= 0.3; distance += 0.1) { + for(int yaw = -55; yaw <= 50; yaw += 25) { + for(int pitch = -55; pitch <= 50; pitch += 25) { + + int markerBorder = iter % 2 + 1; + iter++; + + // get synthetic image + Mat rvec, tvec; + Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(yaw), deg2rad(pitch), + distance, imgSize, markerBorder, rvec, tvec); + + // detect markers + vector > corners; + vector ids; + params.markerBorderBits = markerBorder; + detector.setDetectorParameters(params); + + // detect markers and interpolate charuco corners + vector charucoCorners; + vector charucoIds; + + if(iter % 2 == 0) { + detector.detectBoard(img, charucoCorners, charucoIds, corners, ids); + } else { + aruco::CharucoParameters charucoParameters; + charucoParameters.cameraMatrix = cameraMatrix; + charucoParameters.distCoeffs = distCoeffs; + detector.setCharucoParameters(charucoParameters); + detector.detectBoard(img, charucoCorners, charucoIds, corners, ids); + } + ASSERT_EQ(ids.size(), board.getIds().size()); + if(charucoIds.size() == 0) continue; + + // estimate charuco pose + getCharucoBoardPose(charucoCorners, charucoIds, board, cameraMatrix, distCoeffs, rvec, tvec); + + + // check axes + const float offset = (board.getSquareLength() - board.getMarkerLength()) / 2.f; + vector axes = getAxis(cameraMatrix, distCoeffs, rvec, tvec, board.getSquareLength(), offset); + vector topLeft = getMarkerById(board.getIds()[0], corners, ids); + ASSERT_NEAR(topLeft[0].x, axes[1].x, 3.f); + ASSERT_NEAR(topLeft[0].y, axes[1].y, 3.f); + vector bottomLeft = getMarkerById(board.getIds()[2], corners, ids); + ASSERT_NEAR(bottomLeft[0].x, axes[2].x, 3.f); + ASSERT_NEAR(bottomLeft[0].y, axes[2].y, 3.f); + + // check estimate result + vector< Point2f > projectedCharucoCorners; + + projectPoints(board.getChessboardCorners(), rvec, tvec, cameraMatrix, distCoeffs, + projectedCharucoCorners); + + for(unsigned int i = 0; i < charucoIds.size(); i++) { + + int currentId = charucoIds[i]; + + if(currentId >= (int)board.getChessboardCorners().size()) { + ts->printf(cvtest::TS::LOG, "Invalid Charuco corner id"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + + double repError = cv::norm(charucoCorners[i] - projectedCharucoCorners[currentId]); // TODO cvtest + + + if(repError > 5.) { + ts->printf(cvtest::TS::LOG, "Charuco corner reprojection error too high"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + } + } + } + } +} + + +/** + * @brief Check diamond detection + */ +class CV_CharucoDiamondDetection : public cvtest::BaseTest { + public: + CV_CharucoDiamondDetection(); + + protected: + void run(int); +}; + + +CV_CharucoDiamondDetection::CV_CharucoDiamondDetection() {} + + +void CV_CharucoDiamondDetection::run(int) { + + int iter = 0; + Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1); + Size imgSize(500, 500); + aruco::DetectorParameters params; + params.minDistanceToBorder = 0; + float squareLength = 0.03f; + float markerLength = 0.015f; + aruco::CharucoBoard board(Size(3, 3), squareLength, markerLength, + aruco::getPredefinedDictionary(aruco::DICT_6X6_250)); + aruco::CharucoDetector detector(board); + + + cameraMatrix.at(0, 0) = cameraMatrix.at< double >(1, 1) = 650; + cameraMatrix.at(0, 2) = imgSize.width / 2; + cameraMatrix.at(1, 2) = imgSize.height / 2; + + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + aruco::CharucoParameters charucoParameters; + charucoParameters.cameraMatrix = cameraMatrix; + charucoParameters.distCoeffs = distCoeffs; + detector.setCharucoParameters(charucoParameters); + + // for different perspectives + for(double distance = 0.2; distance <= 0.3; distance += 0.1) { + for(int yaw = -50; yaw <= 50; yaw += 25) { + for(int pitch = -50; pitch <= 50; pitch += 25) { + + int markerBorder = iter % 2 + 1; + vector idsTmp; + for(int i = 0; i < 4; i++) + idsTmp.push_back(4 * iter + i); + board = aruco::CharucoBoard(Size(3, 3), squareLength, markerLength, + aruco::getPredefinedDictionary(aruco::DICT_6X6_250), idsTmp); + detector.setBoard(board); + iter++; + + // get synthetic image + Mat rvec, tvec; + Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(yaw), deg2rad(pitch), + distance, imgSize, markerBorder, rvec, tvec); + + // detect markers + vector> corners; + vector ids; + params.markerBorderBits = markerBorder; + detector.setDetectorParameters(params); + //detector.detectMarkers(img, corners, ids); + + + // detect diamonds + vector> diamondCorners; + vector diamondIds; + + detector.detectDiamonds(img, diamondCorners, diamondIds, corners, ids); + + // check detect + if(ids.size() != 4) { + ts->printf(cvtest::TS::LOG, "Not enough markers for diamond detection"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + + // check results + if(diamondIds.size() != 1) { + ts->printf(cvtest::TS::LOG, "Diamond not detected correctly"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + + for(int i = 0; i < 4; i++) { + if(diamondIds[0][i] != board.getIds()[i]) { + ts->printf(cvtest::TS::LOG, "Incorrect diamond ids"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + } + + + vector< Point2f > projectedDiamondCorners; + + // copy chessboardCorners + vector copyChessboardCorners = board.getChessboardCorners(); + // move copyChessboardCorners points + for (size_t i = 0; i < copyChessboardCorners.size(); i++) + copyChessboardCorners[i] -= board.getRightBottomCorner() / 2.f; + + projectPoints(copyChessboardCorners, rvec, tvec, cameraMatrix, distCoeffs, + projectedDiamondCorners); + + vector< Point2f > projectedDiamondCornersReorder(4); + projectedDiamondCornersReorder[0] = projectedDiamondCorners[0]; + projectedDiamondCornersReorder[1] = projectedDiamondCorners[1]; + projectedDiamondCornersReorder[2] = projectedDiamondCorners[3]; + projectedDiamondCornersReorder[3] = projectedDiamondCorners[2]; + + + for(unsigned int i = 0; i < 4; i++) { + + double repError = cv::norm(diamondCorners[0][i] - projectedDiamondCornersReorder[i]); // TODO cvtest + + if(repError > 5.) { + ts->printf(cvtest::TS::LOG, "Diamond corner reprojection error too high"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + } + + // estimate diamond pose + vector< Vec3d > estimatedRvec, estimatedTvec; + getMarkersPoses(diamondCorners, squareLength, cameraMatrix, distCoeffs, estimatedRvec, + estimatedTvec, noArray(), false); + + // check result + vector< Point2f > projectedDiamondCornersPose; + vector< Vec3f > diamondObjPoints(4); + diamondObjPoints[0] = Vec3f(0.f, 0.f, 0); + diamondObjPoints[1] = Vec3f(squareLength, 0.f, 0); + diamondObjPoints[2] = Vec3f(squareLength, squareLength, 0); + diamondObjPoints[3] = Vec3f(0.f, squareLength, 0); + projectPoints(diamondObjPoints, estimatedRvec[0], estimatedTvec[0], cameraMatrix, + distCoeffs, projectedDiamondCornersPose); + + for(unsigned int i = 0; i < 4; i++) { + double repError = cv::norm(projectedDiamondCornersReorder[i] - projectedDiamondCornersPose[i]); // TODO cvtest + + if(repError > 5.) { + ts->printf(cvtest::TS::LOG, "Charuco pose error too high"); + ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); + return; + } + } + } + } + } +} + +/** +* @brief Check charuco board creation +*/ +class CV_CharucoBoardCreation : public cvtest::BaseTest { +public: + CV_CharucoBoardCreation(); + +protected: + void run(int); +}; + +CV_CharucoBoardCreation::CV_CharucoBoardCreation() {} + +void CV_CharucoBoardCreation::run(int) +{ + aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::DICT_5X5_250); + int n = 6; + + float markerSizeFactor = 0.5f; + + for (float squareSize_mm = 5.0f; squareSize_mm < 35.0f; squareSize_mm += 0.1f) + { + aruco::CharucoBoard board_meters(Size(n, n), squareSize_mm*1e-3f, + squareSize_mm * markerSizeFactor * 1e-3f, dictionary); + + aruco::CharucoBoard board_millimeters(Size(n, n), squareSize_mm, + squareSize_mm * markerSizeFactor, dictionary); + + for (size_t i = 0; i < board_meters.getNearestMarkerIdx().size(); i++) + { + if (board_meters.getNearestMarkerIdx()[i].size() != board_millimeters.getNearestMarkerIdx()[i].size() || + board_meters.getNearestMarkerIdx()[i][0] != board_millimeters.getNearestMarkerIdx()[i][0]) + { + ts->printf(cvtest::TS::LOG, + cv::format("Charuco board topology is sensitive to scale with squareSize=%.1f\n", + squareSize_mm).c_str()); + ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); + break; + } + } + } +} + + +TEST(CV_CharucoDetection, accuracy) { + CV_CharucoDetection test; + test.safe_run(); +} + +TEST(CV_CharucoPoseEstimation, accuracy) { + CV_CharucoPoseEstimation test; + test.safe_run(); +} + +TEST(CV_CharucoDiamondDetection, accuracy) { + CV_CharucoDiamondDetection test; + test.safe_run(); +} + +TEST(CV_CharucoBoardCreation, accuracy) { + CV_CharucoBoardCreation test; + test.safe_run(); +} + +TEST(Charuco, testCharucoCornersCollinear_true) +{ + int squaresX = 13; + int squaresY = 28; + float squareLength = 300; + float markerLength = 150; + int dictionaryId = 11; + + aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::PredefinedDictionaryType(dictionaryId)); + + aruco::CharucoBoard charucoBoard(Size(squaresX, squaresY), squareLength, markerLength, dictionary); + + // consistency with C++98 + const int arrLine[9] = {192, 204, 216, 228, 240, 252, 264, 276, 288}; + vector charucoIdsAxisLine(9, 0); + + for (int i = 0; i < 9; i++){ + charucoIdsAxisLine[i] = arrLine[i]; + } + + const int arrDiag[7] = {198, 209, 220, 231, 242, 253, 264}; + + vector charucoIdsDiagonalLine(7, 0); + + for (int i = 0; i < 7; i++){ + charucoIdsDiagonalLine[i] = arrDiag[i]; + } + + bool resultAxisLine = charucoBoard.checkCharucoCornersCollinear(charucoIdsAxisLine); + EXPECT_TRUE(resultAxisLine); + + bool resultDiagonalLine = charucoBoard.checkCharucoCornersCollinear(charucoIdsDiagonalLine); + EXPECT_TRUE(resultDiagonalLine); +} + +TEST(Charuco, testCharucoCornersCollinear_false) +{ + int squaresX = 13; + int squaresY = 28; + float squareLength = 300; + float markerLength = 150; + int dictionaryId = 11; + + aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::PredefinedDictionaryType(dictionaryId)); + + aruco::CharucoBoard charucoBoard(Size(squaresX, squaresY), squareLength, markerLength, dictionary); + + // consistency with C++98 + const int arr[63] = {192, 193, 194, 195, 196, 197, 198, 204, 205, 206, 207, 208, + 209, 210, 216, 217, 218, 219, 220, 221, 222, 228, 229, 230, + 231, 232, 233, 234, 240, 241, 242, 243, 244, 245, 246, 252, + 253, 254, 255, 256, 257, 258, 264, 265, 266, 267, 268, 269, + 270, 276, 277, 278, 279, 280, 281, 282, 288, 289, 290, 291, + 292, 293, 294}; + + vector charucoIds(63, 0); + for (int i = 0; i < 63; i++){ + charucoIds[i] = arr[i]; + } + + bool result = charucoBoard.checkCharucoCornersCollinear(charucoIds); + + EXPECT_FALSE(result); +} + +// test that ChArUco board detection is subpixel accurate +TEST(Charuco, testBoardSubpixelCoords) +{ + cv::Size res{500, 500}; + cv::Mat K = (cv::Mat_(3,3) << + 0.5*res.width, 0, 0.5*res.width, + 0, 0.5*res.height, 0.5*res.height, + 0, 0, 1); + + // set expected_corners values + cv::Mat expected_corners = (cv::Mat_(9,2) << + 200, 200, + 250, 200, + 300, 200, + 200, 250, + 250, 250, + 300, 250, + 200, 300, + 250, 300, + 300, 300 + ); + + cv::Mat gray; + + aruco::Dictionary dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_36h11); + aruco::CharucoBoard board(Size(4, 4), 1.f, .8f, dict); + + // generate ChArUco board + board.generateImage(Size(res.width, res.height), gray, 150); + cv::GaussianBlur(gray, gray, Size(5, 5), 1.0); + + aruco::DetectorParameters params; + params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_APRILTAG; + + aruco::CharucoParameters charucoParameters; + charucoParameters.cameraMatrix = K; + aruco::CharucoDetector detector(board, charucoParameters); + detector.setDetectorParameters(params); + + std::vector ids; + std::vector> corners; + cv::Mat c_ids, c_corners; + + detector.detectBoard(gray, c_corners, c_ids, corners, ids); + + ASSERT_EQ(ids.size(), size_t(8)); + ASSERT_EQ(c_corners.rows, expected_corners.rows); + EXPECT_NEAR(0, cvtest::norm(expected_corners, c_corners.reshape(1), NORM_INF), 1e-1); +} + +TEST(Charuco, issue_14014) +{ + string imgPath = cvtest::findDataFile("aruco/recover.png"); + Mat img = imread(imgPath); + + aruco::DetectorParameters detectorParams; + detectorParams.cornerRefinementMethod = aruco::CORNER_REFINE_SUBPIX; + detectorParams.cornerRefinementMinAccuracy = 0.01; + aruco::ArucoDetector detector(aruco::getPredefinedDictionary(aruco::DICT_7X7_250), detectorParams); + aruco::CharucoBoard board(Size(8, 5), 0.03455f, 0.02164f, detector.getDictionary()); + + vector corners, rejectedPoints; + vector ids; + + detector.detectMarkers(img, corners, ids, rejectedPoints); + + ASSERT_EQ(corners.size(), 19ull); + EXPECT_EQ(Size(4, 1), corners[0].size()); // check dimension of detected corners + + size_t numRejPoints = rejectedPoints.size(); + ASSERT_EQ(rejectedPoints.size(), 26ull); // optional check to track regressions + EXPECT_EQ(Size(4, 1), rejectedPoints[0].size()); // check dimension of detected corners + + detector.refineDetectedMarkers(img, board, corners, ids, rejectedPoints); + + ASSERT_EQ(corners.size(), 20ull); + EXPECT_EQ(Size(4, 1), corners[0].size()); // check dimension of rejected corners after successfully refine + + ASSERT_EQ(rejectedPoints.size() + 1, numRejPoints); + EXPECT_EQ(Size(4, 1), rejectedPoints[0].size()); // check dimension of rejected corners after successfully refine +} + +}} // namespace -- 2.7.4