Merge pull request #2236 from szk1509:invMarkerContourDetectionImproved

modules/aruco/src/aruco.cpp

@@ -212,17 +212,20 @@ static void _reorderCandidatesCorners(vector< vector< Point2f > > &candidates) {
 
 
 /**
-  * @brief Check candidates that are too close to each other and remove the smaller one
+  * @brief Check candidates that are too close to each other, save the potential candidates
+  *        (i.e. biggest/smallest contour) and remove the rest
   */
 static void _filterTooCloseCandidates(const vector< vector< Point2f > > &candidatesIn,
-                                      vector< vector< Point2f > > &candidatesOut,
+                                      vector< vector< vector< Point2f > > > &candidatesSetOut,
                                       const vector< vector< Point > > &contoursIn,
-                                      vector< vector< Point > > &contoursOut,
-                                      double minMarkerDistanceRate) {
+                                      vector< vector< vector< Point > > > &contoursSetOut,
+                                      double minMarkerDistanceRate, bool detectInvertedMarker) {
 
     CV_Assert(minMarkerDistanceRate >= 0);
 
-    vector< pair< int, int > > nearCandidates;
+    vector<int> candGroup;
+    candGroup.resize(candidatesIn.size(), -1);
+    vector< vector<unsigned int> > groupedCandidates;
     for(unsigned int i = 0; i < candidatesIn.size(); i++) {
         for(unsigned int j = i + 1; j < candidatesIn.size(); j++) {
 
@@ -244,39 +247,86 @@ static void _filterTooCloseCandidates(const vector< vector< Point2f > > &candida
                 // if mean square distance is too low, remove the smaller one of the two markers
                 double minMarkerDistancePixels = double(minimumPerimeter) * minMarkerDistanceRate;
                 if(distSq < minMarkerDistancePixels * minMarkerDistancePixels) {
-                    nearCandidates.push_back(pair< int, int >(i, j));
-                    break;
+
+                    // i and j are not related to a group
+                    if(candGroup[i]<0 && candGroup[j]<0){
+                        // mark candidates with their corresponding group number
+                        candGroup[i] = candGroup[j] = (int)groupedCandidates.size();
+
+                        // create group
+                        vector<unsigned int> grouped;
+                        grouped.push_back(i);
+                        grouped.push_back(j);
+                        groupedCandidates.push_back( grouped );
+                    }
+                    // i is related to a group
+                    else if(candGroup[i] > -1 && candGroup[j] == -1){
+                        int group = candGroup[i];
+                        candGroup[j] = group;
+
+                        // add to group
+                        groupedCandidates[group].push_back( j );
+                    }
+                    // j is related to a group
+                    else if(candGroup[j] > -1 && candGroup[i] == -1){
+                        int group = candGroup[j];
+                        candGroup[i] = group;
+
+                        // add to group
+                        groupedCandidates[group].push_back( i );
+                    }
                 }
             }
         }
     }
 
-    // mark smaller one in pairs to remove
-    vector< bool > toRemove(candidatesIn.size(), false);
-    for(unsigned int i = 0; i < nearCandidates.size(); i++) {
-        // if one of the marker has been already markerd to removed, dont need to do anything
-        if(toRemove[nearCandidates[i].first] || toRemove[nearCandidates[i].second]) continue;
-        size_t perimeter1 = contoursIn[nearCandidates[i].first].size();
-        size_t perimeter2 = contoursIn[nearCandidates[i].second].size();
-        if(perimeter1 > perimeter2)
-            toRemove[nearCandidates[i].second] = true;
-        else
-            toRemove[nearCandidates[i].first] = true;
-    }
-
-    // remove extra candidates
-    candidatesOut.clear();
-    unsigned long totalRemaining = 0;
-    for(unsigned int i = 0; i < toRemove.size(); i++)
-        if(!toRemove[i]) totalRemaining++;
-    candidatesOut.resize(totalRemaining);
-    contoursOut.resize(totalRemaining);
-    for(unsigned int i = 0, currIdx = 0; i < candidatesIn.size(); i++) {
-        if(toRemove[i]) continue;
-        candidatesOut[currIdx] = candidatesIn[i];
-        contoursOut[currIdx] = contoursIn[i];
-        currIdx++;
+    // save possible candidates
+    candidatesSetOut.clear();
+    contoursSetOut.clear();
+
+    vector< vector< Point2f > > biggerCandidates;
+    vector< vector< Point > > biggerContours;
+    vector< vector< Point2f > > smallerCandidates;
+    vector< vector< Point > > smallerContours;
+
+    // save possible candidates
+    for( unsigned int i = 0; i < groupedCandidates.size(); i++ ) {
+        int smallerIdx = groupedCandidates[i][0];
+        int biggerIdx = -1;
+
+        // evaluate group elements
+        for( unsigned int j = 1; j < groupedCandidates[i].size(); j++ ) {
+            size_t currPerim = contoursIn[ groupedCandidates[i][j] ].size();
+
+            // check if current contour is bigger
+            if ( biggerIdx < 0 )
+                biggerIdx = groupedCandidates[i][j];
+            else if(currPerim >= contoursIn[ biggerIdx ].size())
+                biggerIdx = groupedCandidates[i][j];
+
+            // check if current contour is smaller
+            if(currPerim < contoursIn[ smallerIdx ].size() && detectInvertedMarker)
+                smallerIdx = groupedCandidates[i][j];
+        }
+        // add contours und candidates
+        if(biggerIdx > -1){
+
+            biggerCandidates.push_back(candidatesIn[biggerIdx]);
+            biggerContours.push_back(contoursIn[biggerIdx]);
+
+            if( detectInvertedMarker ){
+                smallerCandidates.push_back(candidatesIn[smallerIdx]);
+                smallerContours.push_back(contoursIn[smallerIdx]);
+            }
         }
+    }
+    // to preserve the structure :: candidateSet< defaultCandidates, whiteCandidates >
+    // default candidates
+    candidatesSetOut.push_back(biggerCandidates);
+    contoursSetOut.push_back(biggerContours);
+    // white candidates
+    candidatesSetOut.push_back(smallerCandidates);
+    contoursSetOut.push_back(smallerContours);
 }
 
 
@@ -370,8 +420,8 @@ static void _detectInitialCandidates(const Mat &grey, vector< vector< Point2f >
 /**
  * @brief Detect square candidates in the input image
  */
-static void _detectCandidates(InputArray _image, vector< vector< Point2f > >& candidatesOut,
-                              vector< vector< Point > >& contoursOut, const Ptr<DetectorParameters> &_params) {
+static void _detectCandidates(InputArray _image, vector< vector< vector< Point2f > > >& candidatesSetOut,
+                              vector< vector< vector< Point > > >& contoursSetOut, const Ptr<DetectorParameters> &_params) {
 
     Mat image = _image.getMat();
     CV_Assert(image.total() != 0);
@@ -389,8 +439,9 @@ static void _detectCandidates(InputArray _image, vector< vector< Point2f > >& ca
     _reorderCandidatesCorners(candidates);
 
     /// 4. FILTER OUT NEAR CANDIDATE PAIRS
-    _filterTooCloseCandidates(candidates, candidatesOut, contours, contoursOut,
-                              _params->minMarkerDistanceRate);
+    // save the outter/inner border (i.e. potential candidates)
+    _filterTooCloseCandidates(candidates, candidatesSetOut, contours, contoursSetOut,
+                              _params->minMarkerDistanceRate, _params->detectInvertedMarker);
 }
 
 
@@ -493,8 +544,11 @@ static int _getBorderErrors(const Mat &bits, int markerSize, int borderSize) {
 
 /**
  * @brief Tries to identify one candidate given the dictionary
+ * @return candidate typ. zero if the candidate is not valid,
+ *                           1 if the candidate is a black candidate (default candidate)
+ *                           2 if the candidate is a white candidate
  */
-static bool _identifyOneCandidate(const Ptr<Dictionary>& dictionary, InputArray _image,
+static uint8_t _identifyOneCandidate(const Ptr<Dictionary>& dictionary, InputArray _image,
                                   vector<Point2f>& _corners, int& idx,
                                   const Ptr<DetectorParameters>& params)
 {
@@ -502,6 +556,7 @@ static bool _identifyOneCandidate(const Ptr<Dictionary>& dictionary, InputArray
     CV_Assert(_image.getMat().total() != 0);
     CV_Assert(params->markerBorderBits > 0);
 
+    uint8_t typ=1;
     // get bits
     Mat candidateBits =
         _extractBits(_image, _corners, dictionary->markerSize, params->markerBorderBits,
@@ -523,9 +578,10 @@ static bool _identifyOneCandidate(const Ptr<Dictionary>& dictionary, InputArray
         if(invBError<borderErrors){
             borderErrors = invBError;
             invertedImg.copyTo(candidateBits);
+            typ=2;
         }
     }
-    if(borderErrors > maximumErrorsInBorder) return false;
+    if(borderErrors > maximumErrorsInBorder) return 0; // border is wrong
 
     // take only inner bits
     Mat onlyBits =
@@ -536,13 +592,13 @@ static bool _identifyOneCandidate(const Ptr<Dictionary>& dictionary, InputArray
     // try to indentify the marker
     int rotation;
     if(!dictionary->identify(onlyBits, idx, rotation, params->errorCorrectionRate))
-        return false;
+        return 0;
 
     // shift corner positions to the correct rotation
     if(rotation != 0) {
         std::rotate(_corners.begin(), _corners.begin() + 4 - rotation, _corners.end());
     }
-    return true;
+    return typ;
 }
 
 
@@ -554,22 +610,24 @@ class IdentifyCandidatesParallel : public ParallelLoopBody {
     public:
     IdentifyCandidatesParallel(const Mat& _grey, vector< vector< Point2f > >& _candidates,
                                const Ptr<Dictionary> &_dictionary,
-                               vector< int >& _idsTmp, vector< char >& _validCandidates,
+                               vector< int >& _idsTmp, vector< uint8_t >& _validCandidates,
                                const Ptr<DetectorParameters> &_params)
         : grey(_grey), candidates(_candidates), dictionary(_dictionary),
           idsTmp(_idsTmp), validCandidates(_validCandidates), params(_params) {}
 
-    void operator()(const Range &range) const CV_OVERRIDE {
+    void operator()(const Range &range) const CV_OVERRIDE
+    {
         const int begin = range.start;
         const int end = range.end;
 
         for(int i = begin; i < end; i++) {
             int currId;
-            if(_identifyOneCandidate(dictionary, grey, candidates[i], currId, params)) {
-                validCandidates[i] = 1;
+            validCandidates[i] = _identifyOneCandidate(dictionary, grey, candidates[i], currId, params);
+
+            if(validCandidates[i] > 0)
                 idsTmp[i] = currId;
         }
-        }
+
     }
 
     private:
@@ -579,7 +637,7 @@ class IdentifyCandidatesParallel : public ParallelLoopBody {
     vector< vector< Point2f > >& candidates;
     const Ptr<Dictionary> &dictionary;
     vector< int > &idsTmp;
-    vector< char > &validCandidates;
+    vector< uint8_t > &validCandidates;
     const Ptr<DetectorParameters> &params;
 };
 
@@ -623,14 +681,13 @@ static void _copyVector2Output(vector< vector< Point2f > > &vec, OutputArrayOfAr
 /**
  * @brief Identify square candidates according to a marker dictionary
  */
-static void _identifyCandidates(InputArray _image, vector< vector< Point2f > >& _candidates,
-                                vector< vector<Point> >& _contours, const Ptr<Dictionary> &_dictionary,
-                                vector< vector< Point2f > >& _accepted, vector< int >& ids,
+static void _identifyCandidates(InputArray _image, vector< vector< vector< Point2f > > >& _candidatesSet,
+                                vector< vector< vector<Point> > >& _contoursSet, const Ptr<Dictionary> &_dictionary,
+                                vector< vector< Point2f > >& _accepted, vector< vector<Point> >& _contours, vector< int >& ids,
                                 const Ptr<DetectorParameters> &params,
                                 OutputArrayOfArrays _rejected = noArray()) {
 
-    int ncandidates = (int)_candidates.size();
-
+    int ncandidates = (int)_candidatesSet[0].size();
     vector< vector< Point2f > > accepted;
     vector< vector< Point2f > > rejected;
 
@@ -642,32 +699,33 @@ static void _identifyCandidates(InputArray _image, vector< vector< Point2f > >&
     _convertToGrey(_image.getMat(), grey);
 
     vector< int > idsTmp(ncandidates, -1);
-    vector< char > validCandidates(ncandidates, 0);
+    vector< uint8_t > validCandidates(ncandidates, 0);
 
     //// Analyze each of the candidates
-    // for (int i = 0; i < ncandidates; i++) {
-    //    int currId = i;
-    //    Mat currentCandidate = _candidates.getMat(i);
-    //    if (_identifyOneCandidate(dictionary, grey, currentCandidate, currId, params)) {
-    //        validCandidates[i] = 1;
-    //        idsTmp[i] = currId;
-    //    }
-    //}
-
-    // this is the parallel call for the previous commented loop (result is equivalent)
     parallel_for_(Range(0, ncandidates),
-                  IdentifyCandidatesParallel(grey, _candidates, _dictionary, idsTmp,
+                  IdentifyCandidatesParallel(grey,
+                                             params->detectInvertedMarker ? _candidatesSet[1] : _candidatesSet[0],
+                                             _dictionary, idsTmp,
                                              validCandidates, params));
 
     for(int i = 0; i < ncandidates; i++) {
-        if(validCandidates[i] == 1) {
-            accepted.push_back(_candidates[i]);
+        if(validCandidates[i] > 0) {
+            // add the white valid candidate
+            if( params->detectInvertedMarker && validCandidates[i] == 2 ){
+                accepted.push_back(_candidatesSet[1][i]);
+                ids.push_back(idsTmp[i]);
+
+                contours.push_back(_contoursSet[1][i]);
+                continue;
+            }
+            // add the default (black) valid candidate
+            accepted.push_back(_candidatesSet[0][i]);
             ids.push_back(idsTmp[i]);
 
-            contours.push_back(_contours[i]);
+            contours.push_back(_contoursSet[0][i]);
 
         } else {
-            rejected.push_back(_candidates[i]);
+            rejected.push_back(_candidatesSet[0][i]);
         }
     }
 
@@ -682,80 +740,6 @@ static void _identifyCandidates(InputArray _image, vector< vector< Point2f > >&
 }
 
 
-/**
-  * @brief Final filter of markers after its identification
-  */
-static void _filterDetectedMarkers(vector< vector< Point2f > >& _corners, vector< int >& _ids, vector< vector< Point> >& _contours) {
-
-    CV_Assert(_corners.size() == _ids.size());
-    if(_corners.empty()) return;
-
-    // mark markers that will be removed
-    vector< bool > toRemove(_corners.size(), false);
-    bool atLeastOneRemove = false;
-
-    // remove repeated markers with same id, if one contains the other (doble border bug)
-    for(unsigned int i = 0; i < _corners.size() - 1; i++) {
-        for(unsigned int j = i + 1; j < _corners.size(); j++) {
-            if(_ids[i] != _ids[j]) continue;
-
-            // check if first marker is inside second
-            bool inside = true;
-            for(unsigned int p = 0; p < 4; p++) {
-                Point2f point = _corners[j][p];
-                if(pointPolygonTest(_corners[i], point, false) < 0) {
-                    inside = false;
-                    break;
-                }
-            }
-            if(inside) {
-                toRemove[j] = true;
-                atLeastOneRemove = true;
-                continue;
-            }
-
-            // check the second marker
-            inside = true;
-            for(unsigned int p = 0; p < 4; p++) {
-                Point2f point = _corners[i][p];
-                if(pointPolygonTest(_corners[j], point, false) < 0) {
-                    inside = false;
-                    break;
-                }
-            }
-            if(inside) {
-                toRemove[i] = true;
-                atLeastOneRemove = true;
-                continue;
-            }
-        }
-    }
-
-    // parse output
-    if(atLeastOneRemove) {
-        vector< vector< Point2f > >::iterator filteredCorners = _corners.begin();
-        vector< int >::iterator filteredIds = _ids.begin();
-
-        vector< vector< Point > >::iterator filteredContours = _contours.begin();
-
-        for(unsigned int i = 0; i < toRemove.size(); i++) {
-            if(!toRemove[i]) {
-                *filteredCorners++ = _corners[i];
-                *filteredIds++ = _ids[i];
-
-                *filteredContours++ = _contours[i];
-            }
-        }
-
-        _ids.erase(filteredIds, _ids.end());
-        _corners.erase(filteredCorners, _corners.end());
-
-        _contours.erase(filteredContours, _contours.end());
-    }
-}
-
-
-
 /**
   * @brief Return object points for the system centered in a single marker, given the marker length
   */
@@ -1127,26 +1111,29 @@ void detectMarkers(InputArray _image, const Ptr<Dictionary> &_dictionary, Output
     vector< vector< Point > > contours;
     vector< int > ids;
 
+    vector< vector< vector< Point2f > > > candidatesSet;
+    vector< vector< vector< Point > > > contoursSet;
     /// STEP 1.a Detect marker candidates :: using AprilTag
-    if(_params->cornerRefinementMethod == CORNER_REFINE_APRILTAG)
+    if(_params->cornerRefinementMethod == CORNER_REFINE_APRILTAG){
         _apriltag(grey, _params, candidates, contours);
 
+        candidatesSet.push_back(candidates);
+        contoursSet.push_back(contours);
+    }
+
     /// STEP 1.b Detect marker candidates :: traditional way
     else
-        _detectCandidates(grey, candidates, contours, _params);
+        _detectCandidates(grey, candidatesSet, contoursSet, _params);
 
     /// STEP 2: Check candidate codification (identify markers)
-    _identifyCandidates(grey, candidates, contours, _dictionary, candidates, ids, _params,
+    _identifyCandidates(grey, candidatesSet, contoursSet, _dictionary, candidates, contours, ids, _params,
                         _rejectedImgPoints);
 
-    /// STEP 3: Filter detected markers;
-    _filterDetectedMarkers(candidates, ids, contours);
-
     // copy to output arrays
     _copyVector2Output(candidates, _corners);
     Mat(ids).copyTo(_ids);
 
-    /// STEP 4: Corner refinement :: use corner subpix
+    /// STEP 3: Corner refinement :: use corner subpix
     if( _params->cornerRefinementMethod == CORNER_REFINE_SUBPIX ) {
         CV_Assert(_params->cornerRefinementWinSize > 0 && _params->cornerRefinementMaxIterations > 0 &&
                   _params->cornerRefinementMinAccuracy > 0);
@@ -1165,7 +1152,7 @@ void detectMarkers(InputArray _image, const Ptr<Dictionary> &_dictionary, Output
                       MarkerSubpixelParallel(&grey, _corners, _params));
     }
 
-    /// STEP 4, Optional : Corner refinement :: use contour container
+    /// STEP 3, Optional : Corner refinement :: use contour container
     if( _params->cornerRefinementMethod == CORNER_REFINE_CONTOUR){
 
         if(! _ids.empty()){

modules/aruco/test/test_arucodetection.cpp

@@ -322,19 +322,6 @@ void CV_ArucoDetectionPerspective::run(int tryWith) {
                 }
                 for(int c = 0; c < 4; c++) {
                     double dist = cv::norm(groundTruthCorners[c] - corners[0][c]);  // TODO cvtest
-                    if(CV_ArucoDetectionPerspective::DETECT_INVERTED_MARKER == tryWith){
-                        if(szEnclosed && dist > 3){
-                            ts->printf(cvtest::TS::LOG, "Incorrect marker corners position");
-                            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
-                            return;
-                        }
-                        if(!szEnclosed && dist >15){
-                            ts->printf(cvtest::TS::LOG, "Incorrect marker corners position");
-                            ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
-                            return;
-                        }
-                    }
-                    else{
                     if(dist > 5) {
                             ts->printf(cvtest::TS::LOG, "Incorrect marker corners position");
                             ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
@@ -343,7 +330,6 @@ void CV_ArucoDetectionPerspective::run(int tryWith) {
                 }
             }
         }
-        }
         // change the state :: to detect an enclosed inverted marker
         if( CV_ArucoDetectionPerspective::DETECT_INVERTED_MARKER == tryWith && distance == 0.1 ){
             distance -= 0.1;