std::vector<TagMatch> detectTags(image_u8_t *im) {
const int hamm_hist_max = 10;
int hamm_hist[hamm_hist_max];
memset(hamm_hist, 0, sizeof(hamm_hist));
zarray_t *detections = apriltag_detector_detect(td, im);
std::vector<TagMatch> tag_matches;
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
if (!quiet) {
printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, goodness %8.3f, margin %8.3f\n",
i, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);
// image_u8_draw_line(im, det->p[x][0], det->p[x][1], det->p[x+1][0], det->p[x+1][1], 255, 10);
}
if (tag_id == -1 || det->id == tag_id) {
TagMatch tag_match;
tag_match.id = det->family->d*det->family->d;
tag_match.p0 = cv::Point2d(det->p[0][0], det->p[0][1]);
tag_match.p1 = cv::Point2d(det->p[1][0], det->p[1][1]);
tag_match.p2 = cv::Point2d(det->p[2][0], det->p[2][1]);
tag_match.p3 = cv::Point2d(det->p[3][0], det->p[3][1]);
Eigen::Map<Eigen::Matrix3d> H_map(det->H->data);
tag_match.H = H_map.transpose();
tag_matches.push_back(tag_match);
}
hamm_hist[det->hamming]++;
}
apriltag_detections_destroy(detections);
if (!quiet) {
timeprofile_display(td->tp);
printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
printf("Hamming histogram: ");
for (int i = 0; i < hamm_hist_max; i++)
printf("%5d", hamm_hist[i]);
printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
printf("\n");
}
return tag_matches;
}
int main(int argc, char *argv[])
{
getopt_t *getopt = getopt_create();
getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");
getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");
getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use");
getopt_add_int(getopt, '\0', "border", "1", "Set tag family border size");
getopt_add_int(getopt, 'i', "iters", "1", "Repeat processing on input set this many times");
getopt_add_int(getopt, 't', "threads", "4", "Use this many CPU threads");
getopt_add_double(getopt, 'x', "decimate", "1.0", "Decimate input image by this factor");
getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input");
getopt_add_bool(getopt, '1', "refine-decode", 0, "Spend more time trying to decode tags");
getopt_add_bool(getopt, '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");
if (!getopt_parse(getopt, argc, argv, 1) || getopt_get_bool(getopt, "help")) {
printf("Usage: %s [options] <input files>\n", argv[0]);
getopt_do_usage(getopt);
exit(0);
}
const zarray_t *inputs = getopt_get_extra_args(getopt);
apriltag_family_t *tf = NULL;
const char *famname = getopt_get_string(getopt, "family");
if (!strcmp(famname, "tag36h11"))
tf = tag36h11_create();
else if (!strcmp(famname, "tag36h10"))
tf = tag36h10_create();
else if (!strcmp(famname, "tag36artoolkit"))
tf = tag36artoolkit_create();
else if (!strcmp(famname, "tag25h9"))
tf = tag25h9_create();
else if (!strcmp(famname, "tag25h7"))
tf = tag25h7_create();
else {
printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
exit(-1);
}
tf->black_border = getopt_get_int(getopt, "border");
apriltag_detector_t *td = apriltag_detector_create();
apriltag_detector_add_family(td, tf);
td->quad_decimate = getopt_get_double(getopt, "decimate");
td->quad_sigma = getopt_get_double(getopt, "blur");
td->nthreads = getopt_get_int(getopt, "threads");
td->debug = getopt_get_bool(getopt, "debug");
td->refine_decode = getopt_get_bool(getopt, "refine-decode");
td->refine_pose = getopt_get_bool(getopt, "refine-pose");
int quiet = getopt_get_bool(getopt, "quiet");
int maxiters = getopt_get_int(getopt, "iters");
const int hamm_hist_max = 10;
for (int iter = 0; iter < maxiters; iter++) {
if (maxiters > 1)
printf("iter %d / %d\n", iter + 1, maxiters);
for (int input = 0; input < zarray_size(inputs); input++) {
int hamm_hist[hamm_hist_max];
memset(hamm_hist, 0, sizeof(hamm_hist));
char *path;
zarray_get(inputs, input, &path);
if (!quiet)
printf("loading %s\n", path);
image_u8_t *im = image_u8_create_from_pnm(path);
if (im == NULL) {
printf("couldn't find %s\n", path);
continue;
}
zarray_t *detections = apriltag_detector_detect(td, im);
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
if (!quiet)
printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, goodness %8.3f, margin %8.3f\n",
i, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);
hamm_hist[det->hamming]++;
apriltag_detection_destroy(det);
}
zarray_destroy(detections);
if (!quiet) {
timeprofile_display(td->tp);
printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
}
if (!quiet)
printf("Hamming histogram: ");
for (int i = 0; i < hamm_hist_max; i++)
printf("%5d", hamm_hist[i]);
if (quiet) {
printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
}
printf("\n");
image_u8_destroy(im);
}
}
// don't deallocate contents of inputs; those are the argv
apriltag_detector_destroy(td);
tag36h11_destroy(tf);
return 0;
}
int main(){
bool showGradient = true;
bool found = false;
VideoCapture cap(0); // open the default camera
Size size(854,480); // size of desired frame origionally 1280x720, 1024x576, 854x480
if(!cap.isOpened()) // check if camera opened
return -1;
Mat frame;
Mat src;
/* From apriltag_demo.c */
int maxiters = 5;
const int hamm_hist_max = 10;
int quiet = 0;
apriltag_family_t *tf = tag36h11_create(); // Apriltag family 36h11, can change
tf->black_border = 1; // Set tag family border size
apriltag_detector_t *td = apriltag_detector_create(); // Apriltag detector
apriltag_detector_add_family(td, tf); // Add apriltag family
td->quad_decimate = 1.0; // Decimate input image by factor
td->quad_sigma = 0.0; // No blur (I think)
td->nthreads = 4; // 4 treads provided
td->debug = 0; // No debuging output
td->refine_decode = 0; // Don't refine decode
td->refine_pose = 0; // Don't refine pose
// Output variables
char imgSize[20];
char renderTime[20];
char detectString[50];
char convertTime[50];
char displayString[120];
char outputString[120];
char locationString[120];
double time_taken = 0.0;
long double totalFPS = 0.0;
double count = 0.0;
/* End of apriltag_demo.c */
while(1){
clock_t t;
t = clock();
cap >> src; // Get a new frame from camera
if(found){ resize(src,frame,size); } // Resize to smaller image if tag found
else{ frame = src; } // Keep standard image if no tag
if(showGradient){
cvtColor(src, frame, CV_BGR2GRAY);
cvtColor(frame, frame, CV_GRAY2RGB);
src = gradientEdges(frame); // Show gradient for fun
}else{
cvtColor(src, src, CV_BGR2GRAY);
}
pnm_t *pnm = mat2pnm(&frame); // Convert Mat fram to pnm
image_u8_t *im = pnm_to_image_u8(pnm); // Convert pnm to gray image_u8
if (im == NULL) { // Error - no image created from pnm
std::cout << "Error, not a proper pnm" << std::endl;
return -1;
}
/*** Start from origional Apriltags from apriltag_demo.c ***/
int hamm_hist[hamm_hist_max];
memset(hamm_hist, 0, sizeof(hamm_hist));
zarray_t *detections = apriltag_detector_detect(td, im);
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
sprintf(locationString, "Tag Center: (%f,%f)", det->c[0], det->c[1]);
sprintf(detectString, "detection %2d: id (%2dx%2d)-%-4d, hamming %d, goodness %5.3f, margin %5.3f\n",
i+1, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);
hamm_hist[det->hamming]++;
// draws a vertical rectangle around tag, not ideal, but easy to implement
// det->p[corner][positon], counter clockwise
Point pt1 = Point(det->p[0][0], det->p[0][1]);
Point pt2 = Point(det->p[2][0], det->p[2][1]);
cv::rectangle(src, pt1, pt2, cvScalar(102,255,0));
apriltag_detection_destroy(det);
}
if(zarray_size(detections) < 1){
found = false;
sprintf(detectString, "No tag detected");
sprintf(locationString, "No tag detected");
}else{
found = false;
}
zarray_destroy(detections);
image_u8_destroy(im);
t = clock() - t;
double time_taken = ((double)t)/(CLOCKS_PER_SEC/1000);
//printf("ms to render: %5.3f\n", time_taken);
if (!quiet) {
//timeprofile_display(td->tp);
totalFPS += (1000.0/time_taken);
count += 1.0;
if(count > 30000.0){
totalFPS = 0.0;
count = 0.0;
}
sprintf(displayString, "fps: %2.2Lf, nquads: %d",totalFPS/count, td->nquads);
//std::cout << displayString;
}
//for (int i = 0; i < hamm_hist_max; i++)
//printf("%5d", hamm_hist[i]);
sprintf(renderTime, "Render: %5.3fms", time_taken);
sprintf(imgSize, "%dx%d", frame.cols, frame.rows);
sprintf(outputString, "%s %s %s", renderTime, convertTime, imgSize);
printf("%s %s\r", detectString, outputString);
if (quiet) {
printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
}
printf("\n");
/*** End of origional Apriltags from apriltag_demo.c ***/
// displays fps, edges, segments, quads
putText(src, displayString, cvPoint(30,30),
FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
// displays render time, convert time, and image size
putText(src, outputString, cvPoint(30,50),
FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
// Displays any detections (if any)
putText(src, detectString, cvPoint(30,70),
FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
// Displays tag location (if any)
putText(src, locationString, cvPoint(30,90),
FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
imshow("Display Apriltags", src);
if(waitKey(30) >= 0) break;
}
/* deallocate apriltag constructs */
apriltag_detector_destroy(td);
tag36h11_destroy(tf);
return 0;
}