Merge pull request #1570 from szk1509:apriltag

Apriltag (#1570)

* doCornerRefinement to CornerRefinementMethod :: detected contours points are used to detect the corners

* some little corrections

* samples edited

* documented :)

* tabs corrected

* Docu corrections

* refinement for all candidates

* refinement for all candidates :: copy paste error corrected

* comment

* apriltag

* whitespace corrected

* corr ini

* correction :: warnings, c4244, preprocDirect, ...

* try to ignore 4244, fix 2131, add test, and ...

* try to ignore 4244

* test duplicate deleted

* corrected test, warnings

* test :: correction

* warnings

* warnings and test

* warnings

* perspective test, warning corrections

* warning a_q_t

* warning

* warning

* 3 clause BSD license

* stacksz and typo

* eliminate build warnings

- cv::fastAtan2()
- cvFloor()

* small code refactoring

* fix isfinite()

* get rid of manual calloc/free calls

* update file headers

modules/aruco/include/opencv2/aruco.hpp

@@ -79,7 +79,8 @@ namespace aruco {
 enum CornerRefineMethod{
 	CORNER_REFINE_NONE,     // default corners
 	CORNER_REFINE_SUBPIX,   // refine the corners using subpix
-	CORNER_REFINE_CONTOUR   // refine the corners using the contour-points
+	CORNER_REFINE_CONTOUR,  // refine the corners using the contour-points
+	CORNER_REFINE_APRILTAG, // detect corners using the AprilTag2 approach
 };
 
 /**
@@ -105,7 +106,8 @@ enum CornerRefineMethod{
  *   similar, so that the smaller one is removed. The rate is relative to the smaller perimeter
  *   of the two markers (default 0.05).
  * - cornerRefinementMethod: corner refinement method. (CORNER_REFINE_NONE, no refinement.
- *   CORNER_REFINE_SUBPIX, do subpixel refinement. CORNER_REFINE_CONTOUR use contour-Points)
+ *   CORNER_REFINE_SUBPIX, do subpixel refinement. CORNER_REFINE_CONTOUR use contour-Points,
+ *   CORNER_REFINE_APRILTAG  use the AprilTag2 approach)
  * - cornerRefinementWinSize: window size for the corner refinement process (in pixels) (default 5).
  * - cornerRefinementMaxIterations: maximum number of iterations for stop criteria of the corner
  *   refinement process (default 30).
@@ -125,6 +127,21 @@ enum CornerRefineMethod{
  *   than 128 or not) (default 5.0)
  * - errorCorrectionRate error correction rate respect to the maximun error correction capability
  *   for each dictionary. (default 0.6).
+ * - aprilTagMinClusterPixels: reject quads containing too few pixels.
+ * - aprilTagMaxNmaxima: how many corner candidates to consider when segmenting a group of pixels into a quad.
+ * - aprilTagCriticalRad: Reject quads where pairs of edges have angles that are close to straight or close to
+ *   180 degrees. Zero means that no quads are rejected. (In radians).
+ * - aprilTagMaxLineFitMse:  When fitting lines to the contours, what is the maximum mean squared error
+ *   allowed?  This is useful in rejecting contours that are far from being quad shaped; rejecting
+ *   these quads "early" saves expensive decoding processing.
+ * - aprilTagMinWhiteBlackDiff: When we build our model of black & white pixels, we add an extra check that
+ *   the white model must be (overall) brighter than the black model.  How much brighter? (in pixel values, [0,255]).
+ * - aprilTagDeglitch:  should the thresholded image be deglitched? Only useful for very noisy images
+ * - aprilTagQuadDecimate: Detection of quads can be done on a lower-resolution image, improving speed at a
+ *   cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still
+ *   done at full resolution.
+ * - aprilTagQuadSigma: What Gaussian blur should be applied to the segmented image (used for quad detection?)
+ *   Parameter is the standard deviation in pixels.  Very noisy images benefit from non-zero values (e.g. 0.8).
  */
 struct CV_EXPORTS_W DetectorParameters {
 
@@ -152,6 +169,18 @@ struct CV_EXPORTS_W DetectorParameters {
     CV_PROP_RW double maxErroneousBitsInBorderRate;
     CV_PROP_RW double minOtsuStdDev;
     CV_PROP_RW double errorCorrectionRate;
+
+    // April :: User-configurable parameters.
+    CV_PROP_RW float aprilTagQuadDecimate;
+    CV_PROP_RW float aprilTagQuadSigma;
+
+    // April :: Internal variables
+    CV_PROP_RW int aprilTagMinClusterPixels;
+    CV_PROP_RW int aprilTagMaxNmaxima;
+    CV_PROP_RW float aprilTagCriticalRad;
+    CV_PROP_RW float aprilTagMaxLineFitMse;
+    CV_PROP_RW int aprilTagMinWhiteBlackDiff;
+    CV_PROP_RW int aprilTagDeglitch;
 };
 
 

modules/aruco/src/apriltag_quad_thresh.hpp

+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// 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.
+//
+// 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.
+
+// limitation: image size must be <32768 in width and height. This is
+// because we use a fixed-point 16 bit integer representation with one
+// fractional bit.
+
+#ifndef _OPENCV_APRIL_QUAD_THRESH_HPP_
+#define _OPENCV_APRIL_QUAD_THRESH_HPP_
+
+#include "opencv2/aruco.hpp"
+#include "unionfind.hpp"
+#include "zmaxheap.hpp"
+#include "zarray.hpp"
+
+namespace cv {
+namespace aruco {
+
+static inline uint32_t u64hash_2(uint64_t x) {
+    return uint32_t((2654435761UL * x) >> 32);
+}
+
+struct uint64_zarray_entry{
+    uint64_t id;
+    zarray_t *cluster;
+
+    struct uint64_zarray_entry *next;
+};
+
+struct pt{
+    // Note: these represent 2*actual value.
+    uint16_t x, y;
+    float theta;
+    int16_t gx, gy;
+};
+
+struct remove_vertex{
+    int i;           // which vertex to remove?
+    int left, right; // left vertex, right vertex
+
+    double err;
+};
+
+struct segment{
+    int is_vertex;
+
+    // always greater than zero, but right can be > size, which denotes
+    // a wrap around back to the beginning of the points. and left < right.
+    int left, right;
+};
+
+struct line_fit_pt{
+    double Mx, My;
+    double Mxx, Myy, Mxy;
+    double W; // total weight
+};
+
+/**
+ * lfps contains *cumulative* moments for N points, with
+ * index j reflecting points [0,j] (inclusive).
+ * fit a line to the points [i0, i1] (inclusive). i0, i1 are both (0, sz)
+ * if i1 < i0, we treat this as a wrap around.
+ */
+void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, double *lineparm, double *err, double *mse);
+
+int err_compare_descending(const void *_a, const void *_b);
+
+/**
+  1. Identify A) white points near a black point and B) black points near a white point.
+
+  2. Find the connected components within each of the classes above,
+  yielding clusters of "white-near-black" and
+  "black-near-white". (These two classes are kept separate). Each
+  segment has a unique id.
+
+  3. For every pair of "white-near-black" and "black-near-white"
+  clusters, find the set of points that are in one and adjacent to the
+  other. In other words, a "boundary" layer between the two
+  clusters. (This is actually performed by iterating over the pixels,
+  rather than pairs of clusters.) Critically, this helps keep nearby
+  edges from becoming connected.
+ **/
+int quad_segment_maxima(const Ptr<DetectorParameters> &td, int sz, struct line_fit_pt *lfps, int indices[4]);
+
+/**
+ * returns 0 if the cluster looks bad.
+ */
+int quad_segment_agg(int sz, struct line_fit_pt *lfps, int indices[4]);
+
+/**
+ *  return 1 if the quad looks okay, 0 if it should be discarded
+ *  quad
+ **/
+int fit_quad(const Ptr<DetectorParameters> &_params, const Mat im, zarray_t *cluster, struct sQuad *quad);
+
+/**
+ *
+ * @param mIm
+ * @param parameters
+ * @param mThresh
+ */
+void threshold(const Mat mIm, const Ptr<DetectorParameters> &parameters, Mat& mThresh);
+
+/**
+ *
+ * @param parameters
+ * @param mImg
+ * @param contours
+ * @return
+ */
+zarray_t *apriltag_quad_thresh(const Ptr<DetectorParameters> &parameters, const Mat & mImg, std::vector< std::vector< Point > > &contours);
+
+}}
+#endif

modules/aruco/src/aruco.cpp

@@ -41,14 +41,20 @@ the use of this software, even if advised of the possibility of such damage.
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 
+#include "apriltag_quad_thresh.hpp"
+#include "zarray.hpp"
+
+//#define APRIL_DEBUG
+#ifdef APRIL_DEBUG
+#include "opencv2/imgcodecs.hpp"
+#endif
+
 namespace cv {
 namespace aruco {
 
 using namespace std;
 
 
-
-
 /**
   *
   */
@@ -72,7 +78,15 @@ DetectorParameters::DetectorParameters()
       perspectiveRemoveIgnoredMarginPerCell(0.13),
       maxErroneousBitsInBorderRate(0.35),
       minOtsuStdDev(5.0),
-      errorCorrectionRate(0.6) {}
+      errorCorrectionRate(0.6),
+      aprilTagQuadDecimate(0.0),
+      aprilTagQuadSigma(0.0),
+      aprilTagMinClusterPixels(5),
+      aprilTagMaxNmaxima(10),
+      aprilTagCriticalRad( (float)(10* CV_PI /180) ),
+      aprilTagMaxLineFitMse(10.0),
+      aprilTagMinWhiteBlackDiff(5),
+      aprilTagDeglitch(0){}
 
 
 /**
@@ -950,6 +964,137 @@ class MarkerContourParallel : public ParallelLoopBody {
     const Mat& distCoeff;
 };
 
+#ifdef APRIL_DEBUG
+static void _darken(const Mat &im){
+    for (int y = 0; y < im.rows; y++) {
+        for (int x = 0; x < im.cols; x++) {
+            im.data[im.cols*y+x] /= 2;
+        }
+    }
+}
+#endif
+
+/**
+ *
+ * @param im_orig
+ * @param _params
+ * @param candidates
+ * @param contours
+ */
+static void _apriltag(Mat im_orig, const Ptr<DetectorParameters> & _params, std::vector< std::vector< Point2f > > &candidates,
+        std::vector< std::vector< Point > > &contours){
+
+    ///////////////////////////////////////////////////////////
+    /// Step 1. Detect quads according to requested image decimation
+    /// and blurring parameters.
+    Mat quad_im;
+    im_orig.copyTo(quad_im);
+
+    if (_params->aprilTagQuadDecimate > 1){
+        resize(im_orig, quad_im, Size(), 1/_params->aprilTagQuadDecimate, 1/_params->aprilTagQuadDecimate, INTER_AREA );
+    }
+
+    // Apply a Blur
+    if (_params->aprilTagQuadSigma != 0) {
+        // compute a reasonable kernel width by figuring that the
+        // kernel should go out 2 std devs.
+        //
+        // max sigma          ksz
+        // 0.499              1  (disabled)
+        // 0.999              3
+        // 1.499              5
+        // 1.999              7
+
+        float sigma = fabsf((float) _params->aprilTagQuadSigma);
+
+        int ksz = cvFloor(4 * sigma); // 2 std devs in each direction
+        ksz |= 1; // make odd number
+
+        if (ksz > 1) {
+            if (_params->aprilTagQuadSigma > 0)
+                GaussianBlur(quad_im, quad_im, Size(ksz, ksz), sigma, sigma, BORDER_REPLICATE);
+            else {
+                Mat orig;
+                quad_im.copyTo(orig);
+                GaussianBlur(quad_im, quad_im, Size(ksz, ksz), sigma, sigma, BORDER_REPLICATE);
+
+                // SHARPEN the image by subtracting the low frequency components.
+                for (int y = 0; y < orig.rows; y++) {
+                    for (int x = 0; x < orig.cols; x++) {
+                        int vorig = orig.data[y*orig.step + x];
+                        int vblur = quad_im.data[y*quad_im.step + x];
+
+                        int v = 2*vorig - vblur;
+                        if (v < 0)
+                            v = 0;
+                        if (v > 255)
+                            v = 255;
+
+                        quad_im.data[y*quad_im.step + x] = (uint8_t) v;
+                    }
+                }
+            }
+        }
+    }
+
+#ifdef APRIL_DEBUG
+    imwrite("1.1 debug_preprocess.pnm", quad_im);
+#endif
+
+    ///////////////////////////////////////////////////////////
+    /// Step 2. do the Threshold :: get the set of candidate quads
+    zarray_t *quads = apriltag_quad_thresh(_params, quad_im, contours);
+
+    CV_Assert(quads != NULL);
+
+    // adjust centers of pixels so that they correspond to the
+    // original full-resolution image.
+    if (_params->aprilTagQuadDecimate > 1) {
+        for (int i = 0; i < _zarray_size(quads); i++) {
+            struct sQuad *q;
+            _zarray_get_volatile(quads, i, &q);
+            for (int j = 0; j < 4; j++) {
+                q->p[j][0] *= _params->aprilTagQuadDecimate;
+                q->p[j][1] *= _params->aprilTagQuadDecimate;
+            }
+        }
+    }
+
+#ifdef APRIL_DEBUG
+    Mat im_quads = im_orig.clone();
+    im_quads = im_quads*0.5;
+    srandom(0);
+
+    for (int i = 0; i < _zarray_size(quads); i++) {
+        struct sQuad *quad;
+        _zarray_get_volatile(quads, i, &quad);
+
+        const int bias = 100;
+        int color = bias + (random() % (255-bias));
+
+        line(im_quads, Point(quad->p[0][0], quad->p[0][1]), Point(quad->p[1][0], quad->p[1][1]), color, 1);
+        line(im_quads, Point(quad->p[1][0], quad->p[1][1]), Point(quad->p[2][0], quad->p[2][1]), color, 1);
+        line(im_quads, Point(quad->p[2][0], quad->p[2][1]), Point(quad->p[3][0], quad->p[3][1]), color, 1);
+        line(im_quads, Point(quad->p[3][0], quad->p[3][1]), Point(quad->p[0][0], quad->p[0][1]), color, 1);
+    }
+    imwrite("1.2 debug_quads_raw.pnm", im_quads);
+#endif
+
+    ////////////////////////////////////////////////////////////////
+    /// Step 3. Save the output :: candidate corners
+    for (int i = 0; i < _zarray_size(quads); i++) {
+        struct sQuad *quad;
+        _zarray_get_volatile(quads, i, &quad);
+
+        std::vector< Point2f > corners;
+        corners.push_back(Point2f(quad->p[3][0], quad->p[3][1]));   //pA
+        corners.push_back(Point2f(quad->p[0][0], quad->p[0][1]));   //pB
+        corners.push_back(Point2f(quad->p[1][0], quad->p[1][1]));   //pC
+        corners.push_back(Point2f(quad->p[2][0], quad->p[2][1]));   //pD
+
+        candidates.push_back(corners);
+    }
+}
 
 
 /**
@@ -967,7 +1112,14 @@ void detectMarkers(InputArray _image, const Ptr<Dictionary> &_dictionary, Output
     vector< vector< Point2f > > candidates;
     vector< vector< Point > > contours;
     vector< int > ids;
-    _detectCandidates(grey, candidates, contours, _params);
+
+    /// STEP 1.a Detect marker candidates :: using AprilTag
+    if(_params->cornerRefinementMethod == CORNER_REFINE_APRILTAG)
+        _apriltag(grey, _params, candidates, contours);
+
+    /// STEP 1.b Detect marker candidates :: traditional way
+    else
+        _detectCandidates(grey, candidates, contours, _params);
 
     /// STEP 2: Check candidate codification (identify markers)
     _identifyCandidates(grey, candidates, contours, _dictionary, candidates, ids, _params,

modules/aruco/src/unionfind.hpp

+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// 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.
+//
+// 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.
+#ifndef _OPENCV_UNIONFIND_HPP_
+#define _OPENCV_UNIONFIND_HPP_
+
+#include <stdint.h>
+#include <stdlib.h>
+namespace cv {
+namespace aruco {
+
+typedef struct unionfind unionfind_t;
+struct unionfind{
+    uint32_t maxid;
+    struct ufrec *data;
+};
+
+struct ufrec{
+    // the parent of this node. If a node's parent is its own index,
+    // then it is a root.
+    uint32_t parent;
+
+    // for the root of a connected component, the number of components
+    // connected to it. For intermediate values, it's not meaningful.
+    uint32_t size;
+};
+
+static inline unionfind_t *unionfind_create(uint32_t maxid){
+    unionfind_t *uf = (unionfind_t*) calloc(1, sizeof(unionfind_t));
+    uf->maxid = maxid;
+    uf->data = (struct ufrec*) malloc((maxid+1) * sizeof(struct ufrec));
+    for (unsigned int i = 0; i <= maxid; i++) {
+        uf->data[i].size = 1;
+        uf->data[i].parent = i;
+    }
+    return uf;
+}
+
+static inline void unionfind_destroy(unionfind_t *uf){
+    free(uf->data);
+    free(uf);
+}
+
+/*
+static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id)
+{
+    // base case: a node is its own parent
+    if (uf->data[id].parent == id)
+        return id;
+
+    // otherwise, recurse
+    uint32_t root = unionfind_get_representative(uf, uf->data[id].parent);
+
+    // short circuit the path. [XXX This write prevents tail recursion]
+    uf->data[id].parent = root;
+
+    return root;
+}
+ */
+
+// this one seems to be every-so-slightly faster than the recursive
+// version above.
+static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id){
+    uint32_t root = id;
+
+    // chase down the root
+    while (uf->data[root].parent != root) {
+        root = uf->data[root].parent;
+    }
+
+    // go back and collapse the tree.
+    //
+    // XXX: on some of our workloads that have very shallow trees
+    // (e.g. image segmentation), we are actually faster not doing
+    // this...
+    while (uf->data[id].parent != root) {
+        uint32_t tmp = uf->data[id].parent;
+        uf->data[id].parent = root;
+        id = tmp;
+    }
+
+    return root;
+}
+
+static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id){
+    uint32_t repid = unionfind_get_representative(uf, id);
+    return uf->data[repid].size;
+}
+
+static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid){
+    uint32_t aroot = unionfind_get_representative(uf, aid);
+    uint32_t broot = unionfind_get_representative(uf, bid);
+
+    if (aroot == broot)
+        return aroot;
+
+    // we don't perform "union by rank", but we perform a similar
+    // operation (but probably without the same asymptotic guarantee):
+    // We join trees based on the number of *elements* (as opposed to
+    // rank) contained within each tree. I.e., we use size as a proxy
+    // for rank.  In my testing, it's often *faster* to use size than
+    // rank, perhaps because the rank of the tree isn't that critical
+    // if there are very few nodes in it.
+    uint32_t asize = uf->data[aroot].size;
+    uint32_t bsize = uf->data[broot].size;
+
+    // optimization idea: We could shortcut some or all of the tree
+    // that is grafted onto the other tree. Pro: those nodes were just
+    // read and so are probably in cache. Con: it might end up being
+    // wasted effort -- the tree might be grafted onto another tree in
+    // a moment!
+    if (asize > bsize) {
+        uf->data[broot].parent = aroot;
+        uf->data[aroot].size += bsize;
+        return aroot;
+    } else {
+        uf->data[aroot].parent = broot;
+        uf->data[broot].size += asize;
+        return broot;
+    }
+}
+}}
+#endif

