Squashed 'third_party/apriltag/' content from commit 3e8e974d0
git-subtree-dir: third_party/apriltag
git-subtree-split: 3e8e974d0d8d6ab318abf56d87506d15d7f2cc35
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I04ba3cb2106b6813a1013d57aa8074c26f856598
diff --git a/example/.gitignore b/example/.gitignore
new file mode 100644
index 0000000..1cef2e8
--- /dev/null
+++ b/example/.gitignore
@@ -0,0 +1 @@
+apriltag_demo
diff --git a/example/Makefile b/example/Makefile
new file mode 100644
index 0000000..6027428
--- /dev/null
+++ b/example/Makefile
@@ -0,0 +1,32 @@
+CC = gcc
+CXX = g++
+
+CPPFLAGS = -I.. `pkg-config --cflags opencv4`
+CFLAGS = -g -std=gnu99 -Wall -Wno-unused-parameter -Wno-unused-function -O3
+CXXFLAGS = -g -Wall -O3
+LDFLAGS = -lpthread -lm
+
+TARGETS := apriltag_demo opencv_demo
+
+.PHONY: all
+all: apriltag_demo opencv_demo
+
+apriltag_demo: apriltag_demo.o ../libapriltag.a
+ @echo " [$@]"
+ @$(CC) -o $@ $^ $(LDFLAGS)
+
+opencv_demo: opencv_demo.o ../libapriltag.a
+ @echo " [$@]"
+ @$(CXX) -o $@ $^ $(LDFLAGS) `pkg-config --libs opencv4`
+
+%.o: %.c
+ @echo " $@"
+ @$(CC) -o $@ -c $< $(CFLAGS) $(CPPFLAGS)
+
+%.o: %.cc
+ @echo " $@"
+ @$(CXX) -o $@ -c $< $(CXXFLAGS) $(CPPFLAGS)
+
+.PHONY: clean
+clean:
+ @rm -rf *.o $(TARGETS)
diff --git a/example/README b/example/README
new file mode 100644
index 0000000..5b3167d
--- /dev/null
+++ b/example/README
@@ -0,0 +1 @@
+These example programs are meant for distribution, and thus will not build in the april2 tree without modifications.
diff --git a/example/apriltag_demo.c b/example/apriltag_demo.c
new file mode 100644
index 0000000..6de9054
--- /dev/null
+++ b/example/apriltag_demo.c
@@ -0,0 +1,284 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson@umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdio.h>
+#include <stdint.h>
+#include <inttypes.h>
+#include <ctype.h>
+#include <math.h>
+#include <errno.h>
+
+#ifdef __linux__
+ #include <unistd.h>
+#endif
+
+#include "apriltag.h"
+#include "tag36h11.h"
+#include "tag25h9.h"
+#include "tag16h5.h"
+#include "tagCircle21h7.h"
+#include "tagCircle49h12.h"
+#include "tagCustom48h12.h"
+#include "tagStandard41h12.h"
+#include "tagStandard52h13.h"
+
+#include "common/getopt.h"
+#include "common/image_u8.h"
+#include "common/pjpeg.h"
+#include "common/zarray.h"
+
+#define HAMM_HIST_MAX 10
+
+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, 'i', "iters", "1", "Repeat processing on input set this many times");
+ getopt_add_int(getopt, 't', "threads", "1", "Use this many CPU threads");
+ getopt_add_int(getopt, 'a', "hamming", "1", "Detect tags with up to this many bit errors.");
+ getopt_add_double(getopt, 'x', "decimate", "2.0", "Decimate input image by this factor");
+ getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input; negative sharpens");
+ getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of 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, "tag25h9")) {
+ tf = tag25h9_create();
+ } else if (!strcmp(famname, "tag16h5")) {
+ tf = tag16h5_create();
+ } else if (!strcmp(famname, "tagCircle21h7")) {
+ tf = tagCircle21h7_create();
+ } else if (!strcmp(famname, "tagCircle49h12")) {
+ tf = tagCircle49h12_create();
+ } else if (!strcmp(famname, "tagStandard41h12")) {
+ tf = tagStandard41h12_create();
+ } else if (!strcmp(famname, "tagStandard52h13")) {
+ tf = tagStandard52h13_create();
+ } else if (!strcmp(famname, "tagCustom48h12")) {
+ tf = tagCustom48h12_create();
+ } else {
+ printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
+ exit(-1);
+ }
+
+ apriltag_detector_t *td = apriltag_detector_create();
+ apriltag_detector_add_family_bits(td, tf, getopt_get_int(getopt, "hamming"));
+
+ switch(errno){
+ case EINVAL:
+ printf("\"hamming\" parameter is out-of-range.\n");
+ exit(-1);
+ case ENOMEM:
+ printf("Unable to add family to detector due to insufficient memory to allocate the tag-family decoder. Try reducing \"hamming\" from %d or choose an alternative tag family.\n", getopt_get_int(getopt, "hamming"));
+ exit(-1);
+ }
+
+ 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_edges = getopt_get_bool(getopt, "refine-edges");
+
+ int quiet = getopt_get_bool(getopt, "quiet");
+
+ int maxiters = getopt_get_int(getopt, "iters");
+
+ for (int iter = 0; iter < maxiters; iter++) {
+
+ int total_quads = 0;
+ int total_hamm_hist[HAMM_HIST_MAX];
+ memset(total_hamm_hist, 0, sizeof(int)*HAMM_HIST_MAX);
+ double total_time = 0;
+
+ 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);
+ else
+ printf("%20s ", path);
+
+ image_u8_t *im = NULL;
+ if (str_ends_with(path, "pnm") || str_ends_with(path, "PNM") ||
+ str_ends_with(path, "pgm") || str_ends_with(path, "PGM"))
+ im = image_u8_create_from_pnm(path);
+ else if (str_ends_with(path, "jpg") || str_ends_with(path, "JPG")) {
+ int err = 0;
+ pjpeg_t *pjpeg = pjpeg_create_from_file(path, 0, &err);
+ if (pjpeg == NULL) {
+ printf("pjpeg failed to load: %s, error %d\n", path, err);
+ continue;
+ }
+
+ if (1) {
+ im = pjpeg_to_u8_baseline(pjpeg);
+ } else {
+ printf("illumination invariant\n");
+
+ image_u8x3_t *imc = pjpeg_to_u8x3_baseline(pjpeg);
+
+ im = image_u8_create(imc->width, imc->height);
+
+ for (int y = 0; y < imc->height; y++) {
+ for (int x = 0; x < imc->width; x++) {
+ double r = imc->buf[y*imc->stride + 3*x + 0] / 255.0;
+ double g = imc->buf[y*imc->stride + 3*x + 1] / 255.0;
+ double b = imc->buf[y*imc->stride + 3*x + 2] / 255.0;
+
+ double alpha = 0.42;
+ double v = 0.5 + log(g) - alpha*log(b) - (1-alpha)*log(r);
+ int iv = v * 255;
+ if (iv < 0)
+ iv = 0;
+ if (iv > 255)
+ iv = 255;
+
+ im->buf[y*im->stride + x] = iv;
+ }
+ }
+ image_u8x3_destroy(imc);
+ if (td->debug)
+ image_u8_write_pnm(im, "debug_invariant.pnm");
+ }
+
+ pjpeg_destroy(pjpeg);
+ }
+
+ if (im == NULL) {
+ printf("couldn't load %s\n", path);
+ continue;
+ }
+
+ printf("image: %s %dx%d\n", path, im->width, im->height);
+
+ zarray_t *detections = apriltag_detector_detect(td, im);
+
+ if (errno == EAGAIN) {
+ printf("Unable to create the %d threads requested.\n",td->nthreads);
+ exit(-1);
+ }
+
+ 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, margin %8.3f\n",
+ i, det->family->nbits, det->family->h, det->id, det->hamming, det->decision_margin);
+
+ hamm_hist[det->hamming]++;
+ total_hamm_hist[det->hamming]++;
+ }
+
+ apriltag_detections_destroy(detections);
+
+ if (!quiet) {
+ timeprofile_display(td->tp);
+ }
+
+ total_quads += td->nquads;
+
+ if (!quiet)
+ printf("hamm ");
+
+ for (int i = 0; i < HAMM_HIST_MAX; i++)
+ printf("%5d ", hamm_hist[i]);
+
+ double t = timeprofile_total_utime(td->tp) / 1.0E3;
+ total_time += t;
+ printf("%12.3f ", t);
+ printf("%5d", td->nquads);
+
+ printf("\n");
+
+ image_u8_destroy(im);
+ }
+
+
+ printf("Summary\n");
+
+ printf("hamm ");
+
+ for (int i = 0; i < HAMM_HIST_MAX; i++)
+ printf("%5d ", total_hamm_hist[i]);
+ printf("%12.3f ", total_time);
+ printf("%5d", total_quads);
+ printf("\n");
+
+ }
+
+ // don't deallocate contents of inputs; those are the argv
+ apriltag_detector_destroy(td);
+
+ if (!strcmp(famname, "tag36h11")) {
+ tag36h11_destroy(tf);
+ } else if (!strcmp(famname, "tag25h9")) {
+ tag25h9_destroy(tf);
+ } else if (!strcmp(famname, "tag16h5")) {
+ tag16h5_destroy(tf);
+ } else if (!strcmp(famname, "tagCircle21h7")) {
+ tagCircle21h7_destroy(tf);
+ } else if (!strcmp(famname, "tagCircle49h12")) {
+ tagCircle49h12_destroy(tf);
+ } else if (!strcmp(famname, "tagStandard41h12")) {
+ tagStandard41h12_destroy(tf);
+ } else if (!strcmp(famname, "tagStandard52h13")) {
+ tagStandard52h13_destroy(tf);
+ } else if (!strcmp(famname, "tagCustom48h12")) {
+ tagCustom48h12_destroy(tf);
+ }
+
+ getopt_destroy(getopt);
+
+ return 0;
+}
diff --git a/example/opencv_demo.cc b/example/opencv_demo.cc
new file mode 100644
index 0000000..b43a46e
--- /dev/null
+++ b/example/opencv_demo.cc
@@ -0,0 +1,217 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson@umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <iostream>
+#include <iomanip>
+
+#include "opencv2/opencv.hpp"
+
+extern "C" {
+#include "apriltag.h"
+#include "tag36h11.h"
+#include "tag25h9.h"
+#include "tag16h5.h"
+#include "tagCircle21h7.h"
+#include "tagCircle49h12.h"
+#include "tagCustom48h12.h"
+#include "tagStandard41h12.h"
+#include "tagStandard52h13.h"
+#include "common/getopt.h"
+}
+
+using namespace std;
+using namespace cv;
+
+
+int main(int argc, char *argv[])
+{
+ getopt_t *getopt = getopt_create();
+
+ getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
+ getopt_add_int(getopt, 'c', "camera", "0", "camera ID");
+ 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, 't', "threads", "1", "Use this many CPU threads");
+ getopt_add_double(getopt, 'x', "decimate", "2.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, '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
+
+ if (!getopt_parse(getopt, argc, argv, 1) ||
+ getopt_get_bool(getopt, "help")) {
+ printf("Usage: %s [options]\n", argv[0]);
+ getopt_do_usage(getopt);
+ exit(0);
+ }
+
+ cout << "Enabling video capture" << endl;
+
+ TickMeter meter;
+ meter.start();
+
+ // Initialize camera
+ VideoCapture cap(getopt_get_int(getopt, "camera"));
+ if (!cap.isOpened()) {
+ cerr << "Couldn't open video capture device" << endl;
+ return -1;
+ }
+
+ // Initialize tag detector with options
+ apriltag_family_t *tf = NULL;
+ const char *famname = getopt_get_string(getopt, "family");
+ if (!strcmp(famname, "tag36h11")) {
+ tf = tag36h11_create();
+ } else if (!strcmp(famname, "tag25h9")) {
+ tf = tag25h9_create();
+ } else if (!strcmp(famname, "tag16h5")) {
+ tf = tag16h5_create();
+ } else if (!strcmp(famname, "tagCircle21h7")) {
+ tf = tagCircle21h7_create();
+ } else if (!strcmp(famname, "tagCircle49h12")) {
+ tf = tagCircle49h12_create();
+ } else if (!strcmp(famname, "tagStandard41h12")) {
+ tf = tagStandard41h12_create();
+ } else if (!strcmp(famname, "tagStandard52h13")) {
+ tf = tagStandard52h13_create();
+ } else if (!strcmp(famname, "tagCustom48h12")) {
+ tf = tagCustom48h12_create();
+ } else {
+ printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
+ exit(-1);
+ }
+
+
+ apriltag_detector_t *td = apriltag_detector_create();
+ apriltag_detector_add_family(td, tf);
+
+ if (errno == ENOMEM) {
+ printf("Unable to add family to detector due to insufficient memory to allocate the tag-family decoder with the default maximum hamming value of 2. Try choosing an alternative tag family.\n");
+ exit(-1);
+ }
+
+ 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_edges = getopt_get_bool(getopt, "refine-edges");
+
+ float frame_counter = 0.0f;
+ meter.stop();
+ cout << "Detector " << famname << " initialized in "
+ << std::fixed << std::setprecision(3) << meter.getTimeSec() << " seconds" << endl;
+#if CV_MAJOR_VERSION > 3
+ cout << " " << cap.get(CAP_PROP_FRAME_WIDTH ) << "x" <<
+ cap.get(CAP_PROP_FRAME_HEIGHT ) << " @" <<
+ cap.get(CAP_PROP_FPS) << "FPS" << endl;
+#else
+ cout << " " << cap.get(CV_CAP_PROP_FRAME_WIDTH ) << "x" <<
+ cap.get(CV_CAP_PROP_FRAME_HEIGHT ) << " @" <<
+ cap.get(CV_CAP_PROP_FPS) << "FPS" << endl;
+#endif
+ meter.reset();
+
+ Mat frame, gray;
+ while (true) {
+ errno = 0;
+ cap >> frame;
+ cvtColor(frame, gray, COLOR_BGR2GRAY);
+
+ // Make an image_u8_t header for the Mat data
+ image_u8_t im = { .width = gray.cols,
+ .height = gray.rows,
+ .stride = gray.cols,
+ .buf = gray.data
+ };
+
+ zarray_t *detections = apriltag_detector_detect(td, &im);
+
+ if (errno == EAGAIN) {
+ printf("Unable to create the %d threads requested.\n",td->nthreads);
+ exit(-1);
+ }
+
+ // Draw detection outlines
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+ line(frame, Point(det->p[0][0], det->p[0][1]),
+ Point(det->p[1][0], det->p[1][1]),
+ Scalar(0, 0xff, 0), 2);
+ line(frame, Point(det->p[0][0], det->p[0][1]),
+ Point(det->p[3][0], det->p[3][1]),
+ Scalar(0, 0, 0xff), 2);
+ line(frame, Point(det->p[1][0], det->p[1][1]),
+ Point(det->p[2][0], det->p[2][1]),
+ Scalar(0xff, 0, 0), 2);
+ line(frame, Point(det->p[2][0], det->p[2][1]),
+ Point(det->p[3][0], det->p[3][1]),
+ Scalar(0xff, 0, 0), 2);
+
+ stringstream ss;
+ ss << det->id;
+ String text = ss.str();
+ int fontface = FONT_HERSHEY_SCRIPT_SIMPLEX;
+ double fontscale = 1.0;
+ int baseline;
+ Size textsize = getTextSize(text, fontface, fontscale, 2,
+ &baseline);
+ putText(frame, text, Point(det->c[0]-textsize.width/2,
+ det->c[1]+textsize.height/2),
+ fontface, fontscale, Scalar(0xff, 0x99, 0), 2);
+ }
+ apriltag_detections_destroy(detections);
+
+ imshow("Tag Detections", frame);
+ if (waitKey(30) >= 0)
+ break;
+ }
+
+ apriltag_detector_destroy(td);
+
+ if (!strcmp(famname, "tag36h11")) {
+ tag36h11_destroy(tf);
+ } else if (!strcmp(famname, "tag25h9")) {
+ tag25h9_destroy(tf);
+ } else if (!strcmp(famname, "tag16h5")) {
+ tag16h5_destroy(tf);
+ } else if (!strcmp(famname, "tagCircle21h7")) {
+ tagCircle21h7_destroy(tf);
+ } else if (!strcmp(famname, "tagCircle49h12")) {
+ tagCircle49h12_destroy(tf);
+ } else if (!strcmp(famname, "tagStandard41h12")) {
+ tagStandard41h12_destroy(tf);
+ } else if (!strcmp(famname, "tagStandard52h13")) {
+ tagStandard52h13_destroy(tf);
+ } else if (!strcmp(famname, "tagCustom48h12")) {
+ tagCustom48h12_destroy(tf);
+ }
+
+
+ getopt_destroy(getopt);
+
+ return 0;
+}