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

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<Dictionary> &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 const Dictionary& getDictionary(size_t index = 0) const;
-    CV_WRAP void setDictionary(const Dictionary& dictionary);
+    CV_WRAP void setDictionary(const Dictionary& dictionary, size_t index = 0);
+    CV_WRAP const std::vector<Dictionary>& getDictionaries() const;
+    CV_WRAP void setDictionaries(const std::vector<Dictionary>& 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);

modules/objdetect/src/aruco/aruco_detector.cpp

@@ -10,6 +10,7 @@
 #include "apriltag/apriltag_quad_thresh.hpp"
 #include "aruco_utils.hpp"
 #include <cmath>
+#include <unordered_set>
 
 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
+    /// dictionaries indicates the types of markers that will be searched
-    Dictionary dictionary;
+    std::vector<Dictionary> 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,
+    ArucoDetectorImpl(const std::vector<Dictionary>&_dictionaries, const DetectorParameters &_detectorParams,
-                      const RefineParameters& _refineParams): dictionary(_dictionary),
+                      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<MarkerCandidateTree>
-    filterTooCloseCandidates(vector<vector<Point2f> > &candidates, vector<vector<Point> > &contours) {
+    filterTooCloseCandidates(vector<vector<Point2f> > &candidates, vector<vector<Point> > &contours, int markerSize) {
         CV_Assert(detectorParams.minMarkerDistanceRate >= 0.);
         vector<MarkerCandidateTree> 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<Mat>& image_pyr, vector<MarkerCandidateTree>& selectedContours,
                             vector<vector<Point2f> >& accepted, vector<vector<Point> >& contours,
-                            vector<int>& ids, OutputArrayOfArrays _rejected = noArray()) {
+                            vector<int>& ids, const Dictionary& currentDictionary, OutputArrayOfArrays _rejected = noArray()) {
         size_t ncandidates = selectedContours.size();
         vector<vector<Point2f> > rejected;
 
@@ -807,11 +806,11 @@ struct ArucoDetector::ArucoDetectorImpl {
                     }
                     const float scale = detectorParams.useAruco3Detection ? img.cols / static_cast<float>(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<ArucoDetectorImpl>(_dictionary, _detectorParams, _refineParams);
+    arucoDetectorImpl = makePtr<ArucoDetectorImpl>(vector<Dictionary>{_dictionary}, _detectorParams, _refineParams);
+}
+
+ArucoDetector::ArucoDetector(const std::vector<Dictionary> &_dictionaries,
+                             const DetectorParameters &_detectorParams,
+                             const RefineParameters& _refineParams) {
+    arucoDetectorImpl = makePtr<ArucoDetectorImpl>(_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
@@ -941,31 +945,56 @@ void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corner
     }
 
     /// STEP 2.c FILTER OUT NEAR CANDIDATE PAIRS
-    auto selectedCandidates = arucoDetectorImpl->filterTooCloseCandidates(candidates, contours);
+    unordered_set<int> uniqueMarkerSizes;
+    for (const Dictionary& dictionary : arucoDetectorImpl->dictionaries) {
+        uniqueMarkerSizes.insert(dictionary.markerSize);
+    }
+
+    // create at max 4 marker candidate trees for each dictionary size
+    vector<vector<MarkerCandidateTree>> candidatesPerDictionarySize = {{}, {}, {}, {}};
+    for (int markerSize : uniqueMarkerSizes) {
+        // min marker size is 4, so subtract 4 to get index
+        const auto dictionarySizeIndex = markerSize - 4;
+        // copy candidates
+        vector<vector<Point2f>> candidatesCopy = candidates;
+        vector<vector<Point> > 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,
+    size_t dictIndex = 0;
-                                          ids, _rejectedImgPoints);
+    vector<int> dictIndices;
+    for (const Dictionary&  currentDictionary : arucoDetectorImpl->dictionaries) {
+        const auto dictionarySizeIndex = currentDictionary.markerSize - 4;
+        // temporary variable to store the current candidates
+        vector<vector<Point2f>> 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) {
+            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);
+                        findCornerInPyrImage(scale_init, closest_pyr_image_idx, grey_pyramid, Mat(currentCandidates[i]), detectorParams);
                     }
                     else {
                         int cornerRefinementWinSize = std::max(1, cvRound(detectorParams.relativeCornerRefinmentWinSize*
-                                                  getAverageModuleSize(candidates[i], dictionary.markerSize, detectorParams.markerBorderBits)));
+                                                    getAverageModuleSize(currentCandidates[i], currentDictionary.markerSize, detectorParams.markerBorderBits)));
                         cornerRefinementWinSize = min(cornerRefinementWinSize, detectorParams.cornerRefinementWinSize);
-                    cornerSubPix(grey, Mat(candidates[i]), Size(cornerRefinementWinSize, cornerRefinementWinSize), Size(-1, -1),
+                        cornerSubPix(grey, Mat(currentCandidates[i]), Size(cornerRefinementWinSize, cornerRefinementWinSize), Size(-1, -1),
                                     TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
                                                 detectorParams.cornerRefinementMaxIterations,
                                                 detectorParams.cornerRefinementMinAccuracy));
@@ -973,6 +1002,9 @@ void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corner
                 }
             });
         }
+        candidates.insert(candidates.end(), currentCandidates.begin(), currentCandidates.end());
+        dictIndex++;
+    }
 
     /// STEP 3, Optional : Corner refinement :: use contour container
     if (detectorParams.cornerRefinementMethod == (int)CORNER_REFINE_CONTOUR){
@@ -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
+void ArucoDetector::write(FileStorage &fs) const {
-{
+    fs << "dictionaries" << "[";
-    arucoDetectorImpl->dictionary.writeDictionary(fs);
+    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 {
+const Dictionary& ArucoDetector::getDictionary(size_t index) const {
-    return arucoDetectorImpl->dictionary;
+    CV_Assert(index < arucoDetectorImpl->dictionaries.size());
+    return arucoDetectorImpl->dictionaries[index];
 }
 
-void ArucoDetector::setDictionary(const Dictionary& dictionary) {
+void ArucoDetector::setDictionary(const Dictionary& dictionary, size_t index) {
-    arucoDetectorImpl->dictionary = dictionary;
+    // 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<Dictionary>& ArucoDetector::getDictionaries() const {
+    return arucoDetectorImpl->dictionaries;
+}
+
+void ArucoDetector::setDictionaries(const vector<Dictionary>& 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 {

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<vector<Point2f> > markerCorners;
+    vector<int> 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<vector<Point2f> > markerCorners;
+    vector<int> markerIds;
+    vector<vector<Point2f> > rejectedImgPts;
+    vector<int> 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<aruco::CornerRefineMethod>
 {