bool findAprilTag(Mat &image, int targetID, Mat &ret, bool draw=false, int errorThresh=0) {
vector<TagDetection> detections;
double opticalCenter[2] = { image.cols/2.0, image.rows/2.0 };
detector->process(image, opticalCenter, detections);
for(int id=0; id<(int)detections.size(); ++id) {
TagDetection &dd = detections[id];
if(dd.id!=targetID) continue;
if(dd.hammingDistance>errorThresh) continue; //very strict!
if(draw) {
for(int i=0; i<4; ++i) {
int j = (i+1)%4;
Point r1(dd.p[i][0],dd.p[i][1]);
Point r2(dd.p[j][0],dd.p[j][1]);
line( image, r1, r2, helper::CV_RED, 2 );
}
cv::putText( image, helper::num2str(dd.id), cv::Point(dd.cxy[0],dd.cxy[1]), CV_FONT_NORMAL, 1, helper::CV_BLUE, 2 );
}
cv::Mat tmp(3,3,CV_64FC1, (double*)dd.homography[0]);
double vm[] = {1,0,dd.hxy[0],0,1,dd.hxy[1],0,0,1};
ret = cv::Mat(3,3,CV_64FC1,vm) * tmp;
return true;
}
return false;
}
