mirror of https://github.com/opencv/opencv.git
Merge pull request #22986 from AleksandrPanov:move_contrib_charuco_to_main_objdetect
merge with https://github.com/opencv/opencv_contrib/pull/3394 move Charuco API from contrib to main repo: - add CharucoDetector: ``` CharucoDetector::detectBoard(InputArray image, InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds, OutputArray charucoCorners, OutputArray charucoIds) const // detect charucoCorners and/or markerCorners CharucoDetector::detectDiamonds(InputArray image, InputOutputArrayOfArrays _markerCorners, InputOutputArrayOfArrays _markerIds, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds) const ``` - add `matchImagePoints()` for `CharucoBoard` - remove contrib aruco dependencies from interactive-calibration tool - move almost all aruco tests to objdetect ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMakepull/23053/head
Alexander Panov
2 years ago
committed by
GitHub
parent
9627ab9462
commit
121034876d
21 changed files with 2715 additions and 493 deletions
@ -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}) |
||||
|
||||
@ -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 |
||||
@ -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(); |
||||
} |
||||
|
||||
} |
||||
@ -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); |
||||
} |
||||
} |
||||
} |
||||
|
||||
} |
||||
} |
||||
@ -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); |
||||
} |
||||
|
||||
} |
||||
@ -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); |
||||
|
||||
} |
||||
@ -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
|
||||
@ -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
|
||||
Loading…
Reference in new issue