-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})
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);
#include <vector>
#include <string>
-#include <algorithm>
#include <limits>
using namespace calib;
bool CalibProcessor::detectAndParseChAruco(const cv::Mat &frame)
{
-#ifdef HAVE_OPENCV_ARUCO
cv::Ptr<cv::aruco::Board> board = mCharucoBoard.staticCast<cv::aruco::Board>();
std::vector<std::vector<cv::Point2f> > corners, rejected;
std::vector<int> ids;
- cv::aruco::detectMarkers(frame, cv::makePtr<cv::aruco::Dictionary>(mArucoDictionary), corners, ids, cv::makePtr<cv::aruco::DetectorParameters>(), 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) {
mCurrentCharucoIds = currentCharucoIds;
return true;
}
-#else
- CV_UNUSED(frame);
-#endif
return false;
}
void CalibProcessor::saveFrameData()
{
std::vector<cv::Point3f> objectPoints;
+ std::vector<cv::Point2f> imagePoints;
switch(mBoardType)
{
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);
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<double>(0)) > badAngleThresh || fabs(angles.at<double>(1)) > badAngleThresh) {
- mCalibData->objectPoints.pop_back();
- mCalibData->imagePoints.pop_back();
- isFrameBad = true;
- }
- }
- else {
-#ifdef HAVE_OPENCV_ARUCO
- cv::Mat r, t, angles;
- std::vector<cv::Point3f> allObjPoints;
- allObjPoints.reserve(mCurrentCharucoIds.total());
- for(size_t i = 0; i < mCurrentCharucoIds.total(); i++) {
- int pointID = mCurrentCharucoIds.at<int>((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<double>(0)) > badAngleThresh || fabs(angles.at<double>(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<double>(0)) > badAngleThresh || fabs(angles.at<double>(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;
}
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::aruco::CharucoBoard>(cv::Size(mBoardSize.width + 1, mBoardSize.height + 1), capParams.charucoSquareLength,
+ capParams.charucoMarkerSize, mArucoDictionary);
+ detector = cv::makePtr<cv::aruco::CharucoDetector>(cv::aruco::CharucoDetector(*mCharucoBoard, charucoParameters));
break;
case CirclesGrid:
case AcirclesGrid:
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
-#ifdef HAVE_OPENCV_ARUCO
-#include <opencv2/aruco/charuco.hpp>
-#endif
+#include <opencv2/objdetect.hpp>
#include "calibCommon.hpp"
#include "calibController.hpp"
cv::Mat mCurrentCharucoIds;
cv::Ptr<cv::SimpleBlobDetector> mBlobDetectorPtr;
-#ifdef HAVE_OPENCV_ARUCO
cv::aruco::Dictionary mArucoDictionary;
cv::Ptr<cv::aruco::CharucoBoard> mCharucoBoard;
-#endif
+ cv::Ptr<cv::aruco::CharucoDetector> detector;
int mNeededFramesNum;
unsigned mDelayBetweenCaptures;
#include <opencv2/cvconfig.h>
#include <opencv2/highgui.hpp>
-#ifdef HAVE_OPENCV_ARUCO
-#include <opencv2/aruco/charuco.hpp>
-#endif
#include <string>
#include <vector>
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);
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<cv::aruco::CharucoBoard> 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();
std::string templateType = parser.get<std::string>("t");
+
if(templateType.find("symcircles", 0) == 0) {
mCapParams.board = CirclesGrid;
mCapParams.boardSize = cv::Size(4, 11);
}
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;
@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).
+ @}
+
@}
*/
#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
namespace cv {
namespace aruco {
-//! @addtogroup aruco
+//! @addtogroup objdetect_aruco
//! @{
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<Board> 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
*/
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> 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
* @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<GridBoard> 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>& impl);
+ Ptr<Impl> 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<GridBoard> 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> gridImpl;
- friend class CharucoBoard;
+ CV_DEPRECATED_EXTERNAL // avoid using in C++ code, will be moved to “protected” (need to fix bindings first)
+ GridBoard();
};
/**
* 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<CharucoBoard> 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;
*/
CV_WRAP bool checkCharucoCornersCollinear(InputArray charucoIds) const;
-protected:
- struct CharucoImpl;
- friend struct CharucoImpl;
- Ptr<CharucoImpl> charucoImpl;
+ CV_DEPRECATED_EXTERNAL // avoid using in C++ code, will be moved to “protected” (need to fix bindings first)
+ CharucoBoard();
};
//! @}
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{
* @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
*
* 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> &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);
namespace cv {
namespace aruco {
-//! @addtogroup aruco
+//! @addtogroup objdetect_aruco
//! @{
--- /dev/null
+// 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<std::vector<cv::Point2f> > ). 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<std::vector<cv::Point2f> > ).
+ * @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> 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<std::vector<cv::Point2f> > ). 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<Vec4i>).
+ * 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
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.*;
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);
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);
+ }
+
}
# Python 2/3 compatibility
from __future__ import print_function
-import os, numpy as np
+import os, tempfile, numpy as np
import cv2 as cv
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):
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)
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()
--- /dev/null
+// 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<bool, int> UseArucoParams;
+typedef TestBaseWithParam<UseArucoParams> 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<double>(0, 0) = cameraMatrix.at<double>(1, 1) = imgMarkerSize;
+ cameraMatrix.at<double>(0, 2) = imgMarkerSize / 2.0;
+ cameraMatrix.at<double>(1, 2) = imgMarkerSize / 2.0;
+ }
+
+ static std::pair<Mat, Mat> 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<double>(0)[0] = 0;
+ rotZ.ptr<double>(0)[1] = 0;
+ rotZ.ptr<double>(0)[2] = -0.5 * CV_PI;
+
+ Mat rotX(3, 1, CV_64FC1);
+ rotX.ptr<double>(0)[0] = 0.5 * CV_PI;
+ rotX.ptr<double>(0)[1] = 0;
+ rotX.ptr<double>(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<double>(0)[0] = 0;
+ rotPitch.ptr<double>(0)[1] = pitch;
+ rotPitch.ptr<double>(0)[2] = 0;
+
+ Mat rotYaw(3, 1, CV_64FC1);
+ rotYaw.ptr<double>(0)[0] = yaw;
+ rotYaw.ptr<double>(0)[1] = 0;
+ rotYaw.ptr<double>(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<double>(0)[0] = 0.;
+ tvec.ptr<double>(0)[1] = 0.;
+ tvec.ptr<double>(0)[2] = distance;
+ return rvec_tvec;
+ }
+
+ std::pair<Mat, vector<Point2f> > 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<Point2f>());
+ Mat& img = marker_corners.first;
+ vector<Point2f>& corners = marker_corners.second;
+
+ // canonical image
+ const int markerSizePixels = static_cast<int>(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<Point3f> 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<Point2f> 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<Mat, map<int, vector<Point2f> > > getProjectMarkersTile(const int numMarkers,
+ const aruco::DetectorParameters& params,
+ const aruco::Dictionary& dictionary) {
+ Mat tileImage(imgMarkerSize*numMarkers, imgMarkerSize*numMarkers, CV_8UC1, Scalar::all(255));
+ map<int, vector<Point2f> > 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<Point2f>(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<int, vector<Point2f> > &golds, const vector<int>& ids,
+ const vector<vector<Point2f> >& corners) {
+ std::map<int, double> mapDist;
+ for (const auto& el : golds)
+ mapDist[el.first] = std::numeric_limits<double>::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<int, double>& p1, const pair<int, double>& 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<vector<Point2f> > corners;
+ vector<int> ids;
+ TEST_CYCLE() {
+ detector.detectMarkers(image_map.first, corners, ids);
+ }
+ ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast<int>(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<vector<Point2f> > corners;
+ vector<int> ids;
+ TEST_CYCLE() {
+ detector.detectMarkers(image_map.first, corners, ids);
+ }
+ ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast<int>(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<Aruco3Params, pair<int, int>> ArucoTestParams;
+
+typedef TestBaseWithParam<ArucoTestParams> 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<vector<Point2f> > corners;
+ vector<int> ids;
+ TEST_CYCLE()
+ {
+ detector.detectMarkers(image_map.first, corners, ids);
+ }
+ ASSERT_EQ(numMarkersInRow*numMarkersInRow, static_cast<int>(ids.size()));
+ double maxDistance = getMaxDistance(image_map.second, ids, corners);
+ ASSERT_LT(maxDistance, 3.);
+ SANITY_CHECK_NOTHING();
+}
+
+}
// of this distribution and at http://opencv.org/license.html
#include "../precomp.hpp"
+#include "opencv2/objdetect/aruco_board.hpp"
+
#include <opencv2/objdetect/aruco_dictionary.hpp>
#include <numeric>
namespace aruco {
using namespace std;
-struct Board::BoardImpl {
- std::vector<std::vector<Point3f> > objPoints;
+struct Board::Impl {
Dictionary dictionary;
- Point3f rightBottomBorder;
std::vector<int> ids;
+ std::vector<std::vector<Point3f> > objPoints;
+ Point3f rightBottomBorder;
- BoardImpl() {
- dictionary = Dictionary(getPredefinedDictionary(PredefinedDictionaryType::DICT_4X4_50));
- }
-};
+ explicit Impl(const Dictionary& _dictionary):
+ dictionary(_dictionary)
+ {}
-Board::Board(): boardImpl(makePtr<BoardImpl>()) {}
+ virtual ~Impl() {}
-Board::~Board() {}
+ Impl(const Impl&) = delete;
+ Impl& operator=(const Impl&) = delete;
-Ptr<Board> 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<vector<Point3f> > obj_points_vector;
- Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f);
- for (unsigned int i = 0; i < objPoints.total(); i++) {
- vector<Point3f> 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<Point3f>(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<Board> res = makePtr<Board>(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<vector<Point3f> >& Board::getObjPoints() const {
- return this->boardImpl->objPoints;
-}
+ vector<Point3f> objPnts;
+ objPnts.reserve(nDetectedMarkers);
-const Point3f& Board::getRightBottomCorner() const {
- return this->boardImpl->rightBottomBorder;
-}
+ vector<Point2f> imgPnts;
+ imgPnts.reserve(nDetectedMarkers);
-const vector<int>& 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<Point2f>(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);
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);
}
}
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);
// 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
}
}
-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(_impl)
+{
+ CV_Assert(impl);
+}
- size_t nDetectedMarkers = detectedIds.total();
+Board::Board():
+ impl(nullptr)
+{}
- vector<Point3f> 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<Point2f> imgPnts;
- imgPnts.reserve(nDetectedMarkers);
+ vector<vector<Point3f> > obj_points_vector;
+ Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f);
+ for (unsigned int i = 0; i < objPoints.total(); i++) {
+ vector<Point3f> 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<Point2f>(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<Point3f>(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<vector<Point3f> >& 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<int>& 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<GridImpl>()) {}
-
-Ptr<GridBoard> 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<GridBoard> res = makePtr<GridBoard>(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<vector<Point3f> > objPoints;
objPoints.reserve(totalMarkers);
- ids.copyTo(res->boardImpl->ids);
+
+ if(!ids.empty()) {
+ ids.copyTo(impl->ids);
+ } else {
+ impl->ids = std::vector<int>(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 <Point3f> corners(4);
corners[0] = Point3f(x * (markerLength + markerSeparation),
y * (markerLength + markerSeparation), 0);
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> GridBoard::create(int markersX, int markersY, float markerLength, float markerSeparation,
- const Dictionary &dictionary, int firstMarker) {
- vector<int> 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<GridBoardImpl>(impl)->size;
}
float GridBoard::getMarkerLength() const {
- return gridImpl->markerLength;
+ CV_Assert(impl);
+ return static_pointer_cast<GridBoardImpl>(impl)->markerLength;
}
float GridBoard::getMarkerSeparation() const {
- return gridImpl->markerSeparation;
+ CV_Assert(impl);
+ return static_pointer_cast<GridBoardImpl>(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<Point3f> chessboardCorners;
// for each charuco corner, nearest marker id and nearest marker corner id of each marker
std::vector<std::vector<int> > nearestMarkerIdx;
std::vector<std::vector<int> > 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<CharucoImpl>()) {}
+/** 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<Point3f> objPnts(nDetected);
+ vector<Point2f> imgPnts(nDetected);
+ for(size_t i = 0ull; i < nDetected; i++) {
+ int pointId = detectedIds.getMat().at<int>((int)i);
+ CV_Assert(pointId >= 0 && pointId < (int)chessboardCorners.size());
+ objPnts[i] = chessboardCorners[pointId];
+ imgPnts[i] = detectedCorners.getMat().at<Point2f>((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);
// 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
}
}
-/**
- * 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> 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<CharucoBoard> res = makePtr<CharucoBoard>(board);
-
- res->charucoImpl->sizeX = squaresX;
- res->charucoImpl->sizeY = squaresY;
- res->charucoImpl->squareLength = squareLength;
- res->charucoImpl->markerLength = markerLength;
- res->boardImpl->dictionary = dictionary;
- vector<vector<Point3f> > objPoints;
+ CV_Assert(size.width > 1 && size.height > 1 && markerLength > 0 && squareLength > markerLength);
+ vector<vector<Point3f> > 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
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<Point3f> & c = static_pointer_cast<CharucoBoardImpl>(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<CharucoBoardImpl>(impl)->calcNearestMarkerCorners();
}
-Size CharucoBoard::getChessboardSize() const { return Size(charucoImpl->sizeX, charucoImpl->sizeY); }
+Size CharucoBoard::getChessboardSize() const {
+ CV_Assert(impl);
+ return static_pointer_cast<CharucoBoardImpl>(impl)->size;
+}
-float CharucoBoard::getSquareLength() const { return charucoImpl->squareLength; }
+float CharucoBoard::getSquareLength() const {
+ CV_Assert(impl);
+ return static_pointer_cast<CharucoBoardImpl>(impl)->squareLength;
+}
-float CharucoBoard::getMarkerLength() const { return charucoImpl->markerLength; }
+float CharucoBoard::getMarkerLength() const {
+ CV_Assert(impl);
+ return static_pointer_cast<CharucoBoardImpl>(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<CharucoBoardImpl>(impl);
+ CV_Assert(board->chessboardCorners.size() >= charucoIds.getMat().total());
- Vec<double, 3> point0(charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(0)].x,
- charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(0)].y, 1);
+ Vec<double, 3> point0(board->chessboardCorners[charucoIds.getMat().at<int>(0)].x,
+ board->chessboardCorners[charucoIds.getMat().at<int>(0)].y, 1);
- Vec<double, 3> point1(charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(1)].x,
- charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(1)].y, 1);
+ Vec<double, 3> point1(board->chessboardCorners[charucoIds.getMat().at<int>(1)].x,
+ board->chessboardCorners[charucoIds.getMat().at<int>(1)].y, 1);
// create a line from the first two points.
Vec<double, 3> testLine = point0.cross(point1);
double dotProduct;
for (unsigned int i = 2; i < nCharucoCorners; i++){
- testPoint(0) = charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(i)].x;
- testPoint(1) = charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(i)].y;
+ testPoint(0) = board->chessboardCorners[charucoIds.getMat().at<int>(i)].x;
+ testPoint(1) = board->chessboardCorners[charucoIds.getMat().at<int>(i)].y;
// if testPoint is on testLine, dotProduct will be zero (or very, very close)
dotProduct = testPoint.dot(testLine);
}
std::vector<Point3f> CharucoBoard::getChessboardCorners() const {
- return charucoImpl->chessboardCorners;
+ CV_Assert(impl);
+ return static_pointer_cast<CharucoBoardImpl>(impl)->chessboardCorners;
}
std::vector<std::vector<int> > CharucoBoard::getNearestMarkerIdx() const {
- return charucoImpl->nearestMarkerIdx;
+ CV_Assert(impl);
+ return static_pointer_cast<CharucoBoardImpl>(impl)->nearestMarkerIdx;
}
std::vector<std::vector<int> > CharucoBoard::getNearestMarkerCorners() const {
- return charucoImpl->nearestMarkerCorners;
+ CV_Assert(impl);
+ return static_pointer_cast<CharucoBoardImpl>(impl)->nearestMarkerCorners;
}
}
}
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;
/**
* Project board markers that are not included in the list of detected markers
*/
-static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArrayOfArrays _detectedCorners,
- InputOutputArray _detectedIds, InputArray _cameraMatrix, InputArray _distCoeffs,
- vector<vector<Point2f> >& _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<int>(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<vector<Point2f> >& 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<vector<Point2f> > undetectedCorners;
+ const std::vector<int>& ids = board.getIds();
vector<int> 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<int>()[j]) {
+ for(unsigned int j = 0; j < detectedIds.total(); j++) {
+ if(ids[i] == detectedIds.getMat().ptr<int>()[j]) {
foundIdx = j;
break;
}
// not detected
if(foundIdx == -1) {
undetectedCorners.push_back(vector<Point2f>());
- 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> &_board, InputOutputArrayOfArrays _detectedCorners,
+static void _projectUndetectedMarkers(const Board &_board, InputOutputArrayOfArrays _detectedCorners,
InputOutputArray _detectedIds,
vector<vector<Point2f> >& _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<Point2f> detectedMarkersObj2DAll; // Object coordinates (without Z) of all the detected
// missing markers in different vectors
vector<int> 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<int>()[i] == _board->getIds()[j]) {
+ if(_detectedIds.getMat().ptr<int>()[i] == _board.getIds()[j]) {
for(int c = 0; c < 4; c++) {
imageCornersAll.push_back(_detectedCorners.getMat(i).ptr<Point2f>()[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;
undetectedMarkersObj2D.push_back(vector<Point2f>());
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;
Mat(undetectedMarkersIds).copyTo(_undetectedMarkersIds);
}
-void ArucoDetector::refineDetectedMarkers(InputArray _image, const Ptr<Board> &_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;
--- /dev/null
+// 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 <opencv2/calib3d.hpp>
+#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<Size> getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, InputArray markerIds,
+ InputArray charucoCorners) {
+ size_t nCharucoCorners = charucoCorners.getMat().total();
+
+ CV_Assert(board.getNearestMarkerIdx().size() == nCharucoCorners);
+
+ vector<Size> winSizes(nCharucoCorners, Size(-1, -1));
+ for(size_t i = 0ull; i < nCharucoCorners; i++) {
+ if(charucoCorners.getMat().at<Point2f>((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>((int)k) == markerId) {
+ markerIdx = (int)k;
+ break;
+ }
+ }
+ if(markerIdx == -1) continue;
+ Point2f markerCorner =
+ markerCorners.getMat(markerIdx).at<Point2f>(board.getNearestMarkerCorners()[i][j]);
+ Point2f charucoCorner = charucoCorners.getMat().at<Point2f>((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<Size> &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<Point2f> filteredChessboardImgPoints;
+ vector<Size> filteredWinSizes;
+ vector<int> 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<Point2f>(i))) {
+ filteredChessboardImgPoints.push_back(allCorners.getMat().at<Point2f>(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<Point2f> 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<Point2f> 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<Size> 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<Mat> transformations(nMarkers);
+ vector<bool> validTransform(nMarkers, false);
+ const auto& ids = board.getIds();
+ for(size_t i = 0ull; i < nMarkers; i++) {
+ vector<Point2f> markerObjPoints2D;
+ int markerId = markerIds.getMat().at<int>((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<Point2f> 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<Point2f> 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>((int)k) == markerId) {
+ markerIdx = (int)k;
+ break;
+ }
+ }
+ if (markerIdx != -1 &&
+ validTransform[markerIdx])
+ {
+ vector<Point2f> 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<Size> 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<Point2f> filteredCharucoCorners;
+ vector<int> filteredCharucoIds;
+ // for each charuco corner
+ for(unsigned int i = 0; i < allCharucoIds.getMat().total(); i++) {
+ int currentCharucoId = allCharucoIds.getMat().at<int>(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<int>(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<Point2f>(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<CharucoDetectorImpl>(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<vector<Point2f>> tmpMarkerCorners;
+ vector<int> tmpMarkerIds;
+ InputOutputArrayOfArrays _markerCorners = markerCorners.needed() ? markerCorners : tmpMarkerCorners;
+ InputOutputArray _markerIds = markerIds.needed() ? markerIds : tmpMarkerIds;
+
+ if (markerCorners.empty() && markerIds.empty()) {
+ vector<vector<Point2f> > 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<vector<Point2f>> tmpMarkerCorners;
+ vector<int> 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<vector<Point2f>> diamondCorners;
+ vector<Vec4i> diamondIds;
+
+ // stores if the detected markers have been assigned or not to a diamond
+ vector<bool> 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<Point2f>(c) - corners.at<Point2f>((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<int>(i);
+
+ // prepare data to call refineDetectedMarkers()
+ // detected markers (only the current one)
+ vector<Mat> currentMarker;
+ vector<int> currentMarkerId;
+ currentMarker.push_back(_markerCorners.getMat(i));
+ currentMarkerId.push_back(currentId);
+
+ // marker candidates (the rest of markers if they have not been assigned)
+ vector<Mat> candidates;
+ vector<int> 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<int> 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<int> 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<int>(currentMarkerIdx);
+ assigned[currentMarkerIdx] = true;
+ }
+
+ // interpolate the charuco corners of the diamond
+ vector<Point2f> currentMarkerCorners;
+ Mat aux;
+ detectBoard(grey, currentMarkerCorners, aux, currentMarker, currentMarkerId);
+
+ // if everything is ok, save the diamond
+ if(currentMarkerCorners.size() > 0ull) {
+ // reorder corners
+ vector<Point2f> 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<Point2f>(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<Point2f>((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>((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);
+ }
+ }
+}
+
+}
+}
--- /dev/null
+// 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<Point2f> getAxis(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec,
+ InputArray _tvec, float length, const float offset) {
+ vector<Point3f> 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<Point2f> axis_to_img;
+ projectPoints(axis, _rvec, _tvec, _cameraMatrix, _distCoeffs, axis_to_img);
+ return axis_to_img;
+}
+
+vector<Point2f> getMarkerById(int id, const vector<vector<Point2f> >& corners, const vector<int>& ids) {
+ for (size_t i = 0ull; i < ids.size(); i++)
+ if (ids[i] == id)
+ return corners[i];
+ return vector<Point2f>();
+}
+
+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<double>(0) = -pitch;
+ rotPitch.at<double>(1) = 0;
+ rotPitch.at<double>(2) = 0;
+
+ // rotate "scene" in yaw (y-axis)
+ Mat rotYaw(3, 1, CV_64FC1);
+ rotYaw.at<double>(0) = 0;
+ rotYaw.at<double>(1) = yaw;
+ rotYaw.at<double>(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<double>(0) = 0.;
+ tvec.at<double>(1) = 0.;
+ tvec.at<double>(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<Point2f> corners;
+
+ // get max coordinate of board
+ Point3f maxCoord = board.getRightBottomCorner();
+ // copy objPoints
+ vector<Point3f> 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<Point2f> 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<Point3f> &points) {
+ if(points.size() < 4) return false;
+
+ std::vector<double> sameXValue; // different x values in points
+ std::vector<int> 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<Point3f> 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<Vec3f>(0)[0] = Vec3f(-markerLength/2.f, markerLength/2.f, 0);
+ objPoints.ptr<Vec3f>(0)[1] = Vec3f(markerLength/2.f, markerLength/2.f, 0);
+ objPoints.ptr<Vec3f>(0)[2] = Vec3f(markerLength/2.f, -markerLength/2.f, 0);
+ objPoints.ptr<Vec3f>(0)[3] = Vec3f(-markerLength/2.f, -markerLength/2.f, 0);
+ }
+ else {
+ objPoints.ptr<Vec3f>(0)[0] = Vec3f(0.f, 0.f, 0);
+ objPoints.ptr<Vec3f>(0)[1] = Vec3f(markerLength, 0.f, 0);
+ objPoints.ptr<Vec3f>(0)[2] = Vec3f(markerLength, markerLength, 0);
+ objPoints.ptr<Vec3f>(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<Vec3d>(i), tvecs.at<Vec3d>(i),
+ false, solvePnPMethod);
+
+ if(objPoints.needed())
+ markerObjPoints.convertTo(objPoints, -1);
+}
+
+}
--- /dev/null
+// 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<Point2f> getAxis(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec, InputArray _tvec,
+ float length, const float offset = 0.f);
+
+vector<Point2f> getMarkerById(int id, const vector<vector<Point2f> >& corners, const vector<int>& 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);
+
+}
--- /dev/null
+// 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<int> 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<vector<Point2f> > corners;
+ vector<int> 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<Point2f> axes = getAxis(cameraMatrix, distCoeffs, rvec, tvec, gridboard.getRightBottomCorner().x);
+ vector<Point2f> 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<Point2f> 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<Point2f> 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<vector<Point2f> > corners, rejected;
+ vector<int> 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<cv::Point3f> 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<int> tmpIds = {0, 1};
+ vector<vector<Point3f> > tmpObjectPoints = {pts3d1, pts3d2};
+ aruco::Board board(tmpObjectPoints, aruco::getPredefinedDictionary(0), tmpIds);
+
+ vector<vector<Point2f> > corners;
+ vector<Point2f> 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
--- /dev/null
+// 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<double>(0, 0) = cameraMatrix.at<double>(1, 1) = 600;
+ 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));
+
+ // 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<vector<Point2f> > corners;
+ vector<Point2f> charucoCorners;
+ vector<int> 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<Point3f> 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<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));
+ // 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<vector<Point2f> > corners;
+ vector<int> ids;
+ params.markerBorderBits = markerBorder;
+ detector.setDetectorParameters(params);
+
+ // detect markers and interpolate charuco corners
+ vector<Point2f> charucoCorners;
+ vector<int> 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<Point2f> axes = getAxis(cameraMatrix, distCoeffs, rvec, tvec, board.getSquareLength(), offset);
+ vector<Point2f> 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<Point2f> 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<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::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<int> 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<vector<Point2f>> corners;
+ vector<int> ids;
+ params.markerBorderBits = markerBorder;
+ detector.setDetectorParameters(params);
+ //detector.detectMarkers(img, corners, ids);
+
+
+ // detect diamonds
+ vector<vector<Point2f>> diamondCorners;
+ vector<Vec4i> 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<Point3f> 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<int> 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<int> 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<int> 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_<double>(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_<float>(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<int> ids;
+ std::vector<std::vector<cv::Point2f>> 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<Mat> corners, rejectedPoints;
+ vector<int> 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
projects / platform / upstream / opencv.git / commitdiff