mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-01-18 22:44:02 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #22986 from AleksandrPanov:move_contrib_charuco_to_main_objdetect

Browse Source 
merge with https://github.com/opencv/opencv_contrib/pull/3394

move Charuco API from contrib to main repo:

- add CharucoDetector:
```
CharucoDetector::detectBoard(InputArray image, InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds, 
                             OutputArray charucoCorners, OutputArray charucoIds) const // detect charucoCorners and/or markerCorners
CharucoDetector::detectDiamonds(InputArray image, InputOutputArrayOfArrays _markerCorners,
                                InputOutputArrayOfArrays _markerIds, OutputArrayOfArrays _diamondCorners,
                                OutputArray _diamondIds) const
```

- add `matchImagePoints()` for `CharucoBoard`
- remove contrib aruco dependencies from interactive-calibration tool
- move almost all aruco tests to objdetect

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Alexander Panov 2022-12-28 17:28:59 +03:00 committed by GitHub
parent 9627ab9462
commit 121034876d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
21 changed files with 2721 additions and 499 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
apps/interactive-calibration/CMakeLists.txt
View File

@ -1,6 +1,3 @@
set(DEPS opencv_core opencv_imgproc opencv_features2d opencv_highgui opencv_calib3d opencv_videoio)
if(${BUILD_opencv_aruco})
list(APPEND DEPS opencv_aruco)
endif()
set(DEPS opencv_core opencv_imgproc opencv_features2d opencv_highgui opencv_calib3d opencv_videoio opencv_objdetect)
file(GLOB SRCS *.cpp)
ocv_add_application(opencv_interactive-calibration MODULES ${DEPS} SRCS ${SRCS})

8
apps/interactive-calibration/calibController.cpp
View File

@ -219,10 +219,10 @@ void calib::calibDataController::filterFrames()
if(mCalibData->imagePoints.size()) {
mCalibData->imagePoints.erase(mCalibData->imagePoints.begin() + worstElemIndex);
mCalibData->objectPoints.erase(mCalibData->objectPoints.begin() + worstElemIndex);
}
else {
mCalibData->allCharucoCorners.erase(mCalibData->allCharucoCorners.begin() + worstElemIndex);
mCalibData->allCharucoIds.erase(mCalibData->allCharucoIds.begin() + worstElemIndex);
if (mCalibData->allCharucoCorners.size()) {
mCalibData->allCharucoCorners.erase(mCalibData->allCharucoCorners.begin() + worstElemIndex);
mCalibData->allCharucoIds.erase(mCalibData->allCharucoIds.begin() + worstElemIndex);
}
}
cv::Mat newErrorsVec = cv::Mat((int)numberOfFrames - 1, 1, CV_64F);

62
apps/interactive-calibration/frameProcessor.cpp
View File

