shift corner positions to the correct rotation (5efd1e2f) · Commits · SUMMER2020 / students / proj-2020194

modules/aruco/src/aruco.cpp

+41 −21

Original line number	Diff line number	Diff line
		@@ -210,6 +210,22 @@ static void _reorderCandidatesCorners(vector< vector< Point2f > > &candidates) {
		}
		}

		/**
		* @brief to make sure that the corner's order of both candidates (default/white) is the same
		*/
		static vector< Point2f > alignContourOrder( Point2f corner, vector< Point2f > candidate){
		uint8_t r=0;
		double min = cv::norm( Vec2f( corner - candidate[0] ), NORM_L2SQR);
		for(uint8_t pos=1; pos < 4; pos++) {
		double nDiff = cv::norm( Vec2f( corner - candidate[pos] ), NORM_L2SQR);
		if(nDiff < min){
		r = pos;
		min =nDiff;
		}
		}
		std::rotate(candidate.begin(), candidate.begin() + r, candidate.end());
		return candidate;
		}

		/**
		* @brief Check candidates that are too close to each other, save the potential candidates
		@@ -315,7 +331,7 @@ static void _filterTooCloseCandidates(const vector< vector< Point2f > > &candida
		biggerContours.push_back(contoursIn[biggerIdx]);

		if( detectInvertedMarker ){
		smallerCandidates.push_back(candidatesIn[smallerIdx]);
		smallerCandidates.push_back(alignContourOrder(candidatesIn[biggerIdx][0], candidatesIn[smallerIdx]));
		smallerContours.push_back(contoursIn[smallerIdx]);
		}
		}
		@@ -550,7 +566,7 @@ static int _getBorderErrors(const Mat &bits, int markerSize, int borderSize) {
		*/
		static uint8_t _identifyOneCandidate(const Ptr<Dictionary>& dictionary, InputArray _image,
		vector<Point2f>& _corners, int& idx,
		const Ptr<DetectorParameters>& params)
		const Ptr<DetectorParameters>& params, int& rotation)
		{
		CV_Assert(_corners.size() == 4);
		CV_Assert(_image.getMat().total() != 0);
		@@ -590,14 +606,9 @@ static uint8_t _identifyOneCandidate(const Ptr<Dictionary>& dictionary, InputArr
		.colRange(params->markerBorderBits, candidateBits.rows - params->markerBorderBits);

		// try to indentify the marker
		int rotation;
		if(!dictionary->identify(onlyBits, idx, rotation, params->errorCorrectionRate))
		return 0;

		// shift corner positions to the correct rotation
		if(rotation != 0) {
		std::rotate(_corners.begin(), _corners.begin() + 4 - rotation, _corners.end());
		}
		return typ;
		}

		@@ -611,9 +622,10 @@ class IdentifyCandidatesParallel : public ParallelLoopBody {
		IdentifyCandidatesParallel(const Mat& _grey, vector< vector< Point2f > >& _candidates,
		const Ptr<Dictionary> &_dictionary,
		vector< int >& _idsTmp, vector< uint8_t >& _validCandidates,
		const Ptr<DetectorParameters> &_params)
		const Ptr<DetectorParameters> &_params,
		vector< int > &_rotated)
		: grey(_grey), candidates(_candidates), dictionary(_dictionary),
		idsTmp(_idsTmp), validCandidates(_validCandidates), params(_params) {}
		idsTmp(_idsTmp), validCandidates(_validCandidates), params(_params), rotated(_rotated) {}

		void operator()(const Range &range) const CV_OVERRIDE
		{
		@@ -622,7 +634,7 @@ class IdentifyCandidatesParallel : public ParallelLoopBody {

		for(int i = begin; i < end; i++) {
		int currId;
		validCandidates[i] = _identifyOneCandidate(dictionary, grey, candidates[i], currId, params);
		validCandidates[i] = _identifyOneCandidate(dictionary, grey, candidates[i], currId, params, rotated[i]);

		if(validCandidates[i] > 0)
		idsTmp[i] = currId;
		@@ -639,6 +651,7 @@ class IdentifyCandidatesParallel : public ParallelLoopBody {
		vector< int > &idsTmp;
		vector< uint8_t > &validCandidates;
		const Ptr<DetectorParameters> &params;
		vector< int > &rotated;
		};


		@@ -676,7 +689,12 @@ static void _copyVector2Output(vector< vector< Point2f > > &vec, OutputArrayOfAr
		}
		}


		/**
		* @brief rotate the initial corner to get to the right position
		*/
		static void correctCornerPosition( vector< Point2f >& _candidate, int rotate){
		std::rotate(_candidate.begin(), _candidate.begin() + 4 - rotate, _candidate.end());
		}

		/**
		* @brief Identify square candidates according to a marker dictionary
		@@ -699,6 +717,7 @@ static void _identifyCandidates(InputArray _image, vector< vector< vector< Point
		_convertToGrey(_image.getMat(), grey);

		vector< int > idsTmp(ncandidates, -1);
		vector< int > rotated(ncandidates, 0);
		vector< uint8_t > validCandidates(ncandidates, 0);

		//// Analyze each of the candidates
		@@ -706,23 +725,24 @@ static void _identifyCandidates(InputArray _image, vector< vector< vector< Point
		IdentifyCandidatesParallel(grey,
		params->detectInvertedMarker ? _candidatesSet[1] : _candidatesSet[0],
		_dictionary, idsTmp,
		validCandidates, params));
		validCandidates, params, rotated));

		for(int i = 0; i < ncandidates; 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]);
		// to choose the right set of candidates :: 0 for default, 1 for white markers
		uint8_t set = validCandidates[i]-1;

		contours.push_back(_contoursSet[1][i]);
		// shift corner positions to the correct rotation
		correctCornerPosition(_candidatesSet[set][i], rotated[i]);

		if( !params->detectInvertedMarker && validCandidates[i] == 2 )
		continue;
		}
		// add the default (black) valid candidate
		accepted.push_back(_candidatesSet[0][i]);

		// add valid candidate
		accepted.push_back(_candidatesSet[set][i]);
		ids.push_back(idsTmp[i]);

		contours.push_back(_contoursSet[0][i]);
		contours.push_back(_contoursSet[set][i]);

		} else {
		rejected.push_back(_candidatesSet[0][i]);