Merge pull request #2236 from szk1509:invMarkerContourDetectionImproved (bb5dadd7) · Commits · SUMMER2020 / students / proj-2020194

modules/aruco/src/aruco.cpp

+135 −148

Original line number	Diff line number	Diff line
		@@ -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,40 +247,87 @@ 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;
		// 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){

		// 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++;
		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

+1 −15

Original line number	Diff line number	Diff line
		@@ -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;