@ -11,7 +11,6 @@
#include <vector>
#include <string>
#include <algorithm>
#include <limits>
using namespace calib;
@ -74,17 +73,12 @@ bool CalibProcessor::detectAndParseChessboard(const cv::Mat &frame)
bool CalibProcessor::detectAndParseChAruco(const cv::Mat &frame)
{
#ifdef HAVE_OPENCV_ARUCO
cv::Ptr<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) {
@ -102,9 +96,6 @@ bool CalibProcessor::detectAndParseChAruco(const cv::Mat &frame)
mCurrentCharucoIds = currentCharucoIds;
return true;
}
#else
CV_UNUSED(frame);
#endif
return false;
}
@ -155,6 +146,7 @@ bool CalibProcessor::detectAndParseDualACircles(const cv::Mat &frame)
void CalibProcessor::saveFrameData()
{
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point2f> imagePoints;
switch(mBoardType)
{
@ -169,6 +161,11 @@ void CalibProcessor::saveFrameData()
case chAruco:
mCalibData->allCharucoCorners.push_back(mCurrentCharucoCorners);
mCalibData->allCharucoIds.push_back(mCurrentCharucoIds);
mCharucoBoard->matchImagePoints(mCurrentCharucoCorners, mCurrentCharucoIds, objectPoints, imagePoints);
CV_Assert(mCurrentCharucoIds.total() == imagePoints.size());
mCalibData->imagePoints.push_back(imagePoints);
mCalibData->objectPoints.push_back(objectPoints);
break;
case CirclesGrid:
objectPoints.reserve(mBoardSize.height*mBoardSize.width);
@ -248,37 +245,17 @@ bool CalibProcessor::checkLastFrame()
else
mCalibData->cameraMatrix.copyTo(tmpCamMatrix);
if(mBoardType != chAruco) {
cv::Mat r, t, angles;
cv::solvePnP(mCalibData->objectPoints.back(), mCurrentImagePoints, tmpCamMatrix, mCalibData->distCoeffs, r, t);
RodriguesToEuler(r, angles, CALIB_DEGREES);
if(fabs(angles.at<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;
}
@ -295,15 +272,16 @@ CalibProcessor::CalibProcessor(cv::Ptr<calibrationData> data, captureParameters
mTemplDist = capParams.templDst;
mSaveFrames = capParams.saveFrames;
mZoom = capParams.zoom;
cv::aruco::CharucoParameters charucoParameters;
charucoParameters.tryRefineMarkers = true;
switch(mBoardType)
{
case chAruco:
#ifdef HAVE_OPENCV_ARUCO
mArucoDictionary = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(capParams.charucoDictName));
mCharucoBoard = cv::aruco::CharucoBoard::create(mBoardSize.width, mBoardSize.height, capParams.charucoSquareLength,
capParams.charucoMarkerSize, mArucoDictionary);
#endif
mCharucoBoard = cv::makePtr<cv::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:

7
apps/interactive-calibration/frameProcessor.hpp
View File

@ -7,9 +7,7 @@
#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"
@ -39,10 +37,9 @@ protected:
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;

37
apps/interactive-calibration/main.cpp
View File

@ -7,9 +7,6 @@
#include <opencv2/cvconfig.h>
#include <opencv2/highgui.hpp>
#ifdef HAVE_OPENCV_ARUCO
#include <opencv2/aruco/charuco.hpp>
#endif
#include <string>
#include <vector>
@ -105,11 +102,6 @@ int main(int argc, char** argv)
captureParameters capParams = paramsController.getCaptureParameters();
internalParameters intParams = paramsController.getInternalParameters();
#ifndef HAVE_OPENCV_ARUCO
if(capParams.board == chAruco)
CV_Error(cv::Error::StsNotImplemented, "Aruco module is disabled in current build configuration."
" Consider usage of another calibration pattern\n");
#endif
cv::TermCriteria solverTermCrit = cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS,
intParams.solverMaxIters, intParams.solverEps);
@ -170,29 +162,12 @@ int main(int argc, char** argv)
globalData->imageSize = pipeline->getImageSize();
calibrationFlags = controller->getNewFlags();
if(capParams.board != chAruco) {
globalData->totalAvgErr =
cv::calibrateCamera(globalData->objectPoints, globalData->imagePoints,
globalData->imageSize, globalData->cameraMatrix,
globalData->distCoeffs, cv::noArray(), cv::noArray(),
globalData->stdDeviations, cv::noArray(), globalData->perViewErrors,
calibrationFlags, solverTermCrit);
}
else {
#ifdef HAVE_OPENCV_ARUCO
cv::aruco::Dictionary dictionary =
cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(capParams.charucoDictName));
cv::Ptr<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();

3
apps/interactive-calibration/parametersController.cpp
View File

@ -109,6 +109,7 @@ bool calib::parametersController::loadFromParser(cv::CommandLineParser &parser)
std::string templateType = parser.get<std::string>("t");
if(templateType.find("symcircles", 0) == 0) {
mCapParams.board = CirclesGrid;
mCapParams.boardSize = cv::Size(4, 11);
@ -127,7 +128,7 @@ bool calib::parametersController::loadFromParser(cv::CommandLineParser &parser)
}
else if(templateType.find("charuco", 0) == 0) {
mCapParams.board = chAruco;
mCapParams.boardSize = cv::Size(6, 8);
mCapParams.boardSize = cv::Size(5, 7);
mCapParams.charucoDictName = 0;
mCapParams.charucoSquareLength = 200;
mCapParams.charucoMarkerSize = 100;

21
modules/objdetect/include/opencv2/objdetect.hpp
View File

@ -105,6 +105,26 @@ using a Boosted Cascade of Simple Features. IEEE CVPR, 2001. The paper is availa
@defgroup objdetect_dnn_face DNN-based face detection and recognition
Check @ref tutorial_dnn_face "the corresponding tutorial" for more details.
@defgroup objdetect_common Common functions and classes
@defgroup objdetect_aruco ArUco markers and boards detection for robust camera pose estimation
@{
ArUco Marker Detection
Square fiducial markers (also known as Augmented Reality Markers) are useful for easy,
fast and robust camera pose estimation.
The main functionality of ArucoDetector class is detection of markers in an image. If the markers are grouped
as a board, then you can try to recover the missing markers with ArucoDetector::refineDetectedMarkers().
ArUco markers can also be used for advanced chessboard corner finding. To do this, group the markers in the
CharucoBoard and find the corners of the chessboard with the CharucoDetector::detectBoard().
The implementation is based on the ArUco Library by R. Muñoz-Salinas and S. Garrido-Jurado @cite Aruco2014.
Markers can also be detected based on the AprilTag 2 @cite wang2016iros fiducial detection method.
@sa @cite Aruco2014
This code has been originally developed by Sergio Garrido-Jurado as a project
for Google Summer of Code 2015 (GSoC 15).
@}
@}
*/
@ -852,5 +872,6 @@ protected:
#include "opencv2/objdetect/detection_based_tracker.hpp"
#include "opencv2/objdetect/face.hpp"
#include "opencv2/objdetect/aruco_detector.hpp"
#include "opencv2/objdetect/charuco_detector.hpp"
#endif

141
modules/objdetect/include/opencv2/objdetect/aruco_board.hpp
View File

@ -8,7 +8,7 @@
namespace cv {
namespace aruco {
//! @addtogroup aruco
//! @addtogroup objdetect_aruco
//! @{
class Dictionary;
@ -22,29 +22,15 @@ class Dictionary;
* - The dictionary which indicates the type of markers of the board
* - The identifier of all the markers in the board.
*/
class CV_EXPORTS_W Board {
protected:
Board(); // use ::create()
class CV_EXPORTS_W_SIMPLE Board {
public:
/** @brief Draw a planar board
*
* @param outSize size of the output image in pixels.
* @param img output image with the board. The size of this image will be outSize
* and the board will be on the center, keeping the board proportions.
* @param marginSize minimum margins (in pixels) of the board in the output image
* @param borderBits width of the marker borders.
*
* This function return the image of the GridBoard, ready to be printed.
*/
CV_WRAP virtual void generateImage(Size outSize, OutputArray img, int marginSize = 0, int borderBits = 1) const;
/** @brief Provide way to create Board by passing necessary data. Specially needed in Python.
/** @brief Common Board constructor
*
* @param objPoints array of object points of all the marker corners in the board
* @param dictionary the dictionary of markers employed for this board
* @param ids vector of the identifiers of the markers in the board
*/
CV_WRAP static Ptr<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
*/
@ -72,31 +58,19 @@ public:
CV_WRAP const Point3f& getRightBottomCorner() const;
/** @brief Given a board configuration and a set of detected markers, returns the corresponding
* image points and object points to call solvePnP
* image points and object points to call solvePnP()
*
* @param detectedCorners List of detected marker corners of the board.
* @param detectedIds List of identifiers for each marker.
* For CharucoBoard class you can set list of charuco corners.
* @param detectedIds List of identifiers for each marker or list of charuco identifiers for each corner.
* For CharucoBoard class you can set list of charuco identifiers for each corner.
* @param objPoints Vector of vectors of board marker points in the board coordinate space.
* @param imgPoints Vector of vectors of the projections of board marker corner points.
*/
CV_WRAP void matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds,
OutputArray objPoints, OutputArray imgPoints) const;
virtual ~Board();
protected:
struct BoardImpl;
Ptr<BoardImpl> boardImpl;
};
/** @brief Planar board with grid arrangement of markers
*
* More common type of board. All markers are placed in the same plane in a grid arrangement.
* The board image can be drawn using generateImage() method.
*/
class CV_EXPORTS_W GridBoard : public Board {
protected:
GridBoard();
public:
/** @brief Draw a GridBoard
/** @brief Draw a planar board
*
* @param outSize size of the output image in pixels.
* @param img output image with the board. The size of this image will be outSize
@ -104,50 +78,44 @@ public:
* @param marginSize minimum margins (in pixels) of the board in the output image
* @param borderBits width of the marker borders.
*
* This function return the image of the GridBoard, ready to be printed.
* This function return the image of the board, ready to be printed.
*/
CV_WRAP void generateImage(Size outSize, OutputArray img, int marginSize = 0, int borderBits = 1) const CV_OVERRIDE;
CV_WRAP void generateImage(Size outSize, OutputArray img, int marginSize = 0, int borderBits = 1) const;
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:
/**
* @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 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.
* @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, InputArray ids);
/**
* @overload
* @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 firstMarker id of first marker in dictionary to use on board.
* @return the output GridBoard object
*/
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();
};
/**
@ -156,40 +124,19 @@ protected:
* The benefits of ChArUco boards is that they provide both, ArUco markers versatility and chessboard corner precision,
* which is important for calibration and pose estimation. The board image can be drawn using generateImage() method.
*/
class CV_EXPORTS_W CharucoBoard : public Board {
protected:
CharucoBoard();
class CV_EXPORTS_W_SIMPLE CharucoBoard : public Board {
public:
/** @brief Draw a ChArUco board
/** @brief CharucoBoard constructor
*
* @param outSize size of the output image in pixels.
* @param img output image with the board. The size of this image will be outSize
* and the board will be on the center, keeping the board proportions.
* @param marginSize minimum margins (in pixels) of the board in the output image
* @param borderBits width of the marker borders.
*
* This function return the image of the ChArUco board, ready to be printed.
*/
CV_WRAP void generateImage(Size outSize, OutputArray img, int marginSize = 0, int borderBits = 1) const CV_OVERRIDE;
/** @brief Create a CharucoBoard object
*
* @param squaresX number of chessboard squares in X direction
* @param squaresY number of chessboard squares in Y direction
* @param squareLength chessboard square side length (normally in meters)
* @param size number of chessboard squares in x and y directions
* @param squareLength squareLength chessboard square side length (normally in meters)
* @param markerLength marker side length (same unit than squareLength)
* @param dictionary dictionary of markers indicating the type of markers.
* @param dictionary dictionary of markers indicating the type of markers
* @param ids array of id used markers
* The first markers in the dictionary are used to fill the white chessboard squares.
* @return the output CharucoBoard object
*
* This functions creates a CharucoBoard object given the number of squares in each direction
* and the size of the markers and chessboard squares.
*/
CV_WRAP static Ptr<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;
@ -220,10 +167,8 @@ public:
*/
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();
};
//! @}

29
modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp
View File

@ -10,28 +10,7 @@
namespace cv {
namespace aruco {
/** @defgroup aruco ArUco Marker Detection
* Square fiducial markers (also known as Augmented Reality Markers) are useful for easy,
* fast and robust camera pose estimation.
*
* The main functionality of ArucoDetector class is detection of markers in an image. There are even more
* functionalities implemented in the aruco contrib module (files aruco.hpp, charuco.hpp, aruco_calib.hpp):
* - Pose estimation from a single marker or from a board/set of markers
* - Detection of ChArUco board for high subpixel accuracy
* - Camera calibration from both, ArUco boards and ChArUco boards.
* - Detection of ChArUco diamond markers
* The functionalities from the aruco contrib module is planned to be transferred to the main repository.
*
* The implementation is based on the ArUco Library by R. Muñoz-Salinas and S. Garrido-Jurado @cite Aruco2014.
*
* Markers can also be detected based on the AprilTag 2 @cite wang2016iros fiducial detection method.
*
* @sa @cite Aruco2014
* This code has been originally developed by Sergio Garrido-Jurado as a project
* for Google Summer of Code 2015 (GSoC 15).
*/
//! @addtogroup aruco
//! @addtogroup objdetect_aruco
//! @{
enum CornerRefineMethod{
@ -294,7 +273,7 @@ public:
* @sa undistort, estimatePoseSingleMarkers, estimatePoseBoard
*/
CV_WRAP void detectMarkers(InputArray image, OutputArrayOfArrays corners, OutputArray ids,
OutputArrayOfArrays rejectedImgPoints = noArray());
OutputArrayOfArrays rejectedImgPoints = noArray()) const;
/** @brief Refind not detected markers based on the already detected and the board layout
*
@ -318,11 +297,11 @@ public:
* using projectPoint function. If not, missing marker projections are interpolated using global
* homography, and all the marker corners in the board must have the same Z coordinate.
*/
CV_WRAP void refineDetectedMarkers(InputArray image, const Ptr<Board> &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);

2
modules/objdetect/include/opencv2/objdetect/aruco_dictionary.hpp
View File

@ -9,7 +9,7 @@
namespace cv {
namespace aruco {
//! @addtogroup aruco
//! @addtogroup objdetect_aruco
//! @{

154
modules/objdetect/include/opencv2/objdetect/charuco_detector.hpp Normal file
View File

@ -0,0 +1,154 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#ifndef OPENCV_OBJDETECT_CHARUCO_DETECTOR_HPP
#define OPENCV_OBJDETECT_CHARUCO_DETECTOR_HPP
#include "opencv2/objdetect/aruco_detector.hpp"
namespace cv {
namespace aruco {
//! @addtogroup objdetect_aruco
//! @{
struct CV_EXPORTS_W_SIMPLE CharucoParameters {
CharucoParameters() {
minMarkers = 2;
tryRefineMarkers = false;
}
/// cameraMatrix optional 3x3 floating-point camera matrix
CV_PROP_RW Mat cameraMatrix;
/// distCoeffs optional vector of distortion coefficients
CV_PROP_RW Mat distCoeffs;
/// minMarkers number of adjacent markers that must be detected to return a charuco corner, default = 2
CV_PROP_RW int minMarkers;
/// try to use refine board, default false
CV_PROP_RW bool tryRefineMarkers;
};
class CV_EXPORTS_W CharucoDetector : public Algorithm {
public:
/** @brief Basic CharucoDetector constructor
*
* @param board ChAruco board
* @param charucoParams charuco detection parameters
* @param detectorParams marker detection parameters
* @param refineParams marker refine detection parameters
*/
CV_WRAP CharucoDetector(const CharucoBoard& board,
const CharucoParameters& charucoParams = CharucoParameters(),
const DetectorParameters &detectorParams = DetectorParameters(),
const RefineParameters& refineParams = RefineParameters());
CV_WRAP const CharucoBoard& getBoard() const;
CV_WRAP void setBoard(const CharucoBoard& board);
CV_WRAP const CharucoParameters& getCharucoParameters() const;
CV_WRAP void setCharucoParameters(CharucoParameters& charucoParameters);
CV_WRAP const DetectorParameters& getDetectorParameters() const;
CV_WRAP void setDetectorParameters(const DetectorParameters& detectorParameters);
CV_WRAP const RefineParameters& getRefineParameters() const;
CV_WRAP void setRefineParameters(const RefineParameters& refineParameters);
/**
* @brief detect aruco markers and interpolate position of ChArUco board corners
* @param image input image necesary for corner refinement. Note that markers are not detected and
* should be sent in corners and ids parameters.
* @param charucoCorners interpolated chessboard corners.
* @param charucoIds interpolated chessboard corners identifiers.
* @param markerCorners vector of already detected markers corners. For each marker, its four
* corners are provided, (e.g std::vector<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

32
modules/objdetect/misc/java/test/ArucoTest.java
View File

@ -4,8 +4,10 @@ import java.util.ArrayList;
import java.util.List;
import org.opencv.test.OpenCVTestCase;
import org.junit.Assert;
import org.opencv.core.Scalar;
import org.opencv.core.Mat;
import org.opencv.core.MatOfInt;
import org.opencv.core.Size;
import org.opencv.core.CvType;
import org.opencv.objdetect.*;
@ -29,7 +31,7 @@ public class ArucoTest extends OpenCVTestCase {
Mat ids = new Mat(1, 1, CvType.CV_32SC1);
ids.put(row, col, 0);
Board board = Board.create(objPoints, dictionary, ids);
Board board = new Board(objPoints, dictionary, ids);
Mat image = new Mat();
board.generateImage(new Size(80, 80), image, 2);
@ -80,4 +82,32 @@ public class ArucoTest extends OpenCVTestCase {
assertArrayEquals(new double[]{size + offset - 1, size + offset - 1}, res.get(0, 2), 0.0);
assertArrayEquals(new double[]{offset, size + offset - 1}, res.get(0, 3), 0.0);
}
public void testCharucoDetector() {
Dictionary dictionary = Objdetect.getPredefinedDictionary(0);
int boardSizeX = 3, boardSizeY = 3;
CharucoBoard board = new CharucoBoard(new Size(boardSizeX, boardSizeY), 1.f, 0.8f, dictionary);
CharucoDetector charucoDetector = new CharucoDetector(board);
int cellSize = 80;
Mat boardImage = new Mat();
board.generateImage(new Size(cellSize*boardSizeX, cellSize*boardSizeY), boardImage);
assertTrue(boardImage.total() > 0);
Mat charucoCorners = new Mat();
Mat charucoIds = new Mat();
charucoDetector.detectBoard(boardImage, charucoCorners, charucoIds);
assertEquals(4, charucoIds.total());
int[] intCharucoIds = (new MatOfInt(charucoIds)).toArray();
Assert.assertArrayEquals(new int[]{0, 1, 2, 3}, intCharucoIds);
double eps = 0.2;
assertArrayEquals(new double[]{cellSize, cellSize}, charucoCorners.get(0, 0), eps);
assertArrayEquals(new double[]{2*cellSize, cellSize}, charucoCorners.get(1, 0), eps);
assertArrayEquals(new double[]{cellSize, 2*cellSize}, charucoCorners.get(2, 0), eps);
assertArrayEquals(new double[]{2*cellSize, 2*cellSize}, charucoCorners.get(3, 0), eps);
}
}

62
modules/objdetect/misc/python/test/test_objdetect_aruco.py
View File

@ -3,7 +3,7 @@
# Python 2/3 compatibility
from __future__ import print_function
import os, numpy as np
import os, tempfile, numpy as np
import cv2 as cv
@ -17,14 +17,14 @@ class aruco_objdetect_test(NewOpenCVTests):
rev_ids = ids[::-1]
aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_5X5_250)
board = cv.aruco.CharucoBoard_create(7, 5, 1, 0.5, aruco_dict)
board = cv.aruco.CharucoBoard((7, 5), 1, 0.5, aruco_dict)
np.testing.assert_array_equal(board.getIds().squeeze(), ids)
board = cv.aruco.CharucoBoard_create(7, 5, 1, 0.5, aruco_dict, rev_ids)
board = cv.aruco.CharucoBoard((7, 5), 1, 0.5, aruco_dict, rev_ids)
np.testing.assert_array_equal(board.getIds().squeeze(), rev_ids)
board = cv.aruco.CharucoBoard_create(7, 5, 1, 0.5, aruco_dict, ids)
board = cv.aruco.CharucoBoard((7, 5), 1, 0.5, aruco_dict, ids)
np.testing.assert_array_equal(board.getIds().squeeze(), ids)
def test_identify(self):
@ -72,7 +72,7 @@ class aruco_objdetect_test(NewOpenCVTests):
aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_250)
aruco_detector = cv.aruco.ArucoDetector(aruco_dict, aruco_params)
board_size = (3, 4)
board = cv.aruco.GridBoard_create(board_size[0], board_size[1], 5.0, 1.0, aruco_dict)
board = cv.aruco.GridBoard(board_size, 5.0, 1.0, aruco_dict)
board_image = board.generateImage((board_size[0]*50, board_size[1]*50), marginSize=10)
corners, ids, rejected = aruco_detector.detectMarkers(board_image)
@ -90,5 +90,57 @@ class aruco_objdetect_test(NewOpenCVTests):
self.assertEqual((1, 4, 2), refine_corners[0].shape)
np.testing.assert_array_equal(corners, refine_corners)
def test_write_read_dictionary(self):
try:
aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_5X5_50)
markers_gold = aruco_dict.bytesList
# write aruco_dict
fd, filename = tempfile.mkstemp(prefix="opencv_python_aruco_dict_", suffix=".yml")
os.close(fd)
fs_write = cv.FileStorage(filename, cv.FileStorage_WRITE)
aruco_dict.writeDictionary(fs_write)
fs_write.release()
# reset aruco_dict
aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_6X6_250)
# read aruco_dict
fs_read = cv.FileStorage(filename, cv.FileStorage_READ)
aruco_dict.readDictionary(fs_read.root())
fs_read.release()
# check equal
self.assertEqual(aruco_dict.markerSize, 5)
self.assertEqual(aruco_dict.maxCorrectionBits, 3)
np.testing.assert_array_equal(aruco_dict.bytesList, markers_gold)
finally:
if os.path.exists(filename):
os.remove(filename)
def test_charuco_detector(self):
aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_250)
board_size = (3, 3)
board = cv.aruco.CharucoBoard(board_size, 1.0, .8, aruco_dict)
charuco_detector = cv.aruco.CharucoDetector(board)
cell_size = 100
image = board.generateImage((cell_size*board_size[0], cell_size*board_size[1]))
list_gold_corners = []
for i in range(1, board_size[0]):
for j in range(1, board_size[1]):
list_gold_corners.append((j*cell_size, i*cell_size))
gold_corners = np.array(list_gold_corners, dtype=np.float32)
charucoCorners, charucoIds, markerCorners, markerIds = charuco_detector.detectBoard(image)
self.assertEqual(len(charucoIds), 4)
for i in range(0, 4):
self.assertEqual(charucoIds[i], i)
np.testing.assert_allclose(gold_corners, charucoCorners.reshape(-1, 2), 0.01, 0.1)
if __name__ == '__main__':
NewOpenCVTests.bootstrap()

285
modules/objdetect/perf/perf_aruco.cpp Normal file
View File

@ -0,0 +1,285 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#include "perf_precomp.hpp"
#include "opencv2/calib3d.hpp"
namespace opencv_test {
using namespace perf;
typedef tuple<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();
}
}

546
modules/objdetect/src/aruco/aruco_board.cpp
View File

@ -3,6 +3,8 @@
// 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>
@ -10,72 +12,60 @@ namespace cv {
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)
{}
virtual ~Impl() {}
Impl(const Impl&) = delete;
Impl& operator=(const Impl&) = delete;
virtual void matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray _objPoints,
OutputArray imgPoints) const;
virtual void generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const;
};
Board::Board(): boardImpl(makePtr<BoardImpl>()) {}
void Board::Impl::matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray _objPoints,
OutputArray imgPoints) const {
Board::~Board() {}
CV_Assert(ids.size() == objPoints.size());
CV_Assert(detectedIds.total() == detectedCorners.total());
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);
size_t nDetectedMarkers = detectedIds.total();
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);
vector<Point3f> objPnts;
objPnts.reserve(nDetectedMarkers);
if (corners_mat.type() == CV_32FC1)
corners_mat = corners_mat.reshape(3);
CV_Assert(corners_mat.total() == 4);
vector<Point2f> imgPnts;
imgPnts.reserve(nDetectedMarkers);
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);
// 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]);
}
}
}
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;
// create output
Mat(objPnts).copyTo(_objPoints);
Mat(imgPnts).copyTo(imgPoints);
}
const Dictionary& Board::getDictionary() const {
return this->boardImpl->dictionary;
}
const vector<vector<Point3f> >& Board::getObjPoints() const {
return this->boardImpl->objPoints;
}
const Point3f& Board::getRightBottomCorner() const {
return this->boardImpl->rightBottomBorder;
}
const vector<int>& Board::getIds() const {
return this->boardImpl->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 {
void Board::Impl::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const {
CV_Assert(!outSize.empty());
CV_Assert(marginSize >= 0);
@ -85,17 +75,17 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
// calculate max and min values in XY plane
CV_Assert(this->getObjPoints().size() > 0);
CV_Assert(objPoints.size() > 0);
float minX, maxX, minY, maxY;
minX = maxX = this->getObjPoints()[0][0].x;
minY = maxY = this->getObjPoints()[0][0].y;
minX = maxX = objPoints[0][0].x;
minY = maxY = objPoints[0][0].y;
for(unsigned int i = 0; i < this->getObjPoints().size(); i++) {
for(unsigned int i = 0; i < objPoints.size(); i++) {
for(int j = 0; j < 4; j++) {
minX = min(minX, this->getObjPoints()[i][j].x);
maxX = max(maxX, this->getObjPoints()[i][j].x);
minY = min(minY, this->getObjPoints()[i][j].y);
maxY = max(maxY, this->getObjPoints()[i][j].y);
minX = min(minX, objPoints[i][j].x);
maxX = max(maxX, objPoints[i][j].x);
minY = min(minY, objPoints[i][j].y);
maxY = max(maxY, objPoints[i][j].y);
}
}
@ -121,10 +111,10 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
Mat marker;
Point2f outCorners[3];
Point2f inCorners[3];
for(unsigned int m = 0; m < this->getObjPoints().size(); m++) {
for(unsigned int m = 0; m < objPoints.size(); m++) {
// transform corners to markerZone coordinates
for(int j = 0; j < 3; j++) {
Point2f pf = Point2f(this->getObjPoints()[m][j].x, this->getObjPoints()[m][j].y);
Point2f pf = Point2f(objPoints[m][j].x, objPoints[m][j].y);
// move top left to 0, 0
pf -= Point2f(minX, minY);
pf.x = pf.x / sizeX * float(out.cols);
@ -135,7 +125,7 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
// get marker
Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
getDictionary().generateImageMarker(this->getIds()[m], dst_sz.width, marker, borderBits);
dictionary.generateImageMarker(ids[m], dst_sz.width, marker, borderBits);
if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
// marker is aligned to image axes
@ -155,70 +145,119 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
}
}
void Board::matchImagePoints(InputArray detectedCorners, InputArray detectedIds,
OutputArray _objPoints, OutputArray imgPoints) const {
CV_Assert(getIds().size() == getObjPoints().size());
CV_Assert(detectedIds.total() == detectedCorners.total());
size_t nDetectedMarkers = detectedIds.total();
vector<Point3f> objPnts;
objPnts.reserve(nDetectedMarkers);
vector<Point2f> imgPnts;
imgPnts.reserve(nDetectedMarkers);
// 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]);
}
}
}
}
// create output
Mat(objPnts).copyTo(_objPoints);
Mat(imgPnts).copyTo(imgPoints);
Board::Board(const Ptr<Impl>& _impl):
impl(_impl)
{
CV_Assert(impl);
}
struct GridBoard::GridImpl {
GridImpl(){};
Board::Board():
impl(nullptr)
{}
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<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);
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);
}
ids.copyTo(impl->ids);
impl->objPoints = obj_points_vector;
impl->rightBottomBorder = rightBottomBorder;
}
const Dictionary& Board::getDictionary() const {
CV_Assert(this->impl);
return this->impl->dictionary;
}
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
int sizeX = 3, sizeY = 3;
const Size size;
// marker side length (normally in meters)
float markerLength = 1.f;
float markerLength;
// separation between markers in the grid
float markerSeparation = .5f;
float markerSeparation;
};
GridBoard::GridBoard(): gridImpl(makePtr<GridImpl>()) {}
GridBoard::GridBoard() {}
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;
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) markersX * markersY;
CV_Assert(totalMarkers == ids.total());
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);
@ -228,67 +267,141 @@ Ptr<GridBoard> GridBoard::create(int markersX, int markersY, float markerLength,
objPoints.push_back(corners);
}
}
res->boardImpl->objPoints = objPoints;
res->boardImpl->rightBottomBorder = Point3f(markersX * markerLength + markerSeparation * (markersX - 1),
markersY * markerLength + markerSeparation * (markersY - 1), 0.f);
return res;
}
Ptr<GridBoard> 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);
@ -308,21 +421,21 @@ void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize,
// determine the margins to draw only the markers
// take the minimum just to be sure
double squareSizePixels = min(double(chessboardZoneImg.cols) / double(charucoImpl->sizeX),
double(chessboardZoneImg.rows) / double(charucoImpl->sizeY));
double squareSizePixels = min(double(chessboardZoneImg.cols) / double(size.width),
double(chessboardZoneImg.rows) / double(size.height));
double diffSquareMarkerLength = (charucoImpl->squareLength - charucoImpl->markerLength) / 2;
double diffSquareMarkerLength = (squareLength - markerLength) / 2;
int diffSquareMarkerLengthPixels =
int(diffSquareMarkerLength * squareSizePixels / charucoImpl->squareLength);
int(diffSquareMarkerLength * squareSizePixels / squareLength);
// draw markers
Mat markersImg;
Board::generateImage(chessboardZoneImg.size(), markersImg, diffSquareMarkerLengthPixels, borderBits);
Board::Impl::generateImage(chessboardZoneImg.size(), markersImg, diffSquareMarkerLengthPixels, borderBits);
markersImg.copyTo(chessboardZoneImg);
// now draw black squares
for(int y = 0; y < charucoImpl->sizeY; y++) {
for(int x = 0; x < charucoImpl->sizeX; x++) {
for(int y = 0; y < size.height; y++) {
for(int x = 0; x < size.width; x++) {
if(y % 2 != x % 2) continue; // white corner, dont do anything
@ -338,78 +451,22 @@ void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize,
}
}
/**
* Fill nearestMarkerIdx and nearestMarkerCorners arrays
*/
void CharucoBoard::CharucoImpl::_getNearestMarkerCorners(CharucoBoard &board, float squareLength) {
board.charucoImpl->nearestMarkerIdx.resize(board.charucoImpl->chessboardCorners.size());
board.charucoImpl->nearestMarkerCorners.resize(board.charucoImpl->chessboardCorners.size());
CharucoBoard::CharucoBoard(){}
unsigned int nMarkers = (unsigned int)board.getIds().size();
unsigned int nCharucoCorners = (unsigned int)board.charucoImpl->chessboardCorners.size();
for(unsigned int i = 0; i < nCharucoCorners; i++) {
double minDist = -1; // distance of closest markers
Point3f charucoCorner = board.charucoImpl->chessboardCorners[i];
for(unsigned int j = 0; j < nMarkers; j++) {
// calculate distance from marker center to charuco corner
Point3f center = Point3f(0, 0, 0);
for(unsigned int k = 0; k < 4; k++)
center += board.getObjPoints()[j][k];
center /= 4.;
double sqDistance;
Point3f distVector = charucoCorner - center;
sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * squareLength, 2)) {
// if same minimum distance (or first iteration), add to nearestMarkerIdx vector
board.charucoImpl->nearestMarkerIdx[i].push_back(j);
minDist = sqDistance;
} else if(sqDistance < minDist) {
// if finding a closest marker to the charuco corner
board.charucoImpl->nearestMarkerIdx[i].clear(); // remove any previous added marker
board.charucoImpl->nearestMarkerIdx[i].push_back(j); // add the new closest marker index
minDist = sqDistance;
}
}
// for each of the closest markers, search the marker corner index closer
// to the charuco corner
for(unsigned int j = 0; j < board.charucoImpl->nearestMarkerIdx[i].size(); j++) {
board.charucoImpl->nearestMarkerCorners[i].resize(board.charucoImpl->nearestMarkerIdx[i].size());
double minDistCorner = -1;
for(unsigned int k = 0; k < 4; k++) {
double sqDistance;
Point3f distVector = charucoCorner - board.getObjPoints()[board.charucoImpl->nearestMarkerIdx[i][j]][k];
sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
if(k == 0 || sqDistance < minDistCorner) {
// if this corner is closer to the charuco corner, assing its index
// to nearestMarkerCorners
minDistCorner = sqDistance;
board.charucoImpl->nearestMarkerCorners[i][j] = k;
}
}
}
}
}
CharucoBoard::CharucoBoard(const Size& size, float squareLength, float markerLength,
const Dictionary &dictionary, InputArray ids):
Board(new CharucoBoardImpl(dictionary, size, squareLength, markerLength)) {
Ptr<CharucoBoard> 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);
CV_Assert(size.width > 1 && size.height > 1 && markerLength > 0 && squareLength > markerLength);
res->charucoImpl->sizeX = squaresX;
res->charucoImpl->sizeY = squaresY;
res->charucoImpl->squareLength = squareLength;
res->charucoImpl->markerLength = markerLength;
res->boardImpl->dictionary = dictionary;
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
@ -422,48 +479,60 @@ Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareL
objPoints.push_back(corners);
// first ids in dictionary
if (totalMarkers == 0)
res->boardImpl->ids.push_back(nextId);
impl->ids.push_back(nextId);
nextId++;
}
}
if (totalMarkers > 0 && nextId != totalMarkers)
CV_Error(cv::Error::StsBadSize, "Size of ids must be equal to the number of markers: "+std::to_string(nextId));
res->boardImpl->objPoints = objPoints;
impl->objPoints = objPoints;
// now fill chessboardCorners
for(int y = 0; y < squaresY - 1; y++) {
for(int x = 0; x < squaresX - 1; x++) {
std::vector<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);
@ -477,8 +546,8 @@ bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const {
double dotProduct;
for (unsigned int i = 2; i < nCharucoCorners; i++){
testPoint(0) = charucoImpl->chessboardCorners[charucoIds.getMat().at<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);
@ -492,15 +561,18 @@ bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const {
}
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;
}
}

78
modules/objdetect/src/aruco/aruco_detector.cpp
View File

@ -856,7 +856,7 @@ ArucoDetector::ArucoDetector(const Dictionary &_dictionary,
}
void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corners, OutputArray _ids,
OutputArrayOfArrays _rejectedImgPoints) {
OutputArrayOfArrays _rejectedImgPoints) const {
CV_Assert(!_image.empty());
DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams;
const Dictionary& dictionary = arucoDetectorImpl->dictionary;
@ -994,37 +994,25 @@ void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corner
/**
* Project board markers that are not included in the list of detected markers
*/
static void _projectUndetectedMarkers(const Ptr<Board> &_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;
}
@ -1033,31 +1021,31 @@ static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArray
// 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
@ -1067,14 +1055,14 @@ static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArray
// 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;
@ -1084,9 +1072,9 @@ static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArray
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;
@ -1103,10 +1091,10 @@ static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArray
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;

521
modules/objdetect/src/aruco/charuco_detector.cpp Normal file
View File

@ -0,0 +1,521 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#include "../precomp.hpp"
#include <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);
}
}
}
}
}

205
modules/objdetect/test/test_aruco_utils.cpp Normal file
View File

@ -0,0 +1,205 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
#include "test_aruco_utils.hpp"
namespace opencv_test {
vector<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);
}
}

42
modules/objdetect/test/test_aruco_utils.hpp Normal file
View File

@ -0,0 +1,42 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
#include "opencv2/calib3d.hpp"
namespace opencv_test {
static inline double deg2rad(double deg) { return deg * CV_PI / 180.; }
vector<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);
}

321
modules/objdetect/test/test_boarddetection.cpp Normal file
View File

@ -0,0 +1,321 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
#include "test_aruco_utils.hpp"
namespace opencv_test { namespace {
enum class ArucoAlgParams
{
USE_DEFAULT = 0,
USE_ARUCO3 = 1
};
/**
* @brief Check pose estimation of aruco board
*/
class CV_ArucoBoardPose : public cvtest::BaseTest {
public:
CV_ArucoBoardPose(ArucoAlgParams arucoAlgParams)
{
aruco::DetectorParameters params;
aruco::Dictionary dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250);
params.minDistanceToBorder = 3;
if (arucoAlgParams == ArucoAlgParams::USE_ARUCO3) {
params.useAruco3Detection = true;
params.cornerRefinementMethod = aruco::CORNER_REFINE_SUBPIX;
params.minSideLengthCanonicalImg = 16;
params.errorCorrectionRate = 0.8;
}
detector = aruco::ArucoDetector(dictionary, params);
}
protected:
aruco::ArucoDetector detector;
void run(int);
};
void CV_ArucoBoardPose::run(int) {
int iter = 0;
Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1);
Size imgSize(500, 500);
cameraMatrix.at< double >(0, 0) = cameraMatrix.at< double >(1, 1) = 650;
cameraMatrix.at< double >(0, 2) = imgSize.width / 2;
cameraMatrix.at< double >(1, 2) = imgSize.height / 2;
Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0));
const int sizeX = 3, sizeY = 3;
aruco::DetectorParameters detectorParameters = detector.getDetectorParameters();
// for different perspectives
for(double distance = 0.2; distance <= 0.4; distance += 0.15) {
for(int yaw = -55; yaw <= 50; yaw += 25) {
for(int pitch = -55; pitch <= 50; pitch += 25) {
vector<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

659
modules/objdetect/test/test_charucodetection.cpp Normal file
View File

@ -0,0 +1,659 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
#include "test_aruco_utils.hpp"
namespace opencv_test { namespace {
/**
* @brief Get a synthetic image of Chessboard in perspective
*/
static Mat projectChessboard(int squaresX, int squaresY, float squareSize, Size imageSize,
Mat cameraMatrix, Mat rvec, Mat tvec) {
Mat img(imageSize, CV_8UC1, Scalar::all(255));
Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0));
for(int y = 0; y < squaresY; y++) {
float startY = float(y) * squareSize;
for(int x = 0; x < squaresX; x++) {
if(y % 2 != x % 2) continue;
float startX = float(x) * squareSize;
vector< Point3f > squareCorners;
squareCorners.push_back(Point3f(startX, startY, 0) - Point3f(squaresX*squareSize/2.f, squaresY*squareSize/2.f, 0.f));
squareCorners.push_back(squareCorners[0] + Point3f(squareSize, 0, 0));
squareCorners.push_back(squareCorners[0] + Point3f(squareSize, squareSize, 0));
squareCorners.push_back(squareCorners[0] + Point3f(0, squareSize, 0));
vector< vector< Point2f > > projectedCorners;
projectedCorners.push_back(vector< Point2f >());
projectPoints(squareCorners, rvec, tvec, cameraMatrix, distCoeffs, projectedCorners[0]);
vector< vector< Point > > projectedCornersInt;
projectedCornersInt.push_back(vector< Point >());
for(int k = 0; k < 4; k++)
projectedCornersInt[0]
.push_back(Point((int)projectedCorners[0][k].x, (int)projectedCorners[0][k].y));
fillPoly(img, projectedCornersInt, Scalar::all(0));
}
}
return img;
}
/**
* @brief Check pose estimation of charuco board
*/
static Mat projectCharucoBoard(aruco::CharucoBoard& board, Mat cameraMatrix, double yaw,
double pitch, double distance, Size imageSize, int markerBorder,
Mat &rvec, Mat &tvec) {
getSyntheticRT(yaw, pitch, distance, rvec, tvec);
// project markers
Mat img = Mat(imageSize, CV_8UC1, Scalar::all(255));
for(unsigned int indexMarker = 0; indexMarker < board.getIds().size(); indexMarker++) {
projectMarker(img, board, indexMarker, cameraMatrix, rvec, tvec, markerBorder);
}
// project chessboard
Mat chessboard =
projectChessboard(board.getChessboardSize().width, board.getChessboardSize().height,
board.getSquareLength(), imageSize, cameraMatrix, rvec, tvec);
for(unsigned int i = 0; i < chessboard.total(); i++) {
if(chessboard.ptr< unsigned char >()[i] == 0) {
img.ptr< unsigned char >()[i] = 0;
}
}
return img;
}
/**
* @brief Check Charuco detection
*/
class CV_CharucoDetection : public cvtest::BaseTest {
public:
CV_CharucoDetection();
protected:
void run(int);
};
CV_CharucoDetection::CV_CharucoDetection() {}
void CV_CharucoDetection::run(int) {
int iter = 0;
Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1);
Size imgSize(500, 500);
aruco::DetectorParameters params;
params.minDistanceToBorder = 3;
aruco::CharucoBoard board(Size(4, 4), 0.03f, 0.015f, aruco::getPredefinedDictionary(aruco::DICT_6X6_250));
aruco::CharucoDetector detector(board, aruco::CharucoParameters(), params);
cameraMatrix.at<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