modules/aruco/src/zarray.hpp

+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// 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.
+//
+// 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.
+#ifndef _OPENCV_ZARRAY_HPP_
+#define _OPENCV_ZARRAY_HPP_
+
+#include <stdlib.h>
+#include <string.h>
+
+namespace cv {
+namespace aruco {
+
+
+struct sQuad{
+    float p[4][2]; // corners
+};
+
+/**
+ * Defines a structure which acts as a resize-able array ala Java's ArrayList.
+ */
+typedef struct zarray zarray_t;
+struct zarray{
+    size_t el_sz; // size of each element
+
+    int size; // how many elements?
+    int alloc; // we've allocated storage for how many elements?
+    char *data;
+};
+
+/**
+ * Creates and returns a variable array structure capable of holding elements of
+ * the specified size. It is the caller's responsibility to call zarray_destroy()
+ * on the returned array when it is no longer needed.
+ */
+inline static zarray_t *_zarray_create(size_t el_sz){
+    zarray_t *za = (zarray_t*) calloc(1, sizeof(zarray_t));
+    za->el_sz = el_sz;
+    return za;
+}
+
+/**
+ * Frees all resources associated with the variable array structure which was
+ * created by zarray_create(). After calling, 'za' will no longer be valid for storage.
+ */
+inline static void _zarray_destroy(zarray_t *za){
+    if (za == NULL)
+        return;
+
+    if (za->data != NULL)
+        free(za->data);
+    memset(za, 0, sizeof(zarray_t));
+    free(za);
+}
+
+/**
+ * Retrieves the number of elements currently being contained by the passed
+ * array, which may be different from its capacity. The index of the last element
+ * in the array will be one less than the returned value.
+ */
+inline static int _zarray_size(const zarray_t *za){
+    return za->size;
+}
+
+/**
+ * Allocates enough internal storage in the supplied variable array structure to
+ * guarantee that the supplied number of elements (capacity) can be safely stored.
+ */
+inline static void _zarray_ensure_capacity(zarray_t *za, int capacity){
+    if (capacity <= za->alloc)
+        return;
+
+    while (za->alloc < capacity) {
+        za->alloc *= 2;
+        if (za->alloc < 8)
+            za->alloc = 8;
+    }
+
+    za->data = (char*) realloc(za->data, za->alloc * za->el_sz);
+}
+
+/**
+ * Adds a new element to the end of the supplied array, and sets its value
+ * (by copying) from the data pointed to by the supplied pointer 'p'.
+ * Automatically ensures that enough storage space is available for the new element.
+ */
+inline static void _zarray_add(zarray_t *za, const void *p){
+    _zarray_ensure_capacity(za, za->size + 1);
+
+    memcpy(&za->data[za->size*za->el_sz], p, za->el_sz);
+    za->size++;
+}
+
+/**
+ * Retrieves the element from the supplied array located at the zero-based
+ * index of 'idx' and copies its value into the variable pointed to by the pointer
+ * 'p'.
+ */
+inline static void _zarray_get(const zarray_t *za, int idx, void *p){
+    CV_DbgAssert(idx >= 0);
+    CV_DbgAssert(idx < za->size);
+
+    memcpy(p, &za->data[idx*za->el_sz], za->el_sz);
+}
+
+/**
+ * Similar to zarray_get(), but returns a "live" pointer to the internal
+ * storage, avoiding a memcpy. This pointer is not valid across
+ * operations which might move memory around (i.e. zarray_remove_value(),
+ * zarray_remove_index(), zarray_insert(), zarray_sort(), zarray_clear()).
+ * 'p' should be a pointer to the pointer which will be set to the internal address.
+ */
+inline static void _zarray_get_volatile(const zarray_t *za, int idx, void *p){
+    CV_DbgAssert(idx >= 0);
+    CV_DbgAssert(idx < za->size);
+
+    *((void**) p) = &za->data[idx*za->el_sz];
+}
+
+inline static void _zarray_truncate(zarray_t *za, int sz){
+    za->size = sz;
+}
+
+/**
+ * Sets the value of the current element at index 'idx' by copying its value from
+ * the data pointed to by 'p'. The previous value of the changed element will be
+ * copied into the data pointed to by 'outp' if it is not null.
+ */
+static inline void _zarray_set(zarray_t *za, int idx, const void *p, void *outp){
+    CV_DbgAssert(idx >= 0);
+    CV_DbgAssert(idx < za->size);
+
+    if (outp != NULL)
+        memcpy(outp, &za->data[idx*za->el_sz], za->el_sz);
+
+    memcpy(&za->data[idx*za->el_sz], p, za->el_sz);
+}
+
+}
+}
+#endif

