Try to backport most of the changes from main AprilTag repo as of 188…

…c0e0 version.

3rdparty/apriltag/apriltag.cpp

@@ -42,18 +42,15 @@ either expressed or implied, of the Regents of The University of Michigan.
 #include <stdio.h>
 #include <math.h>
 #include <algorithm>
-//#include <inttypes.h>
 
 #include "common/image_u8.h"
 #include "common/image_u8x3.h"
-#include "common/zhash.h"
 #include "common/zarray.h"
 #include "common/matd.h"
 #include "common/homography.h"
 #include "common/timeprofile.h"
 #include "common/math_util.h"
 #include "common/g2d.h"
-#include "common/floats.h"
 
 #include "apriltag_math.h"
 
@@ -234,7 +231,9 @@ static void quick_decode_init(apriltag_family_t *family, int maxhamming)
     for (int i = 0; i < qd->nentries; i++)
         qd->entries[i].rcode = UINT64_MAX;
 
-    for (int i = 0; i < family->ncodes; i++) {
+    errno = 0;
+
+    for (uint32_t i = 0; i < family->ncodes; i++) {
         uint64_t code = family->codes[i];
 
         // add exact code (hamming = 0)
@@ -299,7 +298,8 @@ static void quick_decode_codeword(apriltag_family_t *tf, uint64_t rcode,
 {
     struct quick_decode *qd = (struct quick_decode*) tf->impl;
 
-    for (int ridx = 0; ridx < 4; ridx++) {
+    // qd might be null if detector_add_family_bits() failed
+    for (int ridx = 0; qd != NULL && ridx < 4; ridx++) {
 
         for (int bucket = rcode % qd->nentries;
              qd->entries[bucket].rcode != UINT64_MAX;
@@ -366,7 +366,7 @@ apriltag_detector_t *apriltag_detector_create()
 
     td->qtp.max_line_fit_mse = 10.0;
     td->qtp.cos_critical_rad = cos(10 * M_PI / 180);
-    td->qtp.deglitch = 0;
+    td->qtp.deglitch = false;
     td->qtp.min_white_black_diff = 5;
 
     td->tag_families = zarray_create(sizeof(apriltag_family_t*));
@@ -375,11 +375,11 @@ apriltag_detector_t *apriltag_detector_create()
 
     td->tp = timeprofile_create();
 
-    td->refine_edges = 1;
+    td->refine_edges = true;
     td->decode_sharpening = 0.25;
 
 
-    td->debug = 0;
+    td->debug = false;
 
     // NB: defer initialization of td->wp so that the user can
     // override td->nthreads.
@@ -447,8 +447,13 @@ static matd_t* homography_compute2(double c[4][4]) {
             }
         }
 
+        if (max_val_idx < 0) {
+            return NULL;
+        }
+
         if (max_val < epsilon) {
             fprintf(stderr, "WRN: Matrix is singular.\n");
+            return NULL;
         }
 
         // Swap to get best row.
@@ -503,12 +508,15 @@ static int quad_update_homographies(struct quad *quad)
 
     // XXX Tunable
     quad->H = homography_compute2(corr_arr);
-
-    quad->Hinv = matd_inverse(quad->H);
-
-    if (quad->H && quad->Hinv)
-        return 0;
-
+    if (quad->H != NULL) {
+        quad->Hinv = matd_inverse(quad->H);
+        if (quad->Hinv != NULL) {
+	    // Success!
+            return 0;
+        }
+        matd_destroy(quad->H);
+        quad->H = NULL;
+    }
     return -1;
 }
 
@@ -620,7 +628,7 @@ static float quad_decode(apriltag_detector_t* td, apriltag_family_t *family, ima
     graymodel_init(&whitemodel);
     graymodel_init(&blackmodel);
 
-    for (int pattern_idx = 0; pattern_idx < sizeof(patterns)/(5*sizeof(float)); pattern_idx ++) {
+    for (long unsigned int pattern_idx = 0; pattern_idx < sizeof(patterns)/(5*sizeof(float)); pattern_idx ++) {
         float *pattern = &patterns[pattern_idx * 5];
 
         int is_white = pattern[4];
@@ -681,7 +689,7 @@ static float quad_decode(apriltag_detector_t* td, apriltag_family_t *family, ima
     double *values = (double *)calloc(family->total_width*family->total_width, sizeof(double));
 
     int min_coord = (family->width_at_border - family->total_width)/2;
-    for (int i = 0; i < family->nbits; i++) {
+    for (uint32_t i = 0; i < family->nbits; i++) {
         int bity = family->bit_y[i];
         int bitx = family->bit_x[i];
 
@@ -714,7 +722,7 @@ static float quad_decode(apriltag_detector_t* td, apriltag_family_t *family, ima
     sharpen(td, values, family->total_width);
 
     uint64_t rcode = 0;
-    for (int i = 0; i < family->nbits; i++) {
+    for (uint32_t i = 0; i < family->nbits; i++) {
         int bity = family->bit_y[i];
         int bitx = family->bit_x[i];
         rcode = (rcode << 1);
@@ -873,9 +881,10 @@ static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, struct qu
 
             double L0 = W00*B0 + W01*B1;
 
-            // compute intersection
-            quad->p[i][0] = lines[i][0] + L0*A00;
-            quad->p[i][1] = lines[i][1] + L0*A10;
+            // Compute intersection. Note that line i represents the line from corner i to (i+1)&3, so
+	    // the intersection of line i with line (i+1)&3 represents corner (i+1)&3.
+            quad->p[(i+1)&3][0] = lines[i][0] + L0*A00;
+            quad->p[(i+1)&3][1] = lines[i][1] + L0*A10;
         } else {
             // this is a bad sign. We'll just keep the corner we had.
 //            printf("bad det: %15f %15f %15f %15f %15f\n", A00, A11, A10, A01, det);
@@ -901,7 +910,7 @@ static void quad_decode_task(void *_u)
         }
 
         // make sure the homographies are computed...
-        if (quad_update_homographies(quad_original))
+        if (quad_update_homographies(quad_original) != 0)
             continue;
 
         for (int famidx = 0; famidx < zarray_size(td->tag_families); famidx++) {
@@ -1005,6 +1014,10 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
     if (td->wp == NULL || td->nthreads != workerpool_get_nthreads(td->wp)) {
         workerpool_destroy(td->wp);
         td->wp = workerpool_create(td->nthreads);
+        if (td->wp == NULL) {
+            // creating workerpool failed - return empty zarray
+            return zarray_create(sizeof(apriltag_detection_t*));
+        }
     }
 
     timeprofile_clear(td->tp);
@@ -1455,10 +1468,10 @@ void apriltag_detections_destroy(zarray_t *detections)
     zarray_destroy(detections);
 }
 
-image_u8_t *apriltag_to_image(apriltag_family_t *fam, int idx)
+image_u8_t *apriltag_to_image(apriltag_family_t *fam, uint32_t idx)
 {
     assert(fam != NULL);
-    assert(idx >= 0 && idx < fam->ncodes);
+    assert(idx < fam->ncodes);
 
     uint64_t code = fam->codes[idx];
 
@@ -1475,7 +1488,7 @@ image_u8_t *apriltag_to_image(apriltag_family_t *fam, int idx)
     }
 
     int border_start = (fam->total_width - fam->width_at_border)/2;
-    for (int i = 0; i < fam->nbits; i++) {
+    for (uint32_t i = 0; i < fam->nbits; i++) {
         if (code & (APRILTAG_U64_ONE << (fam->nbits - i - 1))) {
             im->buf[(fam->bit_y[i] + border_start)*im->stride + fam->bit_x[i] + border_start] = 255;
         }

3rdparty/apriltag/apriltag.h

@@ -144,14 +144,14 @@ struct apriltag_detector
     // (e.g. 0.8).
     float quad_sigma;
 
-    // When non-zero, the edges of the each quad are adjusted to "snap
+    // When true, the edges of the each quad are adjusted to "snap
     // to" strong gradients nearby. This is useful when decimation is
     // employed, as it can increase the quality of the initial quad
-    // estimate substantially. Generally recommended to be on (1).
+    // estimate substantially. Generally recommended to be on (true).
     //
     // Very computationally inexpensive. Option is ignored if
     // quad_decimate = 1.
-    int refine_edges;
+    bool refine_edges;
 
     // How much sharpening should be done to decoded images? This
     // can help decode small tags but may or may not help in odd
@@ -160,10 +160,10 @@ struct apriltag_detector
     // The default value is 0.25.
     double decode_sharpening;
 
-    // When non-zero, write a variety of debugging images to the
+    // When true, write a variety of debugging images to the
     // current working directory at various stages through the
     // detection process. (Somewhat slow).
-    int debug;
+    bool debug;
 
     struct apriltag_quad_thresh_params qtp;
 
@@ -274,7 +274,7 @@ void apriltag_detections_destroy(zarray_t *detections);
 
 // Renders the apriltag.
 // Caller is responsible for calling image_u8_destroy on the image
-image_u8_t *apriltag_to_image(apriltag_family_t *fam, int idx);
+image_u8_t *apriltag_to_image(apriltag_family_t *fam, uint32_t idx);
 
 #ifdef __cplusplus
 //}

3rdparty/apriltag/apriltag_pose.cpp

@@ -2,9 +2,7 @@
 #include <stdio.h>
 
 #include "apriltag_pose.h"
-#include "apriltag_math.h"
 #include "common/homography.h"
-#include "common/image_u8x3.h"
 
 
 /**

3rdparty/apriltag/apriltag_pose.h

@@ -9,16 +9,21 @@
 
 typedef struct {
     apriltag_detection_t* det;
-    double tagsize;
-    double fx;
-    double fy;
-    double cx;
-    double cy;
+    double tagsize; // In meters.
+    double fx; // In pixels.
+    double fy; // In pixels.
+    double cx; // In pixels.
+    double cy; // In pixels.
 } apriltag_detection_info_t;
 
+/**
+ * This struct holds the transformation from the camera optical frame to
+ * the April tag frame. The pose refers to the position of the tag within
+ * the camera frame.
+ */
 typedef struct {
-    matd_t* R;
-    matd_t* t;
+    matd_t* R; // Rotation matrix 3x3 of doubles.
+    matd_t* t; // Translation matrix 3x1 of doubles.
 } apriltag_pose_t;
 
 /**