From c759a7cddea6b2f316440e607fc538062376dd2f Mon Sep 17 00:00:00 2001 From: Benjamin Knecht Date: Mon, 17 Feb 2025 16:49:39 +0100 Subject: [PATCH] Extend ArUcoDetector to run multiple dictionaries in an efficient manner. * Add constructor for multiple dictionaries * Add get/set/remove/add functions for multiple dictionaries * Add unit tests TESTED=unit tests --- .../opencv2/objdetect/aruco_detector.hpp | 26 ++- .../objdetect/src/aruco/aruco_detector.cpp | 177 +++++++++++++----- .../objdetect/test/test_arucodetection.cpp | 88 +++++++++ 3 files changed, 237 insertions(+), 54 deletions(-) diff --git a/modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp b/modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp index 9d30d55d17..0d3eb69647 100644 --- a/modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp +++ b/modules/objdetect/include/opencv2/objdetect/aruco_detector.hpp @@ -285,6 +285,16 @@ public: const DetectorParameters &detectorParams = DetectorParameters(), const RefineParameters& refineParams = RefineParameters()); + /** @brief ArucoDetector constructor for multiple dictionaries + * + * @param dictionaries indicates the type of markers that will be searched + * @param detectorParams marker detection parameters + * @param refineParams marker refine detection parameters + */ + CV_WRAP ArucoDetector(const std::vector &dictionaries, + const DetectorParameters &detectorParams = DetectorParameters(), + const RefineParameters& refineParams = RefineParameters()); + /** @brief Basic marker detection * * @param image input image @@ -296,8 +306,10 @@ public: * The identifiers have the same order than the markers in the imgPoints array. * @param rejectedImgPoints contains the imgPoints of those squares whose inner code has not a * correct codification. Useful for debugging purposes. + * @param dictIndices vector of dictionary indices for each detected marker. Use getDictionaries() to get the + * list of corresponding dictionaries. * - * Performs marker detection in the input image. Only markers included in the specific dictionary + * Performs marker detection in the input image. Only markers included in the specific dictionaries * are searched. For each detected marker, it returns the 2D position of its corner in the image * and its corresponding identifier. * Note that this function does not perform pose estimation. @@ -306,7 +318,7 @@ public: * @sa undistort, estimatePoseSingleMarkers, estimatePoseBoard */ CV_WRAP void detectMarkers(InputArray image, OutputArrayOfArrays corners, OutputArray ids, - OutputArrayOfArrays rejectedImgPoints = noArray()) const; + OutputArrayOfArrays rejectedImgPoints = noArray(), OutputArray dictIndices = noArray()) const; /** @brief Refine not detected markers based on the already detected and the board layout * @@ -329,6 +341,8 @@ public: * If camera parameters and distortion coefficients are provided, missing markers are reprojected * using projectPoint function. If not, missing marker projections are interpolated using global * homography, and all the marker corners in the board must have the same Z coordinate. + * @note This function assumes that the board only contains markers from one dictionary, so only the + * first configured dictionary is used. */ CV_WRAP void refineDetectedMarkers(InputArray image, const Board &board, InputOutputArrayOfArrays detectedCorners, @@ -336,8 +350,12 @@ public: InputArray cameraMatrix = noArray(), InputArray distCoeffs = noArray(), OutputArray recoveredIdxs = noArray()) const; - CV_WRAP const Dictionary& getDictionary() const; - CV_WRAP void setDictionary(const Dictionary& dictionary); + CV_WRAP const Dictionary& getDictionary(size_t index = 0) const; + CV_WRAP void setDictionary(const Dictionary& dictionary, size_t index = 0); + CV_WRAP const std::vector& getDictionaries() const; + CV_WRAP void setDictionaries(const std::vector& dictionaries); + CV_WRAP void addDictionary(const Dictionary& dictionary); + CV_WRAP void removeDictionary(size_t index); CV_WRAP const DetectorParameters& getDetectorParameters() const; CV_WRAP void setDetectorParameters(const DetectorParameters& detectorParameters); diff --git a/modules/objdetect/src/aruco/aruco_detector.cpp b/modules/objdetect/src/aruco/aruco_detector.cpp index a1b04c8181..2c9ddb9160 100644 --- a/modules/objdetect/src/aruco/aruco_detector.cpp +++ b/modules/objdetect/src/aruco/aruco_detector.cpp @@ -10,6 +10,7 @@ #include "apriltag/apriltag_quad_thresh.hpp" #include "aruco_utils.hpp" #include +#include namespace cv { namespace aruco { @@ -641,8 +642,8 @@ static inline void findCornerInPyrImage(const float scale_init, const int closes } struct ArucoDetector::ArucoDetectorImpl { - /// dictionary indicates the type of markers that will be searched - Dictionary dictionary; + /// dictionaries indicates the types of markers that will be searched + std::vector dictionaries; /// marker detection parameters, check DetectorParameters docs to see available settings DetectorParameters detectorParams; @@ -651,8 +652,8 @@ struct ArucoDetector::ArucoDetectorImpl { RefineParameters refineParams; ArucoDetectorImpl() {} - ArucoDetectorImpl(const Dictionary &_dictionary, const DetectorParameters &_detectorParams, - const RefineParameters& _refineParams): dictionary(_dictionary), + ArucoDetectorImpl(const std::vector&_dictionaries, const DetectorParameters &_detectorParams, + const RefineParameters& _refineParams): dictionaries(_dictionaries), detectorParams(_detectorParams), refineParams(_refineParams) {} /** * @brief Detect square candidates in the input image @@ -671,14 +672,12 @@ struct ArucoDetector::ArucoDetectorImpl { * clear candidates and contours */ vector - filterTooCloseCandidates(vector > &candidates, vector > &contours) { + filterTooCloseCandidates(vector > &candidates, vector > &contours, int markerSize) { CV_Assert(detectorParams.minMarkerDistanceRate >= 0.); vector candidateTree(candidates.size()); for(size_t i = 0ull; i < candidates.size(); i++) { candidateTree[i] = MarkerCandidateTree(std::move(candidates[i]), std::move(contours[i])); } - candidates.clear(); - contours.clear(); // sort candidates from big to small std::stable_sort(candidateTree.begin(), candidateTree.end()); @@ -735,7 +734,7 @@ struct ArucoDetector::ArucoDetectorImpl { for (size_t i = 1ull; i < grouped.size(); i++) { size_t id = grouped[i]; float dist = getAverageDistance(candidateTree[id].corners, candidateTree[currId].corners); - float moduleSize = getAverageModuleSize(candidateTree[id].corners, dictionary.markerSize, detectorParams.markerBorderBits); + float moduleSize = getAverageModuleSize(candidateTree[id].corners, markerSize, detectorParams.markerBorderBits); if (dist > detectorParams.minGroupDistance*moduleSize) { currId = id; candidateTree[grouped[0]].closeContours.push_back(candidateTree[id]); @@ -770,7 +769,7 @@ struct ArucoDetector::ArucoDetectorImpl { */ void identifyCandidates(const Mat& grey, const vector& image_pyr, vector& selectedContours, vector >& accepted, vector >& contours, - vector& ids, OutputArrayOfArrays _rejected = noArray()) { + vector& ids, const Dictionary& currentDictionary, OutputArrayOfArrays _rejected = noArray()) { size_t ncandidates = selectedContours.size(); vector > rejected; @@ -807,11 +806,11 @@ struct ArucoDetector::ArucoDetectorImpl { } const float scale = detectorParams.useAruco3Detection ? img.cols / static_cast(grey.cols) : 1.f; - validCandidates[v] = _identifyOneCandidate(dictionary, img, selectedContours[v].corners, idsTmp[v], detectorParams, rotated[v], scale); + validCandidates[v] = _identifyOneCandidate(currentDictionary, img, selectedContours[v].corners, idsTmp[v], detectorParams, rotated[v], scale); if (validCandidates[v] == 0 && checkCloseContours) { for (const MarkerCandidate& closeMarkerCandidate: selectedContours[v].closeContours) { - validCandidates[v] = _identifyOneCandidate(dictionary, img, closeMarkerCandidate.corners, idsTmp[v], detectorParams, rotated[v], scale); + validCandidates[v] = _identifyOneCandidate(currentDictionary, img, closeMarkerCandidate.corners, idsTmp[v], detectorParams, rotated[v], scale); if (validCandidates[v] > 0) { selectedContours[v].corners = closeMarkerCandidate.corners; selectedContours[v].contour = closeMarkerCandidate.contour; @@ -864,14 +863,19 @@ struct ArucoDetector::ArucoDetectorImpl { ArucoDetector::ArucoDetector(const Dictionary &_dictionary, const DetectorParameters &_detectorParams, const RefineParameters& _refineParams) { - arucoDetectorImpl = makePtr(_dictionary, _detectorParams, _refineParams); + arucoDetectorImpl = makePtr(vector{_dictionary}, _detectorParams, _refineParams); +} + +ArucoDetector::ArucoDetector(const std::vector &_dictionaries, + const DetectorParameters &_detectorParams, + const RefineParameters& _refineParams) { + arucoDetectorImpl = makePtr(_dictionaries, _detectorParams, _refineParams); } void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corners, OutputArray _ids, - OutputArrayOfArrays _rejectedImgPoints) const { + OutputArrayOfArrays _rejectedImgPoints, OutputArray _dictIndices) const { CV_Assert(!_image.empty()); DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams; - const Dictionary& dictionary = arucoDetectorImpl->dictionary; CV_Assert(detectorParams.markerBorderBits > 0); // check that the parameters are set correctly if Aruco3 is used @@ -940,38 +944,66 @@ void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corner arucoDetectorImpl->detectCandidates(grey, candidates, contours); } - /// STEP 2.c FILTER OUT NEAR CANDIDATE PAIRS - auto selectedCandidates = arucoDetectorImpl->filterTooCloseCandidates(candidates, contours); + /// STEP 2.c FILTER OUT NEAR CANDIDATE PAIRS + unordered_set uniqueMarkerSizes; + for (const Dictionary& dictionary : arucoDetectorImpl->dictionaries) { + uniqueMarkerSizes.insert(dictionary.markerSize); + } + + // create at max 4 marker candidate trees for each dictionary size + vector> candidatesPerDictionarySize = {{}, {}, {}, {}}; + for (int markerSize : uniqueMarkerSizes) { + // min marker size is 4, so subtract 4 to get index + const auto dictionarySizeIndex = markerSize - 4; + // copy candidates + vector> candidatesCopy = candidates; + vector > contoursCopy = contours; + candidatesPerDictionarySize[dictionarySizeIndex] = arucoDetectorImpl->filterTooCloseCandidates(candidatesCopy, contoursCopy, markerSize); + } + candidates.clear(); + contours.clear(); /// STEP 2: Check candidate codification (identify markers) - arucoDetectorImpl->identifyCandidates(grey, grey_pyramid, selectedCandidates, candidates, contours, - ids, _rejectedImgPoints); + size_t dictIndex = 0; + vector dictIndices; + for (const Dictionary& currentDictionary : arucoDetectorImpl->dictionaries) { + const auto dictionarySizeIndex = currentDictionary.markerSize - 4; + // temporary variable to store the current candidates + vector> currentCandidates; + arucoDetectorImpl->identifyCandidates(grey, grey_pyramid, candidatesPerDictionarySize[dictionarySizeIndex], currentCandidates, contours, + ids, currentDictionary, _rejectedImgPoints); + if (_dictIndices.needed()) { + dictIndices.insert(dictIndices.end(), currentCandidates.size(), dictIndex); + } - /// STEP 3: Corner refinement :: use corner subpix - if (detectorParams.cornerRefinementMethod == (int)CORNER_REFINE_SUBPIX) { - CV_Assert(detectorParams.cornerRefinementWinSize > 0 && detectorParams.cornerRefinementMaxIterations > 0 && - detectorParams.cornerRefinementMinAccuracy > 0); - // Do subpixel estimation. In Aruco3 start on the lowest pyramid level and upscale the corners - parallel_for_(Range(0, (int)candidates.size()), [&](const Range& range) { - const int begin = range.start; - const int end = range.end; + /// STEP 3: Corner refinement :: use corner subpix + if (detectorParams.cornerRefinementMethod == (int)CORNER_REFINE_SUBPIX) { + CV_Assert(detectorParams.cornerRefinementWinSize > 0 && detectorParams.cornerRefinementMaxIterations > 0 && + detectorParams.cornerRefinementMinAccuracy > 0); + // Do subpixel estimation. In Aruco3 start on the lowest pyramid level and upscale the corners + parallel_for_(Range(0, (int)currentCandidates.size()), [&](const Range& range) { + const int begin = range.start; + const int end = range.end; - for (int i = begin; i < end; i++) { - if (detectorParams.useAruco3Detection) { - const float scale_init = (float) grey_pyramid[closest_pyr_image_idx].cols / grey.cols; - findCornerInPyrImage(scale_init, closest_pyr_image_idx, grey_pyramid, Mat(candidates[i]), detectorParams); + for (int i = begin; i < end; i++) { + if (detectorParams.useAruco3Detection) { + const float scale_init = (float) grey_pyramid[closest_pyr_image_idx].cols / grey.cols; + findCornerInPyrImage(scale_init, closest_pyr_image_idx, grey_pyramid, Mat(currentCandidates[i]), detectorParams); + } + else { + int cornerRefinementWinSize = std::max(1, cvRound(detectorParams.relativeCornerRefinmentWinSize* + getAverageModuleSize(currentCandidates[i], currentDictionary.markerSize, detectorParams.markerBorderBits))); + cornerRefinementWinSize = min(cornerRefinementWinSize, detectorParams.cornerRefinementWinSize); + cornerSubPix(grey, Mat(currentCandidates[i]), Size(cornerRefinementWinSize, cornerRefinementWinSize), Size(-1, -1), + TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS, + detectorParams.cornerRefinementMaxIterations, + detectorParams.cornerRefinementMinAccuracy)); + } } - else { - int cornerRefinementWinSize = std::max(1, cvRound(detectorParams.relativeCornerRefinmentWinSize* - getAverageModuleSize(candidates[i], dictionary.markerSize, detectorParams.markerBorderBits))); - cornerRefinementWinSize = min(cornerRefinementWinSize, detectorParams.cornerRefinementWinSize); - cornerSubPix(grey, Mat(candidates[i]), Size(cornerRefinementWinSize, cornerRefinementWinSize), Size(-1, -1), - TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS, - detectorParams.cornerRefinementMaxIterations, - detectorParams.cornerRefinementMinAccuracy)); - } - } - }); + }); + } + candidates.insert(candidates.end(), currentCandidates.begin(), currentCandidates.end()); + dictIndex++; } /// STEP 3, Optional : Corner refinement :: use contour container @@ -1001,6 +1033,12 @@ void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corner // copy to output arrays _copyVector2Output(candidates, _corners); Mat(ids).copyTo(_ids); + if (_dictIndices.needed()) { + _dictIndices.create(dictIndices.size(), 1, CV_32SC1); + Mat dictIndicesMat = _dictIndices.getMat(); + Mat m = cv::Mat1i(dictIndices).t(); + m.copyTo(dictIndicesMat); + } } /** @@ -1114,7 +1152,7 @@ void ArucoDetector::refineDetectedMarkers(InputArray _image, const Board& _board InputOutputArrayOfArrays _rejectedCorners, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _recoveredIdxs) const { DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams; - const Dictionary& dictionary = arucoDetectorImpl->dictionary; + const Dictionary& dictionary = arucoDetectorImpl->dictionaries[0]; RefineParameters& refineParams = arucoDetectorImpl->refineParams; CV_Assert(refineParams.minRepDistance > 0); @@ -1280,25 +1318,64 @@ void ArucoDetector::refineDetectedMarkers(InputArray _image, const Board& _board } } -void ArucoDetector::write(FileStorage &fs) const -{ - arucoDetectorImpl->dictionary.writeDictionary(fs); +void ArucoDetector::write(FileStorage &fs) const { + fs << "dictionaries" << "["; + for (auto& dictionary : arucoDetectorImpl->dictionaries) { + fs << "{"; + dictionary.writeDictionary(fs); + fs << "}"; + } + fs << "]"; arucoDetectorImpl->detectorParams.writeDetectorParameters(fs); arucoDetectorImpl->refineParams.writeRefineParameters(fs); } void ArucoDetector::read(const FileNode &fn) { - arucoDetectorImpl->dictionary.readDictionary(fn); + arucoDetectorImpl->dictionaries.clear(); + if (!fn.empty() && !fn["dictionaries"].empty() && fn["dictionaries"].isSeq()) { + for (const auto& dictionaryNode : fn["dictionaries"]) { + arucoDetectorImpl->dictionaries.emplace_back(); + arucoDetectorImpl->dictionaries.back().readDictionary(dictionaryNode); + } + } else { + // backward compatibility + arucoDetectorImpl->dictionaries.emplace_back(); + arucoDetectorImpl->dictionaries.back().readDictionary(fn); + } arucoDetectorImpl->detectorParams.readDetectorParameters(fn); arucoDetectorImpl->refineParams.readRefineParameters(fn); } -const Dictionary& ArucoDetector::getDictionary() const { - return arucoDetectorImpl->dictionary; +const Dictionary& ArucoDetector::getDictionary(size_t index) const { + CV_Assert(index < arucoDetectorImpl->dictionaries.size()); + return arucoDetectorImpl->dictionaries[index]; } -void ArucoDetector::setDictionary(const Dictionary& dictionary) { - arucoDetectorImpl->dictionary = dictionary; +void ArucoDetector::setDictionary(const Dictionary& dictionary, size_t index) { + // special case: if index is 0, we add the dictionary to the list to preserve the old behavior + CV_Assert(index == 0 || index < arucoDetectorImpl->dictionaries.size()); + if (index == 0 && arucoDetectorImpl->dictionaries.empty()) { + arucoDetectorImpl->dictionaries.push_back(dictionary); + } else { + arucoDetectorImpl->dictionaries.at(index) = dictionary; + } +} + +const vector& ArucoDetector::getDictionaries() const { + return arucoDetectorImpl->dictionaries; +} + +void ArucoDetector::setDictionaries(const vector& dictionaries) { + arucoDetectorImpl->dictionaries = dictionaries; +} + +void ArucoDetector::addDictionary(const Dictionary& dictionary) { + arucoDetectorImpl->dictionaries.push_back(dictionary); +} + +void ArucoDetector::removeDictionary(size_t index) { + CV_Assert(index < arucoDetectorImpl->dictionaries.size()); + arucoDetectorImpl->dictionaries.erase(arucoDetectorImpl->dictionaries.begin() + index); } const DetectorParameters& ArucoDetector::getDetectorParameters() const { diff --git a/modules/objdetect/test/test_arucodetection.cpp b/modules/objdetect/test/test_arucodetection.cpp index 7145b5d663..94e062eb56 100644 --- a/modules/objdetect/test/test_arucodetection.cpp +++ b/modules/objdetect/test/test_arucodetection.cpp @@ -638,6 +638,94 @@ TEST(CV_ArucoDetectMarkers, regression_contour_24220) } } +TEST(CV_ArucoMultiDict, addRemoveDictionary) +{ + aruco::ArucoDetector detector; + detector.addDictionary(aruco::getPredefinedDictionary(aruco::DICT_5X5_100)); + const auto& dicts = detector.getDictionaries(); + ASSERT_EQ(dicts.size(), 2ul); + EXPECT_EQ(dicts[0].markerSize, 4); + EXPECT_EQ(dicts[1].markerSize, 5); + detector.removeDictionary(0); + ASSERT_EQ(dicts.size(), 1ul); + EXPECT_EQ(dicts[0].markerSize, 5); + detector.removeDictionary(0); + EXPECT_EQ(dicts.size(), 0ul); + detector.addDictionary(aruco::getPredefinedDictionary(aruco::DICT_6X6_100)); + detector.addDictionary(aruco::getPredefinedDictionary(aruco::DICT_7X7_250)); + detector.addDictionary(aruco::getPredefinedDictionary(aruco::DICT_APRILTAG_25h9)); + ASSERT_EQ(dicts.size(), 3ul); + EXPECT_EQ(dicts[0].markerSize, 6); + EXPECT_EQ(dicts[1].markerSize, 7); + EXPECT_EQ(dicts[2].markerSize, 5); + detector.setDictionary(aruco::getPredefinedDictionary(aruco::DICT_APRILTAG_36h10), 1); + auto dict = detector.getDictionary(); + EXPECT_EQ(dict.markerSize, 6); + detector.setDictionary(aruco::getPredefinedDictionary(aruco::DICT_APRILTAG_16h5)); + ASSERT_EQ(dicts.size(), 3ul); + EXPECT_EQ(dicts[0].markerSize, 4); + EXPECT_EQ(dicts[1].markerSize, 6); + EXPECT_EQ(dicts[2].markerSize, 5); +} + + +TEST(CV_ArucoMultiDict, noDict) +{ + aruco::ArucoDetector detector; + detector.removeDictionary(0); + + vector > markerCorners; + vector markerIds; + + string img_path = cvtest::findDataFile("aruco/singlemarkersoriginal.jpg"); + Mat image = imread(img_path); + + detector.detectMarkers(image, markerCorners, markerIds); + + EXPECT_EQ(markerIds.size(), 0u); +} + + +TEST(CV_ArucoMultiDict, multiMarkerDetection) +{ + aruco::ArucoDetector detector; + detector.removeDictionary(0); + + const int markerSidePixels = 100; + const int imageSize = markerSidePixels * 2 + 3 * (markerSidePixels / 2); + + // draw synthetic image + Mat img = Mat(imageSize, imageSize, CV_8UC1, Scalar::all(255)); + for(int y = 0; y < 2; y++) { + for(int x = 0; x < 2; x++) { + Mat marker; + int id = y * 2 + x; + int dictId = x * 4 + y * 8; + auto dict = aruco::getPredefinedDictionary(dictId); + detector.addDictionary(dict); + aruco::generateImageMarker(dict, id, markerSidePixels, marker); + Point2f firstCorner = + Point2f(markerSidePixels / 2.f + x * (1.5f * markerSidePixels), + markerSidePixels / 2.f + y * (1.5f * markerSidePixels)); + Mat aux = img.colRange((int)firstCorner.x, (int)firstCorner.x + markerSidePixels) + .rowRange((int)firstCorner.y, (int)firstCorner.y + markerSidePixels); + marker.copyTo(aux); + } + } + img.convertTo(img, CV_8UC3); + + vector > markerCorners; + vector markerIds; + vector > rejectedImgPts; + vector dictIds; + detector.detectMarkers(img, markerCorners, markerIds, rejectedImgPts, dictIds); + ASSERT_EQ(markerIds.size(), 4u); + ASSERT_EQ(dictIds.size(), 4u); + for (size_t i = 0; i < dictIds.size(); ++i) { + EXPECT_EQ(dictIds[i], (int)i); + } +} + struct ArucoThreading: public testing::TestWithParam {