modules/aruco/src/zmaxheap.cpp

+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// 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.
+//
+// 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 "precomp.hpp"
+#include "zmaxheap.hpp"
+
+
+//                 0
+//         1               2
+//      3     4        5       6
+//     7 8   9 10    11 12   13 14
+//
+// Children of node i:  2*i+1, 2*i+2
+// Parent of node i: (i-1) / 2
+//
+// Heap property: a parent is greater than (or equal to) its children.
+
+#define MIN_CAPACITY 16
+namespace cv {
+namespace aruco {
+struct zmaxheap
+{
+    size_t el_sz;
+
+    int size;
+    int alloc;
+
+    float *values;
+    char *data;
+
+    void (*swap)(zmaxheap_t *heap, int a, int b);
+};
+
+static inline void _swap_default(zmaxheap_t *heap, int a, int b)
+{
+    float t = heap->values[a];
+    heap->values[a] = heap->values[b];
+    heap->values[b] = t;
+
+    cv::AutoBuffer<char> tmp(heap->el_sz);
+    memcpy(tmp, &heap->data[a*heap->el_sz], heap->el_sz);
+    memcpy(&heap->data[a*heap->el_sz], &heap->data[b*heap->el_sz], heap->el_sz);
+    memcpy(&heap->data[b*heap->el_sz], tmp, heap->el_sz);
+}
+
+static inline void _swap_pointer(zmaxheap_t *heap, int a, int b)
+{
+    float t = heap->values[a];
+    heap->values[a] = heap->values[b];
+    heap->values[b] = t;
+
+    void **pp = (void**) heap->data;
+    void *tmp = pp[a];
+    pp[a] = pp[b];
+    pp[b] = tmp;
+}
+
+
+zmaxheap_t *zmaxheap_create(size_t el_sz)
+{
+    zmaxheap_t *heap = (zmaxheap_t*)calloc(1, sizeof(zmaxheap_t));
+    heap->el_sz = el_sz;
+
+    heap->swap = _swap_default;
+
+    if (el_sz == sizeof(void*))
+        heap->swap = _swap_pointer;
+
+    return heap;
+}
+
+void zmaxheap_destroy(zmaxheap_t *heap)
+{
+    free(heap->values);
+    free(heap->data);
+    memset(heap, 0, sizeof(zmaxheap_t));
+    free(heap);
+}
+
+static void _zmaxheap_ensure_capacity(zmaxheap_t *heap, int capacity)
+{
+    if (heap->alloc >= capacity)
+        return;
+
+    int newcap = heap->alloc;
+
+    while (newcap < capacity) {
+        if (newcap < MIN_CAPACITY) {
+            newcap = MIN_CAPACITY;
+            continue;
+        }
+
+        newcap *= 2;
+    }
+
+    heap->values = (float*)realloc(heap->values, newcap * sizeof(float));
+    heap->data = (char*)realloc(heap->data, newcap * heap->el_sz);
+    heap->alloc = newcap;
+}
+
+void zmaxheap_add(zmaxheap_t *heap, void *p, float v)
+{
+    _zmaxheap_ensure_capacity(heap, heap->size + 1);
+
+    int idx = heap->size;
+
+    heap->values[idx] = v;
+    memcpy(&heap->data[idx*heap->el_sz], p, heap->el_sz);
+
+    heap->size++;
+
+    while (idx > 0) {
+
+        int parent = (idx - 1) / 2;
+
+        // we're done!
+        if (heap->values[parent] >= v)
+            break;
+
+        // else, swap and recurse upwards.
+        heap->swap(heap, idx, parent);
+        idx = parent;
+    }
+}
+
+// Removes the item in the heap at the given index.  Returns 1 if the
+// item existed. 0 Indicates an invalid idx (heap is smaller than
+// idx). This is mostly intended to be used by zmaxheap_remove_max.
+static int zmaxheap_remove_index(zmaxheap_t *heap, int idx, void *p, float *v)
+{
+    if (idx >= heap->size)
+        return 0;
+
+    // copy out the requested element from the heap.
+    if (v != NULL)
+        *v = heap->values[idx];
+    if (p != NULL)
+        memcpy(p, &heap->data[idx*heap->el_sz], heap->el_sz);
+
+    heap->size--;
+
+    // If this element is already the last one, then there's nothing
+    // for us to do.
+    if (idx == heap->size)
+        return 1;
+
+    // copy last element to first element. (which probably upsets
+    // the heap property).
+    heap->values[idx] = heap->values[heap->size];
+    memcpy(&heap->data[idx*heap->el_sz], &heap->data[heap->el_sz * heap->size], heap->el_sz);
+
+    // now fix the heap. Note, as we descend, we're "pushing down"
+    // the same node the entire time. Thus, while the index of the
+    // parent might change, the parent_score doesn't.
+    int parent = idx;
+    float parent_score = heap->values[idx];
+
+    // descend, fixing the heap.
+    while (parent < heap->size) {
+
+        int left = 2*parent + 1;
+        int right = left + 1;
+
+//            assert(parent_score == heap->values[parent]);
+
+        float left_score = (left < heap->size) ? heap->values[left] : -INFINITY;
+        float right_score = (right < heap->size) ? heap->values[right] : -INFINITY;
+
+        // put the biggest of (parent, left, right) as the parent.
+
+        // already okay?
+        if (parent_score >= left_score && parent_score >= right_score)
+            break;
+
+        // if we got here, then one of the children is bigger than the parent.
+        if (left_score >= right_score) {
+            CV_Assert(left < heap->size);
+            heap->swap(heap, parent, left);
+            parent = left;
+        } else {
+            // right_score can't be less than left_score if right_score is -INFINITY.
+            CV_Assert(right < heap->size);
+            heap->swap(heap, parent, right);
+            parent = right;
+        }
+    }
+
+    return 1;
+}
+
+int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v)
+{
+    return zmaxheap_remove_index(heap, 0, p, v);
+}
+
+}}

