Skip to content

Commit 3644a1d

Browse files
committed
Merge pull request #2141 from paroj:lambda
2 parents 250bcd9 + b2f0827 commit 3644a1d

File tree

2 files changed

+77
-291
lines changed

2 files changed

+77
-291
lines changed

modules/aruco/src/aruco.cpp

Lines changed: 58 additions & 230 deletions
Original file line numberDiff line numberDiff line change
@@ -279,49 +279,6 @@ static void _filterTooCloseCandidates(const vector< vector< Point2f > > &candida
279279
}
280280
}
281281

282-
283-
/**
284-
* ParallelLoopBody class for the parallelization of the basic candidate detections using
285-
* different threhold window sizes. Called from function _detectInitialCandidates()
286-
*/
287-
class DetectInitialCandidatesParallel : public ParallelLoopBody {
288-
public:
289-
DetectInitialCandidatesParallel(const Mat *_grey,
290-
vector< vector< vector< Point2f > > > *_candidatesArrays,
291-
vector< vector< vector< Point > > > *_contoursArrays,
292-
const Ptr<DetectorParameters> &_params)
293-
: grey(_grey), candidatesArrays(_candidatesArrays), contoursArrays(_contoursArrays),
294-
params(_params) {}
295-
296-
void operator()(const Range &range) const CV_OVERRIDE {
297-
const int begin = range.start;
298-
const int end = range.end;
299-
300-
for(int i = begin; i < end; i++) {
301-
int currScale =
302-
params->adaptiveThreshWinSizeMin + i * params->adaptiveThreshWinSizeStep;
303-
// threshold
304-
Mat thresh;
305-
_threshold(*grey, thresh, currScale, params->adaptiveThreshConstant);
306-
307-
// detect rectangles
308-
_findMarkerContours(thresh, (*candidatesArrays)[i], (*contoursArrays)[i],
309-
params->minMarkerPerimeterRate, params->maxMarkerPerimeterRate,
310-
params->polygonalApproxAccuracyRate, params->minCornerDistanceRate,
311-
params->minDistanceToBorder);
312-
}
313-
}
314-
315-
private:
316-
DetectInitialCandidatesParallel &operator=(const DetectInitialCandidatesParallel &);
317-
318-
const Mat *grey;
319-
vector< vector< vector< Point2f > > > *candidatesArrays;
320-
vector< vector< vector< Point > > > *contoursArrays;
321-
const Ptr<DetectorParameters> &params;
322-
};
323-
324-
325282
/**
326283
* @brief Initial steps on finding square candidates
327284
*/
@@ -341,21 +298,23 @@ static void _detectInitialCandidates(const Mat &grey, vector< vector< Point2f >
341298
vector< vector< vector< Point > > > contoursArrays((size_t) nScales);
342299

343300
////for each value in the interval of thresholding window sizes
344-
// for(int i = 0; i < nScales; i++) {
345-
// int currScale = params.adaptiveThreshWinSizeMin + i*params.adaptiveThreshWinSizeStep;
346-
// // treshold
347-
// Mat thresh;
348-
// _threshold(grey, thresh, currScale, params.adaptiveThreshConstant);
349-
// // detect rectangles
350-
// _findMarkerContours(thresh, candidatesArrays[i], contoursArrays[i],
351-
// params.minMarkerPerimeterRate,
352-
// params.maxMarkerPerimeterRate, params.polygonalApproxAccuracyRate,
353-
// params.minCornerDistance, params.minDistanceToBorder);
354-
//}
355-
356-
// this is the parallel call for the previous commented loop (result is equivalent)
357-
parallel_for_(Range(0, nScales), DetectInitialCandidatesParallel(&grey, &candidatesArrays,
358-
&contoursArrays, params));
301+
parallel_for_(Range(0, nScales), [&](const Range& range) {
302+
const int begin = range.start;
303+
const int end = range.end;
304+
305+
for (int i = begin; i < end; i++) {
306+
int currScale = params->adaptiveThreshWinSizeMin + i * params->adaptiveThreshWinSizeStep;
307+
// threshold
308+
Mat thresh;
309+
_threshold(grey, thresh, currScale, params->adaptiveThreshConstant);
310+
311+
// detect rectangles
312+
_findMarkerContours(thresh, candidatesArrays[i], contoursArrays[i],
313+
params->minMarkerPerimeterRate, params->maxMarkerPerimeterRate,
314+
params->polygonalApproxAccuracyRate, params->minCornerDistanceRate,
315+
params->minDistanceToBorder);
316+
}
317+
});
359318

360319
// join candidates
361320
for(int i = 0; i < nScales; i++) {
@@ -545,46 +504,6 @@ static bool _identifyOneCandidate(const Ptr<Dictionary>& dictionary, InputArray
545504
return true;
546505
}
547506

548-
549-
/**
550-
* ParallelLoopBody class for the parallelization of the marker identification step
551-
* Called from function _identifyCandidates()
552-
*/
553-
class IdentifyCandidatesParallel : public ParallelLoopBody {
554-
public:
555-
IdentifyCandidatesParallel(const Mat& _grey, vector< vector< Point2f > >& _candidates,
556-
const Ptr<Dictionary> &_dictionary,
557-
vector< int >& _idsTmp, vector< char >& _validCandidates,
558-
const Ptr<DetectorParameters> &_params)
559-
: grey(_grey), candidates(_candidates), dictionary(_dictionary),
560-
idsTmp(_idsTmp), validCandidates(_validCandidates), params(_params) {}
561-
562-
void operator()(const Range &range) const CV_OVERRIDE {
563-
const int begin = range.start;
564-
const int end = range.end;
565-
566-
for(int i = begin; i < end; i++) {
567-
int currId;
568-
if(_identifyOneCandidate(dictionary, grey, candidates[i], currId, params)) {
569-
validCandidates[i] = 1;
570-
idsTmp[i] = currId;
571-
}
572-
}
573-
}
574-
575-
private:
576-
IdentifyCandidatesParallel &operator=(const IdentifyCandidatesParallel &); // to quiet MSVC
577-
578-
const Mat &grey;
579-
vector< vector< Point2f > >& candidates;
580-
const Ptr<Dictionary> &dictionary;
581-
vector< int > &idsTmp;
582-
vector< char > &validCandidates;
583-
const Ptr<DetectorParameters> &params;
584-
};
585-
586-
587-
588507
/**
589508
* @brief Copy the contents of a corners vector to an OutputArray, settings its size.
590509
*/
@@ -645,19 +564,18 @@ static void _identifyCandidates(InputArray _image, vector< vector< Point2f > >&
645564
vector< char > validCandidates(ncandidates, 0);
646565

647566
//// Analyze each of the candidates
648-
// for (int i = 0; i < ncandidates; i++) {
649-
// int currId = i;
650-
// Mat currentCandidate = _candidates.getMat(i);
651-
// if (_identifyOneCandidate(dictionary, grey, currentCandidate, currId, params)) {
652-
// validCandidates[i] = 1;
653-
// idsTmp[i] = currId;
654-
// }
655-
//}
656-
657-
// this is the parallel call for the previous commented loop (result is equivalent)
658-
parallel_for_(Range(0, ncandidates),
659-
IdentifyCandidatesParallel(grey, _candidates, _dictionary, idsTmp,
660-
validCandidates, params));
567+
parallel_for_(Range(0, ncandidates), [&](const Range &range) {
568+
const int begin = range.start;
569+
const int end = range.end;
570+
571+
for(int i = begin; i < end; i++) {
572+
int currId;
573+
if(_identifyOneCandidate(_dictionary, grey, _candidates[i], currId, params)) {
574+
validCandidates[i] = 1;
575+
idsTmp[i] = currId;
576+
}
577+
}
578+
});
661579

662580
for(int i = 0; i < ncandidates; i++) {
663581
if(validCandidates[i] == 1) {
@@ -772,40 +690,6 @@ static void _getSingleMarkerObjectPoints(float markerLength, OutputArray _objPoi
772690
objPoints.ptr< Vec3f >(0)[3] = Vec3f(-markerLength / 2.f, -markerLength / 2.f, 0);
773691
}
774692

775-
776-
777-
778-
/**
779-
* ParallelLoopBody class for the parallelization of the marker corner subpixel refinement
780-
* Called from function detectMarkers()
781-
*/
782-
class MarkerSubpixelParallel : public ParallelLoopBody {
783-
public:
784-
MarkerSubpixelParallel(const Mat *_grey, OutputArrayOfArrays _corners,
785-
const Ptr<DetectorParameters> &_params)
786-
: grey(_grey), corners(_corners), params(_params) {}
787-
788-
void operator()(const Range &range) const CV_OVERRIDE {
789-
const int begin = range.start;
790-
const int end = range.end;
791-
792-
for(int i = begin; i < end; i++) {
793-
cornerSubPix(*grey, corners.getMat(i),
794-
Size(params->cornerRefinementWinSize, params->cornerRefinementWinSize),
795-
Size(-1, -1), TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
796-
params->cornerRefinementMaxIterations,
797-
params->cornerRefinementMinAccuracy));
798-
}
799-
}
800-
801-
private:
802-
MarkerSubpixelParallel &operator=(const MarkerSubpixelParallel &); // to quiet MSVC
803-
804-
const Mat *grey;
805-
OutputArrayOfArrays corners;
806-
const Ptr<DetectorParameters> &params;
807-
};
808-
809693
/**
810694
* Line fitting A * B = C :: Called from function refineCandidateLines
811695
* @param nContours, contour-container
@@ -948,34 +832,6 @@ static void _refineCandidateLines(std::vector<Point>& nContours, std::vector<Poi
948832
}
949833
}
950834

951-
952-
/**
953-
* ParallelLoopBody class for the parallelization of the marker corner contour refinement
954-
* Called from function detectMarkers()
955-
*/
956-
class MarkerContourParallel : public ParallelLoopBody {
957-
public:
958-
MarkerContourParallel( vector< vector< Point > >& _contours, vector< vector< Point2f > >& _candidates, const Mat& _camMatrix, const Mat& _distCoeff)
959-
: contours(_contours), candidates(_candidates), camMatrix(_camMatrix), distCoeff(_distCoeff){}
960-
961-
void operator()(const Range &range) const CV_OVERRIDE {
962-
963-
for(int i = range.start; i < range.end; i++) {
964-
_refineCandidateLines(contours[i], candidates[i], camMatrix, distCoeff);
965-
}
966-
}
967-
968-
private:
969-
MarkerContourParallel &operator=(const MarkerContourParallel &){
970-
return *this;
971-
}
972-
973-
vector< vector< Point > >& contours;
974-
vector< vector< Point2f > >& candidates;
975-
const Mat& camMatrix;
976-
const Mat& distCoeff;
977-
};
978-
979835
#ifdef APRIL_DEBUG
980836
static void _darken(const Mat &im){
981837
for (int y = 0; y < im.rows; y++) {
@@ -1152,17 +1008,19 @@ void detectMarkers(InputArray _image, const Ptr<Dictionary> &_dictionary, Output
11521008
_params->cornerRefinementMinAccuracy > 0);
11531009

11541010
//// do corner refinement for each of the detected markers
1155-
// for (unsigned int i = 0; i < _corners.cols(); i++) {
1156-
// cornerSubPix(grey, _corners.getMat(i),
1157-
// Size(params.cornerRefinementWinSize, params.cornerRefinementWinSize),
1158-
// Size(-1, -1), TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
1159-
// params.cornerRefinementMaxIterations,
1160-
// params.cornerRefinementMinAccuracy));
1161-
//}
1162-
1163-
// this is the parallel call for the previous commented loop (result is equivalent)
1164-
parallel_for_(Range(0, _corners.cols()),
1165-
MarkerSubpixelParallel(&grey, _corners, _params));
1011+
parallel_for_(Range(0, _corners.cols()), [&](const Range& range) {
1012+
const int begin = range.start;
1013+
const int end = range.end;
1014+
1015+
for (int i = begin; i < end; i++) {
1016+
cornerSubPix(grey, _corners.getMat(i),
1017+
Size(_params->cornerRefinementWinSize, _params->cornerRefinementWinSize),
1018+
Size(-1, -1),
1019+
TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
1020+
_params->cornerRefinementMaxIterations,
1021+
_params->cornerRefinementMinAccuracy));
1022+
}
1023+
});
11661024
}
11671025

11681026
/// STEP 4, Optional : Corner refinement :: use contour container
@@ -1171,50 +1029,19 @@ void detectMarkers(InputArray _image, const Ptr<Dictionary> &_dictionary, Output
11711029
if(! _ids.empty()){
11721030

11731031
// do corner refinement using the contours for each detected markers
1174-
parallel_for_(Range(0, _corners.cols()), MarkerContourParallel(contours, candidates, camMatrix.getMat(), distCoeff.getMat()));
1032+
parallel_for_(Range(0, _corners.cols()), [&](const Range& range) {
1033+
for (int i = range.start; i < range.end; i++) {
1034+
_refineCandidateLines(contours[i], candidates[i], camMatrix.getMat(),
1035+
distCoeff.getMat());
1036+
}
1037+
});
11751038

11761039
// copy the corners to the output array
11771040
_copyVector2Output(candidates, _corners);
11781041
}
11791042
}
11801043
}
11811044

1182-
1183-
1184-
/**
1185-
* ParallelLoopBody class for the parallelization of the single markers pose estimation
1186-
* Called from function estimatePoseSingleMarkers()
1187-
*/
1188-
class SinglePoseEstimationParallel : public ParallelLoopBody {
1189-
public:
1190-
SinglePoseEstimationParallel(Mat& _markerObjPoints, InputArrayOfArrays _corners,
1191-
InputArray _cameraMatrix, InputArray _distCoeffs,
1192-
Mat& _rvecs, Mat& _tvecs)
1193-
: markerObjPoints(_markerObjPoints), corners(_corners), cameraMatrix(_cameraMatrix),
1194-
distCoeffs(_distCoeffs), rvecs(_rvecs), tvecs(_tvecs) {}
1195-
1196-
void operator()(const Range &range) const CV_OVERRIDE {
1197-
const int begin = range.start;
1198-
const int end = range.end;
1199-
1200-
for(int i = begin; i < end; i++) {
1201-
solvePnP(markerObjPoints, corners.getMat(i), cameraMatrix, distCoeffs,
1202-
rvecs.at<Vec3d>(i), tvecs.at<Vec3d>(i));
1203-
}
1204-
}
1205-
1206-
private:
1207-
SinglePoseEstimationParallel &operator=(const SinglePoseEstimationParallel &); // to quiet MSVC
1208-
1209-
Mat& markerObjPoints;
1210-
InputArrayOfArrays corners;
1211-
InputArray cameraMatrix, distCoeffs;
1212-
Mat& rvecs, tvecs;
1213-
};
1214-
1215-
1216-
1217-
12181045
/**
12191046
*/
12201047
void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength,
@@ -1232,15 +1059,16 @@ void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength,
12321059
Mat rvecs = _rvecs.getMat(), tvecs = _tvecs.getMat();
12331060

12341061
//// for each marker, calculate its pose
1235-
// for (int i = 0; i < nMarkers; i++) {
1236-
// solvePnP(markerObjPoints, _corners.getMat(i), _cameraMatrix, _distCoeffs,
1237-
// _rvecs.getMat(i), _tvecs.getMat(i));
1238-
//}
1239-
1240-
// this is the parallel call for the previous commented loop (result is equivalent)
1241-
parallel_for_(Range(0, nMarkers),
1242-
SinglePoseEstimationParallel(markerObjPoints, _corners, _cameraMatrix,
1243-
_distCoeffs, rvecs, tvecs));
1062+
parallel_for_(Range(0, nMarkers), [&](const Range& range) {
1063+
const int begin = range.start;
1064+
const int end = range.end;
1065+
1066+
for (int i = begin; i < end; i++) {
1067+
solvePnP(markerObjPoints, _corners.getMat(i), _cameraMatrix, _distCoeffs, rvecs.at<Vec3d>(i),
1068+
tvecs.at<Vec3d>(i));
1069+
}
1070+
});
1071+
12441072
if(_objPoints.needed()){
12451073
markerObjPoints.convertTo(_objPoints, -1);
12461074
}

0 commit comments

Comments
 (0)