modules/aruco/src/zmaxheap.hpp

+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// 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.
+//
+// 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.
+#ifndef _OPENCV_ZMAXHEAP_HPP_
+#define _OPENCV_ZMAXHEAP_HPP_
+
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+namespace cv {
+namespace aruco {
+typedef struct zmaxheap zmaxheap_t;
+
+typedef struct zmaxheap_iterator zmaxheap_iterator_t;
+struct zmaxheap_iterator {
+    zmaxheap_t *heap;
+    int in, out;
+};
+
+zmaxheap_t *zmaxheap_create(size_t el_sz);
+
+void zmaxheap_destroy(zmaxheap_t *heap);
+
+void zmaxheap_add(zmaxheap_t *heap, void *p, float v);
+
+// returns 0 if the heap is empty, so you can do
+// while (zmaxheap_remove_max(...)) { }
+int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v);
+
+}}
+#endif

modules/aruco/test/test_arucodetection.cpp

@@ -96,6 +96,7 @@ void CV_ArucoDetectionSimple::run(int) {
         vector< vector< Point2f > > corners;
         vector< int > ids;
         Ptr<aruco::DetectorParameters> params = aruco::DetectorParameters::create();
+
         aruco::detectMarkers(img, dictionary, corners, ids, params);
 
         // check detection results
@@ -245,7 +246,7 @@ class CV_ArucoDetectionPerspective : public cvtest::BaseTest {
 CV_ArucoDetectionPerspective::CV_ArucoDetectionPerspective() {}
 
 
-void CV_ArucoDetectionPerspective::run(int) {
+void CV_ArucoDetectionPerspective::run(int aprilDecimate) {
 
     int iter = 0;
     Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1);
@@ -274,6 +275,11 @@ void CV_ArucoDetectionPerspective::run(int) {
                 Ptr<aruco::DetectorParameters> params = aruco::DetectorParameters::create();
                 params->minDistanceToBorder = 1;
                 params->markerBorderBits = markerBorder;
+
+                if(aprilDecimate == 1){
+                        params->cornerRefinementMethod = cv::aruco::CORNER_REFINE_APRILTAG;
+                }
+
                 aruco::detectMarkers(img, dictionary, corners, ids, params);
 
                 // check results
@@ -481,8 +487,12 @@ void CV_ArucoBitCorrection::run(int) {
     }
 }
 
+typedef CV_ArucoDetectionPerspective CV_AprilTagDetectionPerspective;
 
-
+TEST(CV_AprilTagDetectionPerspective, algorithmic) {
+    CV_AprilTagDetectionPerspective test;
+    test.safe_run(1);
+}
 
 TEST(CV_ArucoDetectionSimple, algorithmic) {
     CV_ArucoDetectionSimple test;