[upstream_utils] Upgrade apriltag to latest master (take 2) (#7018)

apriltag/src/main/native/thirdparty/apriltag/include/apriltag.h

@@ -269,7 +269,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
 }

apriltag/src/main/native/thirdparty/apriltag/include/apriltag_pose.h

@@ -63,7 +63,7 @@ void estimate_tag_pose_orthogonal_iteration(
         apriltag_pose_t* pose1,
         double* err2,
         apriltag_pose_t* pose2,
-        int nIters, 
+        int nIters,
         double min_improvement_per_iteration);
 
 /**

apriltag/src/main/native/thirdparty/apriltag/include/common/homography.h

@@ -176,7 +176,7 @@ matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, dou
 // [ 0  0  C  D ]
 // [ 0  0 -1  0 ]
 
-matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D);
+matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B);
 
 #ifdef __cplusplus
 }

apriltag/src/main/native/thirdparty/apriltag/include/common/image_u8x3.h

@@ -57,7 +57,7 @@ void image_u8x3_destroy(image_u8x3_t *im);
 int image_u8x3_write_pnm(const image_u8x3_t *im, const char *path);
 
 // only width 1 supported
-void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3], int width);
+void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3]);
 
 #ifdef __cplusplus
 }

apriltag/src/main/native/thirdparty/apriltag/include/common/matd.h

@@ -113,13 +113,13 @@ matd_t *matd_create_scalar(double v);
  * Retrieves the cell value for matrix 'm' at the given zero-based row and column index.
  * Performs more thorough validation checking than MATD_EL().
  */
-double matd_get(const matd_t *m, int row, int col);
+double matd_get(const matd_t *m, unsigned int row, unsigned int col);
 
 /**
  * Assigns the given value to the matrix cell at the given zero-based row and
  * column index. Performs more thorough validation checking than MATD_EL().
  */
-void matd_put(matd_t *m, int row, int col, double value);
+void matd_put(matd_t *m, unsigned int row, unsigned int col, double value);
 
 /**
  * Retrieves the scalar value of the given element ('m' must be a scalar).
@@ -147,7 +147,7 @@ matd_t *matd_copy(const matd_t *m);
  * beyond the number of rows/columns of 'a'. It is the caller's  responsibility to
  * call matd_destroy() on the returned matrix.
  */
-matd_t *matd_select(const matd_t *a, int r0, int r1, int c0, int c1);
+matd_t *matd_select(const matd_t *a, unsigned int r0, int r1, unsigned int c0, int c1);
 
 /**
  * Prints the supplied matrix 'm' to standard output by applying the supplied

apriltag/src/main/native/thirdparty/apriltag/include/common/math_util.h

@@ -27,31 +27,19 @@ either expressed or implied, of the Regents of The University of Michigan.
 
 #pragma once
 
+#define _USE_MATH_DEFINES
 #include <math.h>
-#include <float.h>
 #include <stdlib.h>
 #include <stdint.h>
 #include <assert.h>
-#include <string.h> // memcpy
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-#ifndef M_TWOPI
-# define M_TWOPI       6.2831853071795862319959  /* 2*pi */
-#endif
-
-#ifndef M_PI
-# define M_PI 3.141592653589793238462643383279502884196
-#endif
-
 #define to_radians(x) ( (x) * (M_PI / 180.0 ))
 #define to_degrees(x) ( (x) * (180.0 / M_PI ))
 
-#define max(A, B) (A < B ? B : A)
-#define min(A, B) (A < B ? A : B)
-
   /* DEPRECATE, threshold meaningless without context.
 static inline int dequals(double a, double b)
 {
@@ -111,7 +99,7 @@ static inline int irand(int bound)
 /** Map vin to [0, 2*PI) **/
 static inline double mod2pi_positive(double vin)
 {
-    return vin - M_TWOPI * floor(vin / M_TWOPI);
+    return vin - M_2_PI * floor(vin / M_2_PI);
 }
 
 /** Map vin to [-PI, PI) **/
@@ -145,7 +133,7 @@ static inline int mod_positive(int vin, int mod) {
 static inline int theta_to_int(double theta, int max)
 {
     theta = mod2pi_ref(M_PI, theta);
-    int v = (int) (theta / M_TWOPI * max);
+    int v = (int) (theta / M_2_PI * max);
 
     if (v == max)
         v = 0;

apriltag/src/main/native/thirdparty/apriltag/include/common/pam.h

@@ -40,7 +40,7 @@ struct pam
     int depth; // bytes per pixel
     int maxval; // maximum value per channel, e.g. 255 for 8bpp
 
-    int datalen; // in bytes
+    size_t datalen; // in bytes
     uint8_t *data;
 };
 

apriltag/src/main/native/thirdparty/apriltag/include/common/pthreads_cross.h

@@ -30,6 +30,7 @@ SOFTWARE.
 #include <pthread.h>
 #include <sched.h>
 #endif
+#include <time.h>
 
 #ifdef _WIN32
 

apriltag/src/main/native/thirdparty/apriltag/include/common/string_util.h

@@ -202,7 +202,7 @@ bool str_matches_any(const char *haystack, const char **needles, int num_needles
  *
  * Note: startidx must be >= endidx
  */
-char *str_substring(const char *str, size_t startidx, long endidx);
+char *str_substring(const char *str, size_t startidx, size_t endidx);
 
 /**
  * Retrieves the zero-based index of the beginning of the supplied substring

apriltag/src/main/native/thirdparty/apriltag/include/common/time_util.h

@@ -40,6 +40,8 @@ typedef long long suseconds_t;
 
 inline int gettimeofday(struct timeval* tp, void* tzp)
 {
+  (void)tzp;
+
   unsigned long t;
   t = time(NULL);
   tp->tv_sec = t / 1000;

apriltag/src/main/native/thirdparty/apriltag/include/common/zarray.h

@@ -355,7 +355,7 @@ static inline void zarray_map(zarray_t *za, void (*f)(void*))
  *
  * void map_function(element_type *element)
  */
-    void zarray_vmap(zarray_t *za, void (*f)(void*));
+    void zarray_vmap(zarray_t *za, void (*f)(void *));
 
 /**
  * Removes all elements from the array and sets its size to zero. Pointers to

apriltag/src/main/native/thirdparty/apriltag/include/common/zmaxheap.h

@@ -39,7 +39,7 @@ struct zmaxheap_iterator {
 
 zmaxheap_t *zmaxheap_create(size_t el_sz);
 
-void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*));
+void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void *));
 
 void zmaxheap_destroy(zmaxheap_t *heap);
 

apriltag/src/main/native/thirdparty/apriltag/include/test/getline.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <stddef.h>
+#include <stdio.h>
+#include <stdint.h>
+
+typedef intptr_t ssize_t;
+
+ssize_t apriltag_test_getline(char **lineptr, size_t *n, FILE *stream);

apriltag/src/main/native/thirdparty/apriltag/src/apriltag.c

@@ -32,6 +32,7 @@ either expressed or implied, of the Regents of The University of Michigan.
 
 #include "apriltag.h"
 
+#define _USE_MATH_DEFINES
 #include <math.h>
 #include <assert.h>
 #include <stdint.h>
@@ -51,10 +52,6 @@ either expressed or implied, of the Regents of The University of Michigan.
 
 #include "apriltag_math.h"
 
-#ifndef M_PI
-# define M_PI 3.141592653589793238462643383279502884196
-#endif
-
 #ifdef _WIN32
 static inline void srandom(unsigned int seed)
 {
@@ -229,7 +226,7 @@ static void quick_decode_init(apriltag_family_t *family, int maxhamming)
 
     errno = 0;
 
-    for (int i = 0; i < family->ncodes; i++) {
+    for (uint32_t i = 0; i < family->ncodes; i++) {
         uint64_t code = family->codes[i];
 
         // add exact code (hamming = 0)
@@ -446,6 +443,10 @@ static matd_t* homography_compute2(double c[4][4]) {
             }
         }
 
+        if (max_val_idx < 0) {
+            return NULL;
+        }
+
         if (max_val < epsilon) {
             debug_print("WRN: Matrix is singular.\n");
             return NULL;
@@ -622,7 +623,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];
@@ -683,7 +684,7 @@ static float quad_decode(apriltag_detector_t* td, apriltag_family_t *family, ima
     double *values = 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];
 
@@ -716,7 +717,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);
@@ -1338,8 +1339,7 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
                 int k = (j + 1) & 3;
                 image_u8x3_draw_line(out,
                                      det->p[j][0], det->p[j][1], det->p[k][0], det->p[k][1],
-                                     (uint8_t[]) { rgb[0], rgb[1], rgb[2] },
+                                     (uint8_t[]) { rgb[0], rgb[1], rgb[2] });
-                                     1);
             }
         }
 
@@ -1419,10 +1419,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];
 
@@ -1439,7 +1439,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;
         }

apriltag/src/main/native/thirdparty/apriltag/src/apriltag_quad_thresh.c

@@ -28,6 +28,7 @@ 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.
+#define _USE_MATH_DEFINES
 #include <math.h>
 #include <assert.h>
 #include <string.h>
@@ -61,10 +62,6 @@ struct uint64_zarray_entry
     struct uint64_zarray_entry *next;
 };
 
-#ifndef M_PI
-# define M_PI 3.141592653589793238462643383279502884196
-#endif
-
 struct pt
 {
     // Note: these represent 2*actual value.
@@ -268,8 +265,13 @@ void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, double *lineparm
         }
 
         double length = sqrtf(M);
-        lineparm[2] = nx/length;
+        if (fabs(length) < 1e-12) {
-        lineparm[3] = ny/length;
+            lineparm[2] = lineparm[3] = 0;
+        }
+        else {
+            lineparm[2] = nx/length;
+            lineparm[3] = ny/length;
+        }
     }
 
     // sum of squared errors =
@@ -436,7 +438,7 @@ int quad_segment_maxima(apriltag_detector_t *td, zarray_t *cluster, struct line_
 
     double err01, err12, err23, err30;
     double mse01, mse12, mse23, mse30;
-    double params01[4], params12[4], params23[4], params30[4];
+    double params01[4], params12[4];
 
     // disallow quads where the angle is less than a critical value.
     double max_dot = td->qtp.cos_critical_rad; //25*M_PI/180);
@@ -466,11 +468,11 @@ int quad_segment_maxima(apriltag_detector_t *td, zarray_t *cluster, struct line_
                 for (int m3 = m2+1; m3 < nmaxima; m3++) {
                     int i3 = maxima[m3];
 
-                    fit_line(lfps, sz, i2, i3, params23, &err23, &mse23);
+                    fit_line(lfps, sz, i2, i3, NULL, &err23, &mse23);
                     if (mse23 > td->qtp.max_line_fit_mse)
                         continue;
 
-                    fit_line(lfps, sz, i3, i0, params30, &err30, &mse30);
+                    fit_line(lfps, sz, i3, i0, NULL, &err30, &mse30);
                     if (mse30 > td->qtp.max_line_fit_mse)
                         continue;
 
@@ -501,7 +503,7 @@ int quad_segment_maxima(apriltag_detector_t *td, zarray_t *cluster, struct line_
 }
 
 // returns 0 if the cluster looks bad.
-int quad_segment_agg(apriltag_detector_t *td, zarray_t *cluster, struct line_fit_pt *lfps, int indices[4])
+int quad_segment_agg(zarray_t *cluster, struct line_fit_pt *lfps, int indices[4])
 {
     int sz = zarray_size(cluster);
 
@@ -848,43 +850,8 @@ int fit_quad(
     // step for segmenting them into four lines.
     if (1) {
         ptsort((struct pt*) cluster->data, zarray_size(cluster));
-
-        // remove duplicate points. (A byproduct of our segmentation system.)
-        if (1) {
-            int outpos = 1;
-
-            struct pt *last;
-            zarray_get_volatile(cluster, 0, &last);
-
-            for (int i = 1; i < sz; i++) {
-
-                struct pt *p;
-                zarray_get_volatile(cluster, i, &p);
-
-                if (p->x != last->x || p->y != last->y) {
-
-                    if (i != outpos)  {
-                        struct pt *out;
-                        zarray_get_volatile(cluster, outpos, &out);
-                        memcpy(out, p, sizeof(struct pt));
-                    }
-
-                    outpos++;
-                }
-
-                last = p;
-            }
-
-            cluster->size = outpos;
-            sz = outpos;
-        }
-
     }
 
-    if (sz < 24)
-        return 0;
-
-
     struct line_fit_pt *lfps = compute_lfps(sz, cluster, im);
 
     int indices[4];
@@ -892,7 +859,7 @@ int fit_quad(
         if (!quad_segment_maxima(td, cluster, lfps, indices))
             goto finish;
     } else {
-        if (!quad_segment_agg(td, cluster, lfps, indices))
+        if (!quad_segment_agg(cluster, lfps, indices))
             goto finish;
     }
 
@@ -903,10 +870,10 @@ int fit_quad(
         int i0 = indices[i];
         int i1 = indices[(i+1)&3];
 
-        double err;
+        double mse;
-        fit_line(lfps, sz, i0, i1, lines[i], NULL, &err);
+        fit_line(lfps, sz, i0, i1, lines[i], NULL, &mse);
 
-        if (err > td->qtp.max_line_fit_mse) {
+        if (mse > td->qtp.max_line_fit_mse) {
             res = 0;
             goto finish;
         }
@@ -936,11 +903,11 @@ int fit_quad(
         double det = A00 * A11 - A10 * A01;
 
         // inverse.
-        double W00 = A11 / det, W01 = -A01 / det;
         if (fabs(det) < 0.001) {
             res = 0;
             goto finish;
         }
+        double W00 = A11 / det, W01 = -A01 / det;
 
         // solve
         double L0 = W00*B0 + W01*B1;
@@ -1013,7 +980,7 @@ int fit_quad(
 
 #define DO_UNIONFIND2(dx, dy) if (im->buf[(y + dy)*s + x + dx] == v) unionfind_connect(uf, y*w + x, (y + dy)*w + x + dx);
 
-static void do_unionfind_first_line(unionfind_t *uf, image_u8_t *im, int h, int w, int s)
+static void do_unionfind_first_line(unionfind_t *uf, image_u8_t *im, int w, int s)
 {
     int y = 0;
     uint8_t v;
@@ -1028,7 +995,7 @@ static void do_unionfind_first_line(unionfind_t *uf, image_u8_t *im, int h, int
     }
 }
 
-static void do_unionfind_line2(unionfind_t *uf, image_u8_t *im, int h, int w, int s, int y)
+static void do_unionfind_line2(unionfind_t *uf, image_u8_t *im, int w, int s, int y)
 {
     assert(y > 0);
 
@@ -1074,7 +1041,7 @@ static void do_unionfind_task2(void *p)
     struct unionfind_task *task = (struct unionfind_task*) p;
 
     for (int y = task->y0; y < task->y1; y++) {
-        do_unionfind_line2(task->uf, task->im, task->h, task->w, task->s, y);
+        do_unionfind_line2(task->uf, task->im, task->w, task->s, y);
     }
 }
 
@@ -1098,10 +1065,10 @@ static void do_quad_task(void *p)
         // a cluster should contain only boundary points around the
         // tag. it cannot be bigger than the whole screen. (Reject
         // large connected blobs that will be prohibitively slow to
-        // fit quads to.) A typical point along an edge is added three
+        // fit quads to.) A typical point along an edge is added two
-        // times (because it has 3 neighbors). The maximum perimeter
+        // times (because it has 2 unique neighbors). The maximum
-        // is 2w+2h.
+        // perimeter is 2w+2h.
-        if (zarray_size(*cluster) > 3*(2*w+2*h)) {
+        if (zarray_size(*cluster) > 2*(2*w+2*h)) {
             continue;
         }
 
@@ -1531,12 +1498,12 @@ unionfind_t* connected_components(apriltag_detector_t *td, image_u8_t* threshim,
     unionfind_t *uf = unionfind_create(w * h);
 
     if (td->nthreads <= 1) {
-        do_unionfind_first_line(uf, threshim, h, w, ts);
+        do_unionfind_first_line(uf, threshim, w, ts);
         for (int y = 1; y < h; y++) {
-            do_unionfind_line2(uf, threshim, h, w, ts, y);
+            do_unionfind_line2(uf, threshim, w, ts, y);
         }
     } else {
-        do_unionfind_first_line(uf, threshim, h, w, ts);
+        do_unionfind_first_line(uf, threshim, w, ts);
 
         int sz = h;
         int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads);
@@ -1566,7 +1533,7 @@ unionfind_t* connected_components(apriltag_detector_t *td, image_u8_t* threshim,
 
         // XXX stitch together the different chunks.
         for (int i = 1; i < ntasks; i++) {
-            do_unionfind_line2(uf, threshim, h, w, ts, tasks[i].y0 - 1);
+            do_unionfind_line2(uf, threshim, w, ts, tasks[i].y0 - 1);
         }
 
         free(tasks);
@@ -1584,15 +1551,19 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int
     mem_pools[mem_pool_idx] = calloc(mem_chunk_size, sizeof(struct uint64_zarray_entry));
 
     for (int y = y0; y < y1; y++) {
+        bool connected_last = false;
         for (int x = 1; x < w-1; x++) {
 
             uint8_t v0 = threshim->buf[y*ts + x];
-            if (v0 == 127)
+            if (v0 == 127) {
+                connected_last = false;
                 continue;
+            }
 
             // XXX don't query this until we know we need it?
             uint64_t rep0 = unionfind_get_representative(uf, y*w + x);
             if (unionfind_get_set_size(uf, rep0) < 25) {
+                connected_last = false;
                 continue;
             }
 
@@ -1616,6 +1587,7 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int
             // A possible optimization would be to combine entries
             // within the same cluster.
 
+            bool connected;
 #define DO_CONN(dx, dy)                                                 \
             if (1) {                                                    \
                 uint8_t v1 = threshim->buf[(y + dy)*ts + x + dx];       \
@@ -1653,6 +1625,7 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int
                                                                             \
                         struct pt p = { .x = 2*x + dx, .y = 2*y + dy, .gx = dx*((int) v1-v0), .gy = dy*((int) v1-v0)}; \
                         zarray_add(entry->cluster, &p);                     \
+                        connected = true;                                   \
                     }                                                   \
                 }                                                       \
             }
@@ -1662,8 +1635,16 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int
             DO_CONN(0, 1);
 
             // do 8 connectivity
-            DO_CONN(-1, 1);
+            if (!connected_last) {
+                // Checking 1, 1 on the previous x, y, and -1, 1 on the current
+                // x, y result in duplicate points in the final list.  Only
+                // check the potential duplicate if adding this one won't
+                // create a duplicate.
+                DO_CONN(-1, 1);
+            }
+            connected = false;
             DO_CONN(1, 1);
+            connected_last = connected;
         }
     }
 #undef DO_CONN
@@ -1828,7 +1809,8 @@ zarray_t* fit_quads(apriltag_detector_t *td, int w, int h, zarray_t* clusters, i
         normal_border |= !family->reversed_border;
         reversed_border |= family->reversed_border;
     }
-    min_tag_width /= td->quad_decimate;
+    if (td->quad_decimate > 1)
+        min_tag_width /= td->quad_decimate;
     if (min_tag_width < 3) {
         min_tag_width = 3;
     }
@@ -1890,7 +1872,7 @@ zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
             for (int x = 0; x < w; x++) {
                 uint32_t v = unionfind_get_representative(uf, y*w+x);
 
-                if (unionfind_get_set_size(uf, v) < td->qtp.min_cluster_pixels)
+                if ((int)unionfind_get_set_size(uf, v) < td->qtp.min_cluster_pixels)
                     continue;
 
                 uint32_t color = colors[v];

apriltag/src/main/native/thirdparty/apriltag/src/common/g2d.c

@@ -33,13 +33,6 @@ either expressed or implied, of the Regents of The University of Michigan.
 #include "g2d.h"
 #include "common/math_util.h"
 
-#ifdef _WIN32
-static inline long int random(void)
-{
-        return rand();
-}
-#endif
-
 double g2d_distance(const double a[2], const double b[2])
 {
     return sqrtf(sq(a[0]-b[0]) + sq(a[1]-b[1]));
@@ -127,7 +120,7 @@ int g2d_polygon_contains_point_ref(const zarray_t *poly, double q[2])
 
     double acc_theta = 0;
 
-    double last_theta;
+    double last_theta = 0;
 
     for (int i = 0; i <= psz; i++) {
         double p[2];
@@ -328,7 +321,7 @@ int g2d_polygon_contains_point(const zarray_t *poly, double q[2])
     int psz = zarray_size(poly);
     assert(psz > 0);
 
-    int last_quadrant;
+    int last_quadrant = 0;
     int quad_acc = 0;
 
     for (int i = 0; i <= psz; i++) {
@@ -725,6 +718,13 @@ int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x)
 */
 #if 0
 
+#ifdef _WIN32
+static inline long int random(void)
+{
+        return rand();
+}
+#endif
+
 #include "timeprofile.h"
 
 int main(int argc, char *argv[])

apriltag/src/main/native/thirdparty/apriltag/src/common/homography.c

@@ -357,7 +357,7 @@ matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, dou
 // [ 0  0  C  D ]
 // [ 0  0 -1  0 ]
 
-matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D)
+matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B)
 {
     // Note that every variable that we compute is proportional to the scale factor of H.
     double R20 = -MATD_EL(H, 2, 0);

apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8.c

@@ -211,7 +211,7 @@ int image_u8_write_pnm(const image_u8_t *im, const char *path)
     fprintf(f, "P5\n%d %d\n255\n", im->width, im->height);
 
     for (int y = 0; y < im->height; y++) {
-        if (im->width != fwrite(&im->buf[y*im->stride], 1, im->width, f)) {
+        if (im->width != (int32_t)fwrite(&im->buf[y*im->stride], 1, im->width, f)) {
             res = -2;
             goto finish;
         }
@@ -265,6 +265,9 @@ void image_u8_draw_annulus(image_u8_t *im, float x0, float y0, float r0, float r
 void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width)
 {
     double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0));
+    if (dist == 0) {
+        return;
+    }
     double delta = 0.5 / dist;
 
     // terrible line drawing code

apriltag/src/main/native/thirdparty/apriltag/src/common/image_u8x3.c

@@ -147,7 +147,7 @@ int image_u8x3_write_pnm(const image_u8x3_t *im, const char *path)
 
     // Only outputs to RGB
     fprintf(f, "P6\n%d %d\n255\n", im->width, im->height);
-    int linesz = im->width * 3;
+    size_t linesz = im->width * 3;
     for (int y = 0; y < im->height; y++) {
         if (linesz != fwrite(&im->buf[y*im->stride], 1, linesz, f)) {
             res = -1;
@@ -163,7 +163,7 @@ finish:
 }
 
 // only width 1 supported
-void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3], int width)
+void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3])
 {
     double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0));
     double delta = 0.5 / dist;

apriltag/src/main/native/thirdparty/apriltag/src/common/matd.c

@@ -105,20 +105,18 @@ matd_t *matd_identity(int dim)
 }
 
 // row and col are zero-based
-TYPE matd_get(const matd_t *m, int row, int col)
+TYPE matd_get(const matd_t *m, unsigned int row, unsigned int col)
 {
     assert(m != NULL);
     assert(!matd_is_scalar(m));
-    assert(row >= 0);
     assert(row < m->nrows);
-    assert(col >= 0);
     assert(col < m->ncols);
 
     return MATD_EL(m, row, col);
 }
 
 // row and col are zero-based
-void matd_put(matd_t *m, int row, int col, TYPE value)
+void matd_put(matd_t *m, unsigned int row, unsigned int col, TYPE value)
 {
     assert(m != NULL);
 
@@ -127,9 +125,7 @@ void matd_put(matd_t *m, int row, int col, TYPE value)
         return;
     }
 
-    assert(row >= 0);
     assert(row < m->nrows);
-    assert(col >= 0);
     assert(col < m->ncols);
 
     MATD_EL(m, row, col) = value;
@@ -164,12 +160,12 @@ matd_t *matd_copy(const matd_t *m)
     return x;
 }
 
-matd_t *matd_select(const matd_t * a, int r0, int r1, int c0, int c1)
+matd_t *matd_select(const matd_t * a, unsigned int r0, int r1, unsigned int c0, int c1)
 {
     assert(a != NULL);
 
-    assert(r0 >= 0 && r0 < a->nrows);
+    assert(r0 < a->nrows);
-    assert(c0 >= 0 && c0 < a->ncols);
+    assert(c0 < a->ncols);
 
     int nrows = r1 - r0 + 1;
     int ncols = c1 - c0 + 1;
@@ -192,8 +188,8 @@ void matd_print(const matd_t *m, const char *fmt)
         printf(fmt, MATD_EL(m, 0, 0));
         printf("\n");
     } else {
-        for (int i = 0; i < m->nrows; i++) {
+        for (unsigned int i = 0; i < m->nrows; i++) {
-            for (int j = 0; j < m->ncols; j++) {
+            for (unsigned int j = 0; j < m->ncols; j++) {
                 printf(fmt, MATD_EL(m, i, j));
             }
             printf("\n");
@@ -210,8 +206,8 @@ void matd_print_transpose(const matd_t *m, const char *fmt)
         printf(fmt, MATD_EL(m, 0, 0));
         printf("\n");
     } else {
-        for (int j = 0; j < m->ncols; j++) {
+        for (unsigned int j = 0; j < m->ncols; j++) {
-            for (int i = 0; i < m->nrows; i++) {
+            for (unsigned int i = 0; i < m->nrows; i++) {
                 printf(fmt, MATD_EL(m, i, j));
             }
             printf("\n");
@@ -241,10 +237,10 @@ matd_t *matd_multiply(const matd_t *a, const matd_t *b)
     assert(a->ncols == b->nrows);
     matd_t *m = matd_create(a->nrows, b->ncols);
 
-    for (int i = 0; i < m->nrows; i++) {
+    for (unsigned int i = 0; i < m->nrows; i++) {
-        for (int j = 0; j < m->ncols; j++) {
+        for (unsigned int j = 0; j < m->ncols; j++) {
             TYPE acc = 0;
-            for (int k = 0; k < a->ncols; k++) {
+            for (unsigned int k = 0; k < a->ncols; k++) {
                 acc += MATD_EL(a, i, k) * MATD_EL(b, k, j);
             }
             MATD_EL(m, i, j) = acc;
@@ -263,8 +259,8 @@ matd_t *matd_scale(const matd_t *a, double s)
 
     matd_t *m = matd_create(a->nrows, a->ncols);
 
-    for (int i = 0; i < m->nrows; i++) {
+    for (unsigned int i = 0; i < m->nrows; i++) {
-        for (int j = 0; j < m->ncols; j++) {
+        for (unsigned int j = 0; j < m->ncols; j++) {
             MATD_EL(m, i, j) = s * MATD_EL(a, i, j);
         }
     }
@@ -281,8 +277,8 @@ void matd_scale_inplace(matd_t *a, double s)
         return;
     }
 
-    for (int i = 0; i < a->nrows; i++) {
+    for (unsigned int i = 0; i < a->nrows; i++) {
-        for (int j = 0; j < a->ncols; j++) {
+        for (unsigned int j = 0; j < a->ncols; j++) {
             MATD_EL(a, i, j) *= s;
         }
     }
@@ -300,8 +296,8 @@ matd_t *matd_add(const matd_t *a, const matd_t *b)
 
     matd_t *m = matd_create(a->nrows, a->ncols);
 
-    for (int i = 0; i < m->nrows; i++) {
+    for (unsigned int i = 0; i < m->nrows; i++) {
-        for (int j = 0; j < m->ncols; j++) {
+        for (unsigned int j = 0; j < m->ncols; j++) {
             MATD_EL(m, i, j) = MATD_EL(a, i, j) + MATD_EL(b, i, j);
         }
     }
@@ -321,8 +317,8 @@ void matd_add_inplace(matd_t *a, const matd_t *b)
         return;
     }
 
-    for (int i = 0; i < a->nrows; i++) {
+    for (unsigned int i = 0; i < a->nrows; i++) {
-        for (int j = 0; j < a->ncols; j++) {
+        for (unsigned int j = 0; j < a->ncols; j++) {
             MATD_EL(a, i, j) += MATD_EL(b, i, j);
         }
     }
@@ -341,8 +337,8 @@ matd_t *matd_subtract(const matd_t *a, const matd_t *b)
 
     matd_t *m = matd_create(a->nrows, a->ncols);
 
-    for (int i = 0; i < m->nrows; i++) {
+    for (unsigned int i = 0; i < m->nrows; i++) {
-        for (int j = 0; j < m->ncols; j++) {
+        for (unsigned int j = 0; j < m->ncols; j++) {
             MATD_EL(m, i, j) = MATD_EL(a, i, j) - MATD_EL(b, i, j);
         }
     }
@@ -362,8 +358,8 @@ void matd_subtract_inplace(matd_t *a, const matd_t *b)
         return;
     }
 
-    for (int i = 0; i < a->nrows; i++) {
+    for (unsigned int i = 0; i < a->nrows; i++) {
-        for (int j = 0; j < a->ncols; j++) {
+        for (unsigned int j = 0; j < a->ncols; j++) {
             MATD_EL(a, i, j) -= MATD_EL(b, i, j);
         }
     }
@@ -379,8 +375,8 @@ matd_t *matd_transpose(const matd_t *a)
 
     matd_t *m = matd_create(a->ncols, a->nrows);
 
-    for (int i = 0; i < a->nrows; i++) {
+    for (unsigned int i = 0; i < a->nrows; i++) {
-        for (int j = 0; j < a->ncols; j++) {
+        for (unsigned int j = 0; j < a->ncols; j++) {
             MATD_EL(m, j, i) = MATD_EL(a, i, j);
         }
     }
@@ -398,7 +394,7 @@ double matd_det_general(const matd_t *a)
     // The determinants of the L and U matrices are the products of
     // their respective diagonal elements
     double detL = 1; double detU = 1;
-    for (int i = 0; i < a->nrows; i++) {
+    for (unsigned int i = 0; i < a->nrows; i++) {
         detL *= matd_get(L, i, i);
         detU *= matd_get(U, i, i);
     }
@@ -877,6 +873,8 @@ double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n)
     (void) lena;
     (void) lenb;
     assert(n <= lena && n <= lenb);
+    (void)lena;
+    (void)lenb;
 
     double mag = 0.0;
     for (int i = 0; i < n; i++)
@@ -912,6 +910,7 @@ double matd_vec_dot_product(const matd_t *a, const matd_t *b)
     int bdim = b->ncols*b->nrows;
     (void) bdim;
     assert(adim == bdim);
+    (void)bdim;
 
     double acc = 0;
     for (int i = 0; i < adim; i++) {
@@ -960,8 +959,8 @@ TYPE matd_err_inf(const matd_t *a, const matd_t *b)
 
     TYPE maxf = 0;
 
-    for (int i = 0; i < a->nrows; i++) {
+    for (unsigned int i = 0; i < a->nrows; i++) {
-        for (int j = 0; j < a->ncols; j++) {
+        for (unsigned int j = 0; j < a->ncols; j++) {
             TYPE av = MATD_EL(a, i, j);
             TYPE bv = MATD_EL(b, i, j);
 
@@ -1005,7 +1004,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
     // RS: cumulative right-handed transformations.
     matd_t *RS = matd_identity(A->ncols);
 
-    for (int hhidx = 0; hhidx < A->nrows; hhidx++)  {
+    for (unsigned int hhidx = 0; hhidx < A->nrows; hhidx++)  {
 
         if (hhidx < A->ncols) {
             // We construct the normal of the reflection plane: let u
@@ -1065,7 +1064,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
             // LS = matd_op("F*M", LS, Q);
             // Implementation: take each row of LS, compute dot product with n,
             // subtract n (scaled by dot product) from it.
-            for (int i = 0; i < LS->nrows; i++) {
+            for (unsigned int i = 0; i < LS->nrows; i++) {
                 double dot = 0;
                 for (int j = 0; j < vlen; j++)
                     dot += MATD_EL(LS, i, hhidx+j) * v[j];
@@ -1074,7 +1073,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
             }
 
             //  B = matd_op("M*F", Q, B); // should be Q', but Q is symmetric.
-            for (int i = 0; i < B->ncols; i++) {
+            for (unsigned int i = 0; i < B->ncols; i++) {
                 double dot = 0;
                 for (int j = 0; j < vlen; j++)
                     dot += MATD_EL(B, hhidx+j, i) * v[j];
@@ -1123,7 +1122,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
             //       MATD_EL(Q, i+1+hhidx, j+1+hhidx) -= 2*v[i]*v[j];
 
             //  RS = matd_op("F*M", RS, Q);
-            for (int i = 0; i < RS->nrows; i++) {
+            for (unsigned int i = 0; i < RS->nrows; i++) {
                 double dot = 0;
                 for (int j = 0; j < vlen; j++)
                     dot += MATD_EL(RS, i, hhidx+1+j) * v[j];
@@ -1132,7 +1131,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
             }
 
             //   B = matd_op("F*M", B, Q); // should be Q', but Q is symmetric.
-            for (int i = 0; i < B->nrows; i++) {
+            for (unsigned int i = 0; i < B->nrows; i++) {
                 double dot = 0;
                 for (int j = 0; j < vlen; j++)
                     dot += MATD_EL(B, i, hhidx+1+j) * v[j];
@@ -1151,7 +1150,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
     assert(maxiters > 0); // reassure clang
     int iter;
 
-    double maxv; // maximum non-zero value being reduced this iteration
+    double maxv = 0; // maximum non-zero value being reduced this iteration
 
     double tol = 1E-10;
 
@@ -1161,11 +1160,11 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
 
     // for each of the first B->ncols rows, which index has the
     // maximum absolute value? (used by method 1)
-    int *maxrowidx = malloc(sizeof(int)*B->ncols);
+    unsigned int *maxrowidx = malloc(sizeof(int)*B->ncols);
-    int lastmaxi, lastmaxj;
+    unsigned int lastmaxi, lastmaxj;
 
     if (find_max_method == 1) {
-        for (int i = 2; i < B->ncols; i++)
+        for (unsigned int i = 2; i < B->ncols; i++)
             maxrowidx[i] = max_idx(B, i, B->ncols);
 
         // note that we started the array at 2. That's because by setting
@@ -1198,7 +1197,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
             // modified. Update maxrowidx accordingly.
 
             // now, EVERY row also had columns lastmaxi and lastmaxj modified.
-            for (int rowi = 0; rowi < B->ncols; rowi++) {
+            for (unsigned int rowi = 0; rowi < B->ncols; rowi++) {
 
                 // the magnitude of the largest off-diagonal element
                 // in this row.
@@ -1271,8 +1270,8 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
             maxv = -1;
 
             // only search top "square" portion
-            for (int i = 0; i < B->ncols; i++) {
+            for (unsigned int i = 0; i < B->ncols; i++) {
-                for (int j = 0; j < B->ncols; j++) {
+                for (unsigned int j = 0; j < B->ncols; j++) {
                     if (i == j)
                         continue;
 
@@ -1337,7 +1336,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
 */
 
             //  LS = matd_op("F*M", LS, QL);
-            for (int i = 0; i < LS->nrows; i++) {
+            for (unsigned int i = 0; i < LS->nrows; i++) {
                 double vi = MATD_EL(LS, i, maxi);
                 double vj = MATD_EL(LS, i, maxj);
 
@@ -1346,7 +1345,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
             }
 
             //  RS = matd_op("F*M", RS, QR); // remember we'll transpose RS.
-            for (int i = 0; i < RS->nrows; i++) {
+            for (unsigned int i = 0; i < RS->nrows; i++) {
                 double vi = MATD_EL(RS, i, maxi);
                 double vj = MATD_EL(RS, i, maxj);
 
@@ -1356,7 +1355,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
 
             // B = matd_op("M'*F*M", QL, B, QR);
             // The QL matrix mixes rows of B.
-            for (int i = 0; i < B->ncols; i++) {
+            for (unsigned int i = 0; i < B->ncols; i++) {
                 double vi = MATD_EL(B, maxi, i);
                 double vj = MATD_EL(B, maxj, i);
 
@@ -1365,7 +1364,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
             }
 
             // The QR matrix mixes columns of B.
-            for (int i = 0; i < B->nrows; i++) {
+            for (unsigned int i = 0; i < B->nrows; i++) {
                 double vi = MATD_EL(B, i, maxi);
                 double vj = MATD_EL(B, i, maxj);
 
@@ -1388,7 +1387,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
     // U/LS.
     int *idxs = malloc(sizeof(int)*A->ncols);
     double *vals = malloc(sizeof(double)*A->ncols);
-    for (int i = 0; i < A->ncols; i++) {
+    for (unsigned int i = 0; i < A->ncols; i++) {
         idxs[i] = i;
         vals[i] = MATD_EL(B, i, i);
     }
@@ -1398,7 +1397,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
     do {
         changed = 0;
 
-        for (int i = 0; i + 1 < A->ncols; i++) {
+        for (unsigned int i = 0; i + 1 < A->ncols; i++) {
             if (fabs(vals[i+1]) > fabs(vals[i])) {
                 int tmpi = idxs[i];
                 idxs[i] = idxs[i+1];
@@ -1416,7 +1415,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
     matd_t *LP = matd_identity(A->nrows);
     matd_t *RP = matd_identity(A->ncols);
 
-    for (int i = 0; i < A->ncols; i++) {
+    for (unsigned int i = 0; i < A->ncols; i++) {
         MATD_EL(LP, idxs[i], idxs[i]) = 0; // undo the identity above
         MATD_EL(RP, idxs[i], idxs[i]) = 0;
 
@@ -1444,8 +1443,8 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags)
 
     // make B exactly diagonal
 
-    for (int i = 0; i < B->nrows; i++) {
+    for (unsigned int i = 0; i < B->nrows; i++) {
-        for (int j = 0; j < B->ncols; j++) {
+        for (unsigned int j = 0; j < B->ncols; j++) {
             if (i != j)
                 MATD_EL(B, i, j) = 0;
         }
@@ -1522,11 +1521,11 @@ matd_plu_t *matd_plu(const matd_t *a)
 
     matd_plu_t *mlu = calloc(1, sizeof(matd_plu_t));
 
-    for (int i = 0; i < a->nrows; i++)
+    for (unsigned int i = 0; i < a->nrows; i++)
         piv[i] = i;
 
-    for (int j = 0; j < a->ncols; j++) {
+    for (unsigned int j = 0; j < a->ncols; j++) {
-        for (int i = 0; i < a->nrows; i++) {
+        for (unsigned int i = 0; i < a->nrows; i++) {
             int kmax = i < j ? i : j; // min(i,j)
 
             // compute dot product of row i with column j (up through element kmax)
@@ -1538,9 +1537,9 @@ matd_plu_t *matd_plu(const matd_t *a)
         }
 
         // find pivot and exchange if necessary.
-        int p = j;
+        unsigned int p = j;
         if (1) {
-            for (int i = j+1; i < lu->nrows; i++) {
+            for (unsigned int i = j+1; i < lu->nrows; i++) {
                 if (fabs(MATD_EL(lu,i,j)) > fabs(MATD_EL(lu, p, j))) {
                     p = i;
                 }
@@ -1579,7 +1578,7 @@ matd_plu_t *matd_plu(const matd_t *a)
 
         if (j < lu->ncols && j < lu->nrows && LUjj != 0) {
             LUjj = 1.0 / LUjj;
-            for (int i = j+1; i < lu->nrows; i++)
+            for (unsigned int i = j+1; i < lu->nrows; i++)
                 MATD_EL(lu, i, j) *= LUjj;
         }
     }
@@ -1605,7 +1604,7 @@ double matd_plu_det(const matd_plu_t *mlu)
     double det = mlu->pivsign;
 
     if (lu->nrows == lu->ncols) {
-        for (int i = 0; i < lu->ncols; i++)
+        for (unsigned int i = 0; i < lu->ncols; i++)
             det *= MATD_EL(lu, i, i);
     }
 
@@ -1617,7 +1616,7 @@ matd_t *matd_plu_p(const matd_plu_t *mlu)
     matd_t *lu = mlu->lu;
     matd_t *P = matd_create(lu->nrows, lu->nrows);
 
-    for (int i = 0; i < lu->nrows; i++) {
+    for (unsigned int i = 0; i < lu->nrows; i++) {
         MATD_EL(P, mlu->piv[i], i) = 1;
     }
 
@@ -1629,10 +1628,10 @@ matd_t *matd_plu_l(const matd_plu_t *mlu)
     matd_t *lu = mlu->lu;
 
     matd_t *L = matd_create(lu->nrows, lu->ncols);
-    for (int i = 0; i < lu->nrows; i++) {
+    for (unsigned int i = 0; i < lu->nrows; i++) {
         MATD_EL(L, i, i) = 1;
 
-        for (int j = 0; j < i; j++) {
+        for (unsigned int j = 0; j < i; j++) {
             MATD_EL(L, i, j) = MATD_EL(lu, i, j);
         }
     }
@@ -1645,8 +1644,8 @@ matd_t *matd_plu_u(const matd_plu_t *mlu)
     matd_t *lu = mlu->lu;
 
     matd_t *U = matd_create(lu->ncols, lu->ncols);
-    for (int i = 0; i < lu->ncols; i++) {
+    for (unsigned int i = 0; i < lu->ncols; i++) {
-        for (int j = 0; j < lu->ncols; j++) {
+        for (unsigned int j = 0; j < lu->ncols; j++) {
             if (i <= j)
                 MATD_EL(U, i, j) = MATD_EL(lu, i, j);
         }
@@ -1664,14 +1663,14 @@ matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b)
     matd_t *x = matd_copy(b);
 
     // permute right hand side
-    for (int i = 0; i < mlu->lu->nrows; i++)
+    for (unsigned int i = 0; i < mlu->lu->nrows; i++)
         memcpy(&MATD_EL(x, i, 0), &MATD_EL(b, mlu->piv[i], 0), sizeof(TYPE) * b->ncols);
 
     // solve Ly = b
-    for (int k = 0; k < mlu->lu->nrows; k++) {
+    for (unsigned int k = 0; k < mlu->lu->nrows; k++) {
-        for (int i = k+1; i < mlu->lu->nrows; i++) {
+        for (unsigned int i = k+1; i < mlu->lu->nrows; i++) {
             double LUik = -MATD_EL(mlu->lu, i, k);
-            for (int t = 0; t < b->ncols; t++)
+            for (unsigned int t = 0; t < b->ncols; t++)
                 MATD_EL(x, i, t) += MATD_EL(x, k, t) * LUik;
         }
     }
@@ -1679,12 +1678,12 @@ matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b)
     // solve Ux = y
     for (int k = mlu->lu->ncols-1; k >= 0; k--) {
         double LUkk = 1.0 / MATD_EL(mlu->lu, k, k);
-        for (int t = 0; t < b->ncols; t++)
+        for (unsigned int t = 0; t < b->ncols; t++)
             MATD_EL(x, k, t) *= LUkk;
 
         for (int i = 0; i < k; i++) {
             double LUik = -MATD_EL(mlu->lu, i, k);
-            for (int t = 0; t < b->ncols; t++)
+            for (unsigned int t = 0; t < b->ncols; t++)
                 MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik;
         }
     }
@@ -1912,7 +1911,7 @@ void matd_ltransposetriangle_solve(matd_t *u, const TYPE *b, TYPE *x)
     for (int i = 0; i < n; i++) {
         x[i] /= MATD_EL(u, i, i);
 
-        for (int j = i+1; j < u->ncols; j++) {
+        for (unsigned int j = i+1; j < u->ncols; j++) {
             x[j] -= x[i] * MATD_EL(u, i, j);
         }
     }
@@ -1942,7 +1941,7 @@ void matd_utriangle_solve(matd_t *u, const TYPE *b, TYPE *x)
 
         double diag = MATD_EL(u, i, i);
 
-        for (int j = i+1; j < u->ncols; j++)
+        for (unsigned int j = i+1; j < u->ncols; j++)
             bi -= MATD_EL(u, i, j)*x[j];
 
         x[i] = bi / diag;
@@ -1959,17 +1958,17 @@ matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b)
 
     // solve Ly = b ==> (U')y = b
 
-    for (int i = 0; i < u->nrows; i++) {
+    for (unsigned int i = 0; i < u->nrows; i++) {
-        for (int j = 0; j < i; j++) {
+        for (unsigned int j = 0; j < i; j++) {
             // b[i] -= L[i,j]*x[j]... replicated across columns of b
             //   ==> i.e., ==>
             // b[i,k] -= L[i,j]*x[j,k]
-            for (int k = 0; k < b->ncols; k++) {
+            for (unsigned int k = 0; k < b->ncols; k++) {
                 MATD_EL(x, i, k) -= MATD_EL(u, j, i)*MATD_EL(x, j, k);
             }
         }
         // x[i] = b[i] / L[i,i]
-        for (int k = 0; k < b->ncols; k++) {
+        for (unsigned int k = 0; k < b->ncols; k++) {
             MATD_EL(x, i, k) /= MATD_EL(u, i, i);
         }
     }
@@ -1977,12 +1976,12 @@ matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b)
     // solve Ux = y
     for (int k = u->ncols-1; k >= 0; k--) {
         double LUkk = 1.0 / MATD_EL(u, k, k);
-        for (int t = 0; t < b->ncols; t++)
+        for (unsigned int t = 0; t < b->ncols; t++)
             MATD_EL(x, k, t) *= LUkk;
 
         for (int i = 0; i < k; i++) {
             double LUik = -MATD_EL(u, i, k);
-            for (int t = 0; t < b->ncols; t++)
+            for (unsigned int t = 0; t < b->ncols; t++)
                 MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik;
         }
     }
@@ -2018,8 +2017,8 @@ matd_t *matd_chol_inverse(matd_t *a)
 double matd_max(matd_t *m)
 {
     double d = -DBL_MAX;
-    for(int x=0; x<m->nrows; x++) {
+    for(unsigned int x=0; x<m->nrows; x++) {
-        for(int y=0; y<m->ncols; y++) {
+        for(unsigned int y=0; y<m->ncols; y++) {
             if(MATD_EL(m, x, y) > d)
                 d = MATD_EL(m, x, y);
         }

apriltag/src/main/native/thirdparty/apriltag/src/common/pam.c

@@ -181,7 +181,7 @@ int pam_write_file(pam_t *pam, const char *outpath)
 
     fprintf(f, "P7\nWIDTH %d\nHEIGHT %d\nDEPTH %d\nMAXVAL %d\nTUPLTYPE %s\nENDHDR\n",
             pam->width, pam->height, pam->depth, pam->maxval, tupl);
-    int len = pam->width * pam->height * pam->depth;
+    size_t len = pam->width * pam->height * pam->depth;
     if (len != fwrite(pam->data, 1, len, f)) {
         fclose(f);
         return -2;

apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg-idct.c

@@ -25,13 +25,10 @@ of the authors and should not be interpreted as representing official policies,
 either expressed or implied, of the Regents of The University of Michigan.
 */
 
+#define _USE_MATH_DEFINES
 #include <math.h>
 #include <stdint.h>
 
-#ifndef M_PI
-# define M_PI 3.141592653589793238462643383279502884196
-#endif
-
 // 8 bits of fixed-point output
 //
 // This implementation has a worst-case complexity of 22 multiplies

apriltag/src/main/native/thirdparty/apriltag/src/common/pjpeg.c

@@ -423,7 +423,7 @@ static int pjpeg_decode_buffer(struct pjpeg_decode_state *pjd)
                             if (code_pos + ncodes > 0xffff)
                                 return PJPEG_ERR_DHT;
 
-                            for (int ci = 0; ci < ncodes; ci++) {
+                            for (unsigned int ci = 0; ci < ncodes; ci++) {
                                 pjd->huff_codes[htidx][code_pos].nbits = nbits;
                                 pjd->huff_codes[htidx][code_pos].code = code;
                                 code_pos++;
@@ -753,8 +753,8 @@ image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj)
 
     if (Cr_factor_y == 1 && Cr_factor_x == 1 && Cb_factor_y == 1 && Cb_factor_x == 1) {
 
-        for (int y = 0; y < pj->height; y++) {
+        for (uint32_t y = 0; y < pj->height; y++) {
-            for (int x = 0; x < pj->width; x++) {
+            for (uint32_t x = 0; x < pj->width; x++) {
                 int32_t y_val  = Y->data[y*Y->stride + x] * 65536;
                 int32_t cb_val = Cb->data[y*Cb->stride + x] - 128;
                 int32_t cr_val = Cr->data[y*Cr->stride + x] - 128;
@@ -769,8 +769,8 @@ image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj)
             }
         }
     } else if (Cb_factor_y == Cr_factor_y && Cb_factor_x == Cr_factor_x) {
-        for (int by = 0; by < pj->height / Cb_factor_y; by++) {
+        for (uint32_t by = 0; by < pj->height / Cb_factor_y; by++) {
-            for (int bx = 0; bx < pj->width / Cb_factor_x; bx++) {
+            for (uint32_t bx = 0; bx < pj->width / Cb_factor_x; bx++) {
 
                 int32_t cb_val = Cb->data[by*Cb->stride + bx] - 128;
                 int32_t cr_val = Cr->data[by*Cr->stride + bx] - 128;
@@ -800,8 +800,8 @@ image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj)
         }
     } else {
 
-        for (int y = 0; y < pj->height; y++) {
+        for (uint32_t y = 0; y < pj->height; y++) {
-            for (int x = 0; x < pj->width; x++) {
+            for (uint32_t x = 0; x < pj->width; x++) {
                 int32_t y_val  = Y->data[y*Y->stride + x];
                 int32_t cb_val = Cb->data[(y / Cb_factor_y)*Cb->stride + (x / Cb_factor_x)] - 128;
                 int32_t cr_val = Cr->data[(y / Cr_factor_y)*Cr->stride + (x / Cr_factor_x)] - 128;
@@ -865,6 +865,7 @@ pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int
         int result = pjpeg_decode_buffer(&pjd);
         (void) result;
         assert(result == 0);
+        (void)result;
     }
 
     pjd.in = buf;

apriltag/src/main/native/thirdparty/apriltag/src/common/pthreads_cross.c

@@ -21,7 +21,6 @@ SOFTWARE.
  */
 
 #include "common/pthreads_cross.h"
-#include <time.h>
 
 #ifdef _WIN32
 
@@ -230,15 +229,10 @@ void ms_to_timespec(struct timespec *ts, unsigned int ms)
 
 unsigned int timespec_to_ms(const struct timespec *abstime)
 {
-    DWORD t;
-
     if (abstime == NULL)
         return INFINITE;
 
-    t = ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000);
+    return ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000);
-    if (t < 0)
-        t = 1;
-    return t;
 }
 
 unsigned int pcthread_get_num_procs(void)

apriltag/src/main/native/thirdparty/apriltag/src/common/string_util.c

@@ -38,7 +38,7 @@ either expressed or implied, of the Regents of The University of Michigan.
 struct string_buffer
 {
     char *s;
-    int alloc;
+    size_t alloc;
     size_t size; // as if strlen() was called; not counting terminating \0
 };
 
@@ -130,7 +130,7 @@ int str_diff_idx(const char * a, const char * b)
     assert(a != NULL);
     assert(b != NULL);
 
-    int i = 0;
+    size_t i = 0;
 
     size_t lena = strlen(a);
     size_t lenb = strlen(b);
@@ -293,7 +293,7 @@ char *str_tolowercase(char *s)
 	assert(s != NULL);
 
     size_t slen = strlen(s);
-    for (int i = 0; i < slen; i++) {
+    for (size_t i = 0; i < slen; i++) {
         if (s[i] >= 'A' && s[i] <= 'Z')
             s[i] = s[i] + 'a' - 'A';
     }
@@ -306,7 +306,7 @@ char *str_touppercase(char *s)
     assert(s != NULL);
 
     size_t slen = strlen(s);
-    for (int i = 0; i < slen; i++) {
+    for (size_t i = 0; i < slen; i++) {
         if (s[i] >= 'a' && s[i] <= 'z')
             s[i] = s[i] - ('a' - 'A');
     }
@@ -499,14 +499,13 @@ char string_feeder_next(string_feeder_t *sf)
 char *string_feeder_next_length(string_feeder_t *sf, size_t length)
 {
     assert(sf != NULL);
-    assert(length >= 0);
     assert(sf->pos <= sf->len);
 
     if (sf->pos + length > sf->len)
         length = sf->len - sf->pos;
 
     char *substr = calloc(length+1, sizeof(char));
-    for (int i = 0 ; i < length ; i++)
+    for (size_t i = 0 ; i < length ; i++)
         substr[i] = string_feeder_next(sf);
     return substr;
 }
@@ -522,7 +521,6 @@ char string_feeder_peek(string_feeder_t *sf)
 char *string_feeder_peek_length(string_feeder_t *sf, size_t length)
 {
     assert(sf != NULL);
-    assert(length >= 0);
     assert(sf->pos <= sf->len);
 
     if (sf->pos + length > sf->len)
@@ -550,10 +548,11 @@ void string_feeder_require(string_feeder_t *sf, const char *str)
 
     size_t len = strlen(str);
 
-    for (int i = 0; i < len; i++) {
+    for (size_t i = 0; i < len; i++) {
         char c = string_feeder_next(sf);
         (void) c;
         assert(c == str[i]);
+        (void)c;
     }
 }
 
@@ -621,15 +620,12 @@ bool str_matches_any(const char *haystack, const char **needles, int num_needles
     return false;
 }
 
-char *str_substring(const char *str, size_t startidx, long endidx)
+char *str_substring(const char *str, size_t startidx, size_t endidx)
 {
     assert(str != NULL);
-    assert(startidx >= 0 && startidx <= strlen(str)+1);
+    assert(startidx <= strlen(str)+1);
-    assert(endidx < 0 || endidx >= startidx);
+    assert(endidx >= startidx);
-    assert(endidx < 0 || endidx <= strlen(str)+1);
+    assert(endidx <= strlen(str)+1);
-
-    if (endidx < 0)
-        endidx = (long) strlen(str);
 
     size_t blen = endidx - startidx; // not counting \0
     char *b = malloc(blen + 1);

apriltag/src/main/native/thirdparty/apriltag/src/common/workerpool.c

@@ -30,6 +30,7 @@ either expressed or implied, of the Regents of The University of Michigan.
 #define __USE_GNU
 #include "common/pthreads_cross.h"
 #include <assert.h>
+#include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #ifdef _WIN32
@@ -51,6 +52,7 @@ struct workerpool {
 
     pthread_mutex_t mutex;
     pthread_cond_t startcond;   // used to signal the availability of work
+    bool start_predicate;       // predicate that prevents spurious wakeups on startcond
     pthread_cond_t endcond;     // used to signal completion of all work
 
     int end_count; // how many threads are done?
@@ -66,26 +68,19 @@ void *worker_thread(void *p)
 {
     workerpool_t *wp = (workerpool_t*) p;
 
-//    int cnt = 0;
-
     while (1) {
         struct task *task;
 
         pthread_mutex_lock(&wp->mutex);
-        while (wp->taskspos == zarray_size(wp->tasks)) {
+        while (wp->taskspos == zarray_size(wp->tasks) || !wp->start_predicate) {
             wp->end_count++;
-//          printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt);
             pthread_cond_broadcast(&wp->endcond);
             pthread_cond_wait(&wp->startcond, &wp->mutex);
-//            cnt = 0;
-//            printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self());
         }
 
         zarray_get_volatile(wp->tasks, wp->taskspos, &task);
         wp->taskspos++;
-//        cnt++;
         pthread_mutex_unlock(&wp->mutex);
-//        pthread_yield();
         sched_yield();
 
         // we've been asked to exit.
@@ -105,6 +100,7 @@ workerpool_t *workerpool_create(int nthreads)
     workerpool_t *wp = calloc(1, sizeof(workerpool_t));
     wp->nthreads = nthreads;
     wp->tasks = zarray_create(sizeof(struct task));
+    wp->start_predicate = false;
 
     if (nthreads > 1) {
         wp->threads = calloc(wp->nthreads, sizeof(pthread_t));
@@ -121,6 +117,13 @@ workerpool_t *workerpool_create(int nthreads)
                 return NULL;
             }
         }
+
+        // Wait for the worker threads to be ready
+        pthread_mutex_lock(&wp->mutex);
+        while (wp->end_count < wp->nthreads) {
+            pthread_cond_wait(&wp->endcond, &wp->mutex);
+        }
+        pthread_mutex_unlock(&wp->mutex);
     }
 
     return wp;
@@ -137,6 +140,7 @@ void workerpool_destroy(workerpool_t *wp)
             workerpool_add_task(wp, NULL, NULL);
 
         pthread_mutex_lock(&wp->mutex);
+        wp->start_predicate = true;
         pthread_cond_broadcast(&wp->startcond);
         pthread_mutex_unlock(&wp->mutex);
 
@@ -164,7 +168,13 @@ void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p)
     t.f = f;
     t.p = p;
 
-    zarray_add(wp->tasks, &t);
+    if (wp->nthreads > 1) {
+        pthread_mutex_lock(&wp->mutex);
+        zarray_add(wp->tasks, &t);
+        pthread_mutex_unlock(&wp->mutex);
+    } else {
+        zarray_add(wp->tasks, &t);
+    }
 }
 
 void workerpool_run_single(workerpool_t *wp)
@@ -182,9 +192,9 @@ void workerpool_run_single(workerpool_t *wp)
 void workerpool_run(workerpool_t *wp)
 {
     if (wp->nthreads > 1) {
-        wp->end_count = 0;
-
         pthread_mutex_lock(&wp->mutex);
+        wp->end_count = 0;
+        wp->start_predicate = true;
         pthread_cond_broadcast(&wp->startcond);
 
         while (wp->end_count < wp->nthreads) {
@@ -192,9 +202,9 @@ void workerpool_run(workerpool_t *wp)
             pthread_cond_wait(&wp->endcond, &wp->mutex);
         }
 
-        pthread_mutex_unlock(&wp->mutex);
-
         wp->taskspos = 0;
+        wp->start_predicate = false;
+        pthread_mutex_unlock(&wp->mutex);
 
         zarray_clear(wp->tasks);
 
@@ -205,7 +215,7 @@ void workerpool_run(workerpool_t *wp)
 
 int workerpool_get_nprocs(void)
 {
-#ifdef WIN32
+#ifdef _WIN32
     SYSTEM_INFO sysinfo;
     GetSystemInfo(&sysinfo);
     return sysinfo.dwNumberOfProcessors;

apriltag/src/main/native/thirdparty/apriltag/src/common/zhash.c

@@ -352,7 +352,7 @@ void zhash_iterator_remove(zhash_iterator_t *zit)
     zit->last_entry--;
 }
 
-void zhash_map_keys(zhash_t *zh, void (*f)(void *))
+void zhash_map_keys(zhash_t *zh, void (*f)(void*))
 {
     assert(zh != NULL);
     if (f == NULL)
@@ -368,7 +368,7 @@ void zhash_map_keys(zhash_t *zh, void (*f)(void *))
     }
 }
 
-void zhash_vmap_keys(zhash_t * zh, void (*f)(void *))
+void zhash_vmap_keys(zhash_t * zh, void (*f)(void*))
 {
     assert(zh != NULL);
     if (f == NULL)
@@ -385,7 +385,7 @@ void zhash_vmap_keys(zhash_t * zh, void (*f)(void *))
     }
 }
 
-void zhash_map_values(zhash_t * zh, void (*f)(void *))
+void zhash_map_values(zhash_t * zh, void (*f)(void*))
 {
     assert(zh != NULL);
     if (f == NULL)
@@ -400,7 +400,7 @@ void zhash_map_values(zhash_t * zh, void (*f)(void *))
     }
 }
 
-void zhash_vmap_values(zhash_t * zh, void (*f)(void *))
+void zhash_vmap_values(zhash_t * zh, void (*f)(void*))
 {
     assert(zh != NULL);
     if (f == NULL)

apriltag/src/main/native/thirdparty/apriltag/src/common/zmaxheap.c

@@ -321,7 +321,6 @@ static void maxheapify(zmaxheap_t *heap, int parent)
     if (betterchild != parent) {
         heap->swap(heap, parent, betterchild);
         maxheapify(heap, betterchild);
-        return;
     }
 }
 
@@ -376,7 +375,6 @@ void zmaxheap_test(void)
             // add a value
             int32_t v = (int32_t) (random() / 1000);
             float fv = v;
-            assert(v == fv);
 
             vals[sz] = v;
             zmaxheap_add(heap, &v, fv);
@@ -395,12 +393,13 @@ void zmaxheap_test(void)
             }
 
 
-            int32_t outv;
+            int32_t outv = 0;
-            float outfv;
+            float outfv = 0;
             int res = zmaxheap_remove_max(heap, &outv, &outfv);
             if (sz == 0) {
                 (void) res;
                 assert(res == 0);
+                (void)res;
             } else {
 //                printf("%d %d %d %f\n", sz, maxv, outv, outfv);
                 assert(outv == outfv);

upstream_utils/apriltag.py

@@ -39,6 +39,7 @@ def copy_upstream_src(wpilib_root):
     src_files = walk_cwd_and_copy_if(
         lambda dp, f: (f.endswith(".c") or f.endswith(".cpp"))
         and not dp.startswith(os.path.join(".", "example"))
+        and not dp.startswith(os.path.join(".", "test"))
         and not f.endswith("getopt.c")
         and not "py" in f
         and not remove_tag(f),
@@ -69,7 +70,7 @@ def copy_upstream_src(wpilib_root):
 def main():
     name = "apriltag"
     url = "https://github.com/AprilRobotics/apriltag.git"
-    tag = "ebdb2017e04b8e36f7d8a12ce60060416a905e12"
+    tag = "3806edf38ac4400153677e510c9f9dcb81f472c8"
 
     patch_options = {
         "ignore_whitespace": True,

upstream_utils/apriltag_patches/0001-apriltag_pose.c-Set-NULL-when-second-solution-could-.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sun, 4 Dec 2022 11:01:56 -0800
-Subject: [PATCH 1/9] apriltag_pose.c: Set NULL when second solution could not
+Subject: [PATCH 1/8] apriltag_pose.c: Set NULL when second solution could not
  be determined
 
 ---

upstream_utils/apriltag_patches/0002-Avoid-unused-variable-warnings-in-release-builds.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sun, 4 Dec 2022 11:42:13 -0800
-Subject: [PATCH 3/9] Avoid unused variable warnings in release builds
+Subject: [PATCH 2/8] Avoid unused variable warnings in release builds
 
 ---
  common/matd.c        | 4 +++-
@@ -11,10 +11,10 @@ Subject: [PATCH 3/9] Avoid unused variable warnings in release builds
  4 files changed, 6 insertions(+), 1 deletion(-)
 
 diff --git a/common/matd.c b/common/matd.c
-index 54449d9f27a4dae6d1422c37c71bdeeb046c94ba..f426890416dd021d7392b78bd19959c70f44247b 100644
+index 6b8a0405ce094536c395ad667fa640bda335fb6a..7c9088235aed537f5ecdc95f49424e362fb842eb 100644
 --- a/common/matd.c
 +++ b/common/matd.c
-@@ -874,7 +874,8 @@ double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n)
+@@ -870,7 +870,8 @@ double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n)
  
      int lena = a->nrows*a->ncols;
      int lenb = b->nrows*b->ncols;
@@ -22,18 +22,18 @@ index 54449d9f27a4dae6d1422c37c71bdeeb046c94ba..f426890416dd021d7392b78bd19959c7
 +    (void) lena;
 +    (void) lenb;
      assert(n <= lena && n <= lenb);
- 
+     (void)lena;
-     double mag = 0.0;
+     (void)lenb;
-@@ -909,6 +910,7 @@ double matd_vec_dot_product(const matd_t *a, const matd_t *b)
+@@ -907,6 +908,7 @@ double matd_vec_dot_product(const matd_t *a, const matd_t *b)
      assert(matd_is_vector(a) && matd_is_vector(b));
      int adim = a->ncols*a->nrows;
      int bdim = b->ncols*b->nrows;
 +    (void) bdim;
      assert(adim == bdim);
+     (void)bdim;
  
-     double acc = 0;
 diff --git a/common/pjpeg.c b/common/pjpeg.c
-index 7e3d089ad849df67549a58873eb6574c85fda6ee..4a5dd028ee8a3d79fbe7a5abed30886cc6e40dae 100644
+index 924ac04dd7602aea2eaad76076002785c7b32ba2..d410c11bf84b12fca6a38544d05b446879f8a38b 100644
 --- a/common/pjpeg.c
 +++ b/common/pjpeg.c
 @@ -863,6 +863,7 @@ pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int
@@ -42,29 +42,29 @@ index 7e3d089ad849df67549a58873eb6574c85fda6ee..4a5dd028ee8a3d79fbe7a5abed30886c
          int result = pjpeg_decode_buffer(&pjd);
 +        (void) result;
          assert(result == 0);
+         (void)result;
      }
- 
 diff --git a/common/string_util.c b/common/string_util.c
-index 4f0c98056b084af62a9a04bc8425e16dbdf9041a..3d86eb2f2d602a5b00c29952808368dc73c0b0cd 100644
+index 16a9ffb7b02e37df6f09231e67914422af89f2a4..492eabfa09ef6219c6e74bfc7de6d4407ed913c7 100644
 --- a/common/string_util.c
 +++ b/common/string_util.c
-@@ -552,6 +552,7 @@ void string_feeder_require(string_feeder_t *sf, const char *str)
+@@ -550,6 +550,7 @@ void string_feeder_require(string_feeder_t *sf, const char *str)
  
-     for (int i = 0; i < len; i++) {
+     for (size_t i = 0; i < len; i++) {
          char c = string_feeder_next(sf);
 +        (void) c;
          assert(c == str[i]);
+         (void)c;
      }
- }
 diff --git a/common/zmaxheap.c b/common/zmaxheap.c
-index e410664fd57dfa5ebd30e0680d77b008bb41801c..2c671236bf07ed4c43e15c02f4bf10df76880cb9 100644
+index 75de99502a70ec88b4dc51fda567c7e0c7bd852c..abbb549e41b6073f581b5c3216f024ed370b9212 100644
 --- a/common/zmaxheap.c
 +++ b/common/zmaxheap.c
-@@ -399,6 +399,7 @@ void zmaxheap_test()
+@@ -397,6 +397,7 @@ void zmaxheap_test()
-             float outfv;
+             float outfv = 0;
              int res = zmaxheap_remove_max(heap, &outv, &outfv);
              if (sz == 0) {
 +                (void) res;
                  assert(res == 0);
+                 (void)res;
              } else {
- //                printf("%d %d %d %f\n", sz, maxv, outv, outfv);

upstream_utils/apriltag_patches/0002-zmaxheapify-Avoid-return-of-void-expression.patch

@@ -1,23 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Peter Johnson <johnson.peter@gmail.com>
-Date: Sun, 4 Dec 2022 11:25:12 -0800
-Subject: [PATCH 2/9] zmaxheapify: Avoid return of void expression
-
----
- common/zmaxheap.c | 3 ++-
- 1 file changed, 2 insertions(+), 1 deletion(-)
-
-diff --git a/common/zmaxheap.c b/common/zmaxheap.c
-index e04d03efa8a79163293eb306e9f4beab6e4a27df..e410664fd57dfa5ebd30e0680d77b008bb41801c 100644
---- a/common/zmaxheap.c
-+++ b/common/zmaxheap.c
-@@ -320,7 +320,8 @@ static void maxheapify(zmaxheap_t *heap, int parent)
- 
-     if (betterchild != parent) {
-         heap->swap(heap, parent, betterchild);
--        return maxheapify(heap, betterchild);
-+        maxheapify(heap, betterchild);
-+        return;
-     }
- }
- 

upstream_utils/apriltag_patches/0003-Make-orthogonal_iteration-exit-early-upon-convergenc.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Tue, 10 Jan 2023 18:36:36 -0800
-Subject: [PATCH 4/9] Make orthogonal_iteration() exit early upon convergence
+Subject: [PATCH 3/8] Make orthogonal_iteration() exit early upon convergence
 
 The current approach wastes iterations doing no work. Exiting early can
 give lower latencies and higher FPS.
@@ -86,7 +86,7 @@ index f0003a2d187df13236992026ee6ff7f9d6f7aff9..782729225c3555edcfebb7d8a21f847a
          pose->R = pose1.R;
          pose->t = pose1.t;
 diff --git a/apriltag_pose.h b/apriltag_pose.h
-index 07ee37b2cb4185bcbdb46d1c9ccec306f0f2e96d..72993f11514199754c452369bad32157b6d64eb1 100644
+index 07ee37b2cb4185bcbdb46d1c9ccec306f0f2e96d..6bbe4a4e000402f990b0a13b8b87e6becc8c48a8 100644
 --- a/apriltag_pose.h
 +++ b/apriltag_pose.h
 @@ -63,7 +63,8 @@ void estimate_tag_pose_orthogonal_iteration(
@@ -94,13 +94,13 @@ index 07ee37b2cb4185bcbdb46d1c9ccec306f0f2e96d..72993f11514199754c452369bad32157
          double* err2,
          apriltag_pose_t* pose2,
 -        int nIters);
-+        int nIters, 
++        int nIters,
 +        double min_improvement_per_iteration);
  
  /**
   * Estimate tag pose.
 diff --git a/example/apriltag_demo.c b/example/apriltag_demo.c
-index 6de90540fe2f22f5160f725bce03d50bb3967c74..841d7788756011b0c7237e989242d87de76b14ef 100644
+index 6de90540fe2f22f5160f725bce03d50bb3967c74..ccd55cdceeaf46712891a7fab971b8b3c6afa950 100644
 --- a/example/apriltag_demo.c
 +++ b/example/apriltag_demo.c
 @@ -42,6 +42,7 @@ either expressed or implied, of the Regents of The University of Michigan.
@@ -124,13 +124,13 @@ index 6de90540fe2f22f5160f725bce03d50bb3967c74..841d7788756011b0c7237e989242d87d
 +                int nIters = 200;
 +                estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2, &pose2, nIters, 1e-7);
 +
-+                printf("Primary translation %f %f %f\nerror: %f\n", 
++                printf("Primary translation %f %f %f\nerror: %f\n",
 +                    pose1.t->data[0],
 +                    pose1.t->data[1],
 +                    pose1.t->data[2],
 +                    err1
 +                );
-+                printf("Alt translation %f %f %f\nerror: %f\n", 
++                printf("Alt translation %f %f %f\nerror: %f\n",
 +                    pose2.t->data[0],
 +                    pose2.t->data[1],
 +                    pose2.t->data[2],

upstream_utils/apriltag_patches/0004-Fix-signed-left-shift-warning.patch

@@ -1,14 +1,14 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Wed, 19 Jul 2023 20:48:21 -0700
-Subject: [PATCH 5/9] Fix signed left shift warning
+Subject: [PATCH 4/8] Fix signed left shift warning
 
 ---
  common/pjpeg.c | 4 ++--
  1 file changed, 2 insertions(+), 2 deletions(-)
 
 diff --git a/common/pjpeg.c b/common/pjpeg.c
-index 4a5dd028ee8a3d79fbe7a5abed30886cc6e40dae..1941c27b7752a6a2420e8370451b0d29268f324d 100644
+index d410c11bf84b12fca6a38544d05b446879f8a38b..dca5368f98e91307b58ed62f6f39c4494c0b0c74 100644
 --- a/common/pjpeg.c
 +++ b/common/pjpeg.c
 @@ -594,7 +594,7 @@ static int pjpeg_decode_buffer(struct pjpeg_decode_state *pjd)

upstream_utils/apriltag_patches/0005-Avoid-incompatible-pointer-warning.patch

@@ -1,14 +1,14 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Wed, 19 Jul 2023 21:28:43 -0700
-Subject: [PATCH 6/9] Avoid incompatible pointer warning
+Subject: [PATCH 5/8] Avoid incompatible pointer warning
 
 ---
  common/getopt.c | 3 ++-
  1 file changed, 2 insertions(+), 1 deletion(-)
 
 diff --git a/common/getopt.c b/common/getopt.c
-index 7613b69c346f3f818688bb9f4704463367d877f6..71ae57bd83b2ed50c7f80f3e3952ddfcc53cb7bc 100644
+index 21ec6fb1ed2a72343206716d87c283196511b50b..339d43f171b50af97201e934e8bfdafc0fc833cc 100644
 --- a/common/getopt.c
 +++ b/common/getopt.c
 @@ -76,8 +76,9 @@ getopt_t *getopt_create()

upstream_utils/apriltag_patches/0006-Remove-calls-to-postscript_image.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Fri, 19 Jul 2024 21:45:29 -0700
-Subject: [PATCH 8/9] Remove calls to postscript_image()
+Subject: [PATCH 6/8] Remove calls to postscript_image()
 
 ---
  apriltag.c             | 5 -----
@@ -9,19 +9,19 @@ Subject: [PATCH 8/9] Remove calls to postscript_image()
  2 files changed, 8 deletions(-)
 
 diff --git a/apriltag.c b/apriltag.c
-index 3086228868281eaad5cc5382d305227757d4a5cf..b514d974971827e6a4f4f0a4fa12e6b0392d7282 100644
+index a513cb98b69b9cc1b7b74ecc10ce5ab1feccc541..2f9846437619897c871d267963f143886f71b577 100644
 --- a/apriltag.c
 +++ b/apriltag.c
-@@ -51,8 +51,6 @@ either expressed or implied, of the Regents of The University of Michigan.
+@@ -52,8 +52,6 @@ either expressed or implied, of the Regents of The University of Michigan.
  
  #include "apriltag_math.h"
  
 -#include "common/postscript_utils.h"
 -
- #ifndef M_PI
+ #ifdef _WIN32
- # define M_PI 3.141592653589793238462643383279502884196
+ static inline void srandom(unsigned int seed)
- #endif
+ {
-@@ -1282,7 +1280,6 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
+@@ -1283,7 +1281,6 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
          fprintf(f, "%f %f scale\n", scale, scale);
          fprintf(f, "0 %d translate\n", darker->height);
          fprintf(f, "1 -1 scale\n");
@@ -39,10 +39,10 @@ index 3086228868281eaad5cc5382d305227757d4a5cf..b514d974971827e6a4f4f0a4fa12e6b0
  
          for (int i = 0; i < zarray_size(quads); i++) {
 diff --git a/apriltag_quad_thresh.c b/apriltag_quad_thresh.c
-index 735520c6b1dd7387c837e8922d2ecd68130b9b5c..677f365765493bcbf043b0841bb28016a0d3acde 100644
+index 2b57774065d231abd5129acbc97e1a63e9e53d95..f8f6aff721ced5edad460512db7bb953296b92c6 100644
 --- a/apriltag_quad_thresh.c
 +++ b/apriltag_quad_thresh.c
-@@ -40,7 +40,6 @@ either expressed or implied, of the Regents of The University of Michigan.
+@@ -41,7 +41,6 @@ either expressed or implied, of the Regents of The University of Michigan.
  #include "common/unionfind.h"
  #include "common/timeprofile.h"
  #include "common/zmaxheap.h"
@@ -50,7 +50,7 @@ index 735520c6b1dd7387c837e8922d2ecd68130b9b5c..677f365765493bcbf043b0841bb28016
  #include "common/math_util.h"
  
  #ifdef _WIN32
-@@ -1979,8 +1978,6 @@ zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
+@@ -1961,8 +1960,6 @@ zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
          fprintf(f, "0 %d translate\n", im2->height);
          fprintf(f, "1 -1 scale\n");
  

upstream_utils/apriltag_patches/0007-Fix-GCC-14-calloc-warning.patch

@@ -1,32 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Tyler Veness <calcmogul@gmail.com>
-Date: Sun, 23 Jun 2024 05:53:42 -0700
-Subject: [PATCH 7/9] Fix GCC 14 calloc() warning
-MIME-Version: 1.0
-Content-Type: text/plain; charset=UTF-8
-Content-Transfer-Encoding: 8bit
-
-```
-/home/tav/frc/wpilib/allwpilib/build-cmake-release/_deps/apriltaglib-src/common/zmaxheap.c: In function ‘zmaxheap_test’:
-/home/tav/frc/wpilib/allwpilib/build-cmake-release/_deps/apriltaglib-src/common/zmaxheap.c:365:35: error: ‘calloc’ sizes specified with ‘sizeof’ in the earlier argument and not in the later argument [-Werror=calloc-transposed-args]
-  365 |     int32_t *vals = calloc(sizeof(int32_t), cap);
-      |                                   ^~~~~~~
-/home/tav/frc/wpilib/allwpilib/build-cmake-release/_deps/apriltaglib-src/common/zmaxheap.c:365:35: note: earlier argument should specify number of elements, later size of each element
-```
----
- common/zmaxheap.c | 2 +-
- 1 file changed, 1 insertion(+), 1 deletion(-)
-
-diff --git a/common/zmaxheap.c b/common/zmaxheap.c
-index 2c671236bf07ed4c43e15c02f4bf10df76880cb9..ed271420d229a98b56c7027f47d9830a78a97db1 100644
---- a/common/zmaxheap.c
-+++ b/common/zmaxheap.c
-@@ -362,7 +362,7 @@ void zmaxheap_test()
- {
-     int cap = 10000;
-     int sz = 0;
--    int32_t *vals = calloc(sizeof(int32_t), cap);
-+    int32_t *vals = calloc(cap, sizeof(int32_t));
- 
-     zmaxheap_t *heap = zmaxheap_create(sizeof(int32_t));
- 

upstream_utils/apriltag_patches/0007-Fix-clang-16-warnings.patch

@@ -1,56 +1,51 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Thu, 29 Jun 2023 22:14:05 -0700
-Subject: [PATCH 9/9] Fix clang 16 warnings
+Subject: [PATCH 7/8] Fix clang 16 warnings
 
 ---
- apriltag.c                | 2 +-
+ apriltag.c              | 2 +-
- apriltag.h                | 2 +-
+ apriltag.h              | 2 +-
- common/g2d.c              | 2 +-
+ common/g2d.c            | 2 +-
- common/g2d.h              | 2 +-
+ common/g2d.h            | 2 +-
- common/getopt.c           | 2 +-
+ common/getopt.c         | 2 +-
- common/getopt.h           | 2 +-
+ common/getopt.h         | 2 +-
- common/math_util.h        | 4 ++--
+ common/math_util.h      | 4 ++--
- common/pthreads_cross.cpp | 6 +++---
+ common/pthreads_cross.c | 6 +++---
- common/pthreads_cross.h   | 2 +-
+ common/pthreads_cross.h | 2 +-
- common/string_util.c      | 2 +-
+ common/string_util.c    | 2 +-
- common/string_util.h      | 2 +-
+ common/string_util.h    | 2 +-
- common/time_util.c        | 4 ++--
+ common/time_util.c      | 4 ++--
- common/time_util.h        | 4 ++--
+ common/time_util.h      | 4 ++--
- common/timeprofile.h      | 2 +-
+ common/timeprofile.h    | 2 +-
- common/workerpool.c       | 8 ++++----
+ common/workerpool.c     | 2 +-
- common/workerpool.h       | 2 +-
+ common/workerpool.h     | 2 +-
- common/zarray.c           | 2 +-
+ common/zmaxheap.c       | 2 +-
- common/zarray.h           | 2 +-
+ tag16h5.c               | 2 +-
- common/zhash.c            | 8 ++++----
+ tag16h5.h               | 2 +-
- common/zhash.h            | 8 ++++----
+ tag25h9.c               | 2 +-
- common/zmaxheap.c         | 4 ++--
+ tag25h9.h               | 2 +-
- common/zmaxheap.h         | 2 +-
+ tag36h10.c              | 2 +-
- tag16h5.c                 | 2 +-
+ tag36h10.h              | 2 +-
- tag16h5.h                 | 2 +-
+ tag36h11.c              | 2 +-
- tag25h9.c                 | 2 +-
+ tag36h11.h              | 2 +-
- tag25h9.h                 | 2 +-
+ tagCircle21h7.c         | 2 +-
- tag36h10.c                | 2 +-
+ tagCircle21h7.h         | 2 +-
- tag36h10.h                | 2 +-
+ tagCircle49h12.c        | 2 +-
- tag36h11.c                | 2 +-
+ tagCircle49h12.h        | 2 +-
- tag36h11.h                | 2 +-
+ tagCustom48h12.c        | 2 +-
- tagCircle21h7.c           | 2 +-
+ tagCustom48h12.h        | 2 +-
- tagCircle21h7.h           | 2 +-
+ tagStandard41h12.c      | 2 +-
- tagCircle49h12.c          | 2 +-
+ tagStandard41h12.h      | 2 +-
- tagCircle49h12.h          | 2 +-
+ tagStandard52h13.c      | 2 +-
- tagCustom48h12.c          | 2 +-
+ tagStandard52h13.h      | 2 +-
- tagCustom48h12.h          | 2 +-
+ 35 files changed, 40 insertions(+), 40 deletions(-)
- tagStandard41h12.c        | 2 +-
- tagStandard41h12.h        | 2 +-
- tagStandard52h13.c        | 2 +-
- tagStandard52h13.h        | 2 +-
- 40 files changed, 55 insertions(+), 55 deletions(-)
 
 diff --git a/apriltag.c b/apriltag.c
-index b514d974971827e6a4f4f0a4fa12e6b0392d7282..bfff1ea9d502b8ef08d00e61d4c75a967c06462b 100644
+index 2f9846437619897c871d267963f143886f71b577..b7e24c4bc279643f478d810d494345216be991f1 100644
 --- a/apriltag.c
 +++ b/apriltag.c
-@@ -352,7 +352,7 @@ void apriltag_detector_clear_families(apriltag_detector_t *td)
+@@ -349,7 +349,7 @@ void apriltag_detector_clear_families(apriltag_detector_t *td)
      zarray_clear(td->tag_families);
  }
  
@@ -60,7 +55,7 @@ index b514d974971827e6a4f4f0a4fa12e6b0392d7282..bfff1ea9d502b8ef08d00e61d4c75a96
      apriltag_detector_t *td = (apriltag_detector_t*) calloc(1, sizeof(apriltag_detector_t));
  
 diff --git a/apriltag.h b/apriltag.h
-index 2d772cda60185de81fb279e83134074284b8e491..eb58a71d976ee84b94a232e3c778120a52f2b609 100644
+index 164ad8618f10ab2cbe489b813c9eed64be6e96e0..895b3459b8a84064989378fe533fd676964a1687 100644
 --- a/apriltag.h
 +++ b/apriltag.h
 @@ -231,7 +231,7 @@ struct apriltag_detection
@@ -73,10 +68,10 @@ index 2d772cda60185de81fb279e83134074284b8e491..eb58a71d976ee84b94a232e3c778120a
  // add a family to the apriltag detector. caller still "owns" the family.
  // a single instance should only be provided to one apriltag detector instance.
 diff --git a/common/g2d.c b/common/g2d.c
-index 4645f206e52ff14c20c04ec9601d20725339197b..e0106437717b872b8b9823135c076da7ba0a1e84 100644
+index ebb85e55975fd91c6fd6cde7830efa5185580e98..64c9b1b3342cc4b56de91733d720098bc9f09fff 100644
 --- a/common/g2d.c
 +++ b/common/g2d.c
-@@ -45,7 +45,7 @@ double g2d_distance(const double a[2], const double b[2])
+@@ -38,7 +38,7 @@ double g2d_distance(const double a[2], const double b[2])
      return sqrtf(sq(a[0]-b[0]) + sq(a[1]-b[1]));
  }
  
@@ -99,7 +94,7 @@ index 21c21ac64d8988578f3c2d108a7a07d54e780954..5a0dd0b59869099e24453eda78ada1e5
  void g2d_polygon_add(zarray_t *poly, double v[2]);
  
 diff --git a/common/getopt.c b/common/getopt.c
-index 71ae57bd83b2ed50c7f80f3e3952ddfcc53cb7bc..458ef7705019b73dcd4c9fa03fe15555ea087fcf 100644
+index 339d43f171b50af97201e934e8bfdafc0fc833cc..51fe9b9a8f61708cfd0704673fb95cac1b66ec75 100644
 --- a/common/getopt.c
 +++ b/common/getopt.c
 @@ -64,7 +64,7 @@ struct getopt
@@ -112,7 +107,7 @@ index 71ae57bd83b2ed50c7f80f3e3952ddfcc53cb7bc..458ef7705019b73dcd4c9fa03fe15555
      getopt_t *gopt = (getopt_t*) calloc(1, sizeof(getopt_t));
  
 diff --git a/common/getopt.h b/common/getopt.h
-index 69dbb05c8286a9d5d301382fc44fdea8c8f7875e..75266f033a0485b22cc34ec08e894781b6658894 100644
+index 1f0366283bf5edd4f01b4dfcd9254ec2cc2abd8e..1b2d69c982d6ba0ca4a960eea9e03340ba753e0e 100644
 --- a/common/getopt.h
 +++ b/common/getopt.h
 @@ -36,7 +36,7 @@ extern "C" {
@@ -122,13 +117,13 @@ index 69dbb05c8286a9d5d301382fc44fdea8c8f7875e..75266f033a0485b22cc34ec08e894781
 -getopt_t *getopt_create();
 +getopt_t *getopt_create(void);
  void getopt_destroy(getopt_t *gopt);
+ void getopt_option_destroy_void(void *goo);
  
- // Parse args. Returns 1 on success
 diff --git a/common/math_util.h b/common/math_util.h
-index 9271a01834501a311f28525ca460f61117524eab..5434355a512c1a6225bbfbf335caa279d17ed7aa 100644
+index aa08883608a8aca5cc7828134ecb4cd6e9f06a61..a68ed8bf8eda0a9e23cc4d2784ccc6b7d82701c4 100644
 --- a/common/math_util.h
 +++ b/common/math_util.h
-@@ -86,13 +86,13 @@ static inline double sgn(double v)
+@@ -74,13 +74,13 @@ static inline double sgn(double v)
  }
  
  // random number between [0, 1)
@@ -144,11 +139,11 @@ index 9271a01834501a311f28525ca460f61117524eab..5434355a512c1a6225bbfbf335caa279
  {
      return randf()*2 - 1;
  }
-diff --git a/common/pthreads_cross.cpp b/common/pthreads_cross.cpp
+diff --git a/common/pthreads_cross.c b/common/pthreads_cross.c
-index f7721912f9088bf84224943aa836a69356949f18..09d6ce6e97074f4ffbcef1ef6e06fd53ee22ff0f 100644
+index 3403863f385c96d089db9652079b038fea3e5f51..d48b81a1bbafa15f1c8a5b4fec6a755847947ce6 100644
---- a/common/pthreads_cross.cpp
+--- a/common/pthreads_cross.c
-+++ b/common/pthreads_cross.cpp
++++ b/common/pthreads_cross.c
-@@ -216,7 +216,7 @@ int pthread_rwlock_unlock(pthread_rwlock_t *rwlock)
+@@ -218,7 +218,7 @@ int pthread_rwlock_unlock(pthread_rwlock_t *rwlock)
      return 0;
  }
  
@@ -157,8 +152,8 @@ index f7721912f9088bf84224943aa836a69356949f18..09d6ce6e97074f4ffbcef1ef6e06fd53
      return (int)SwitchToThread();
  }
  
-@@ -241,7 +241,7 @@ unsigned int timespec_to_ms(const struct timespec *abstime)
+@@ -238,7 +238,7 @@ unsigned int timespec_to_ms(const struct timespec *abstime)
-     return t;
+     return ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000);
  }
  
 -unsigned int pcthread_get_num_procs()
@@ -166,7 +161,7 @@ index f7721912f9088bf84224943aa836a69356949f18..09d6ce6e97074f4ffbcef1ef6e06fd53
  {
      SYSTEM_INFO sysinfo;
  
-@@ -252,7 +252,7 @@ unsigned int pcthread_get_num_procs()
+@@ -249,7 +249,7 @@ unsigned int pcthread_get_num_procs()
  #else
  
  #include <unistd.h>
@@ -176,10 +171,10 @@ index f7721912f9088bf84224943aa836a69356949f18..09d6ce6e97074f4ffbcef1ef6e06fd53
      return (unsigned int)sysconf(_SC_NPROCESSORS_ONLN);
  }
 diff --git a/common/pthreads_cross.h b/common/pthreads_cross.h
-index 897a333573e88263b6ba58ec3d31031304c50e54..209c28f855efcbdf41424f11a302ba2794836796 100644
+index 5970c679316037505ee4dc1e96ab9fda1f4f60d5..89ff7cc79ba93a5676fbc8663fcd7127cd1757d9 100644
 --- a/common/pthreads_cross.h
 +++ b/common/pthreads_cross.h
-@@ -70,7 +70,7 @@ int sched_yield(void);
+@@ -71,7 +71,7 @@ int sched_yield(void);
  #ifdef __cplusplus
  extern "C" {
  #endif
@@ -189,7 +184,7 @@ index 897a333573e88263b6ba58ec3d31031304c50e54..209c28f855efcbdf41424f11a302ba27
  void ms_to_timespec(struct timespec *ts, unsigned int ms);
  unsigned int timespec_to_ms(const struct timespec *abstime);
 diff --git a/common/string_util.c b/common/string_util.c
-index 3d86eb2f2d602a5b00c29952808368dc73c0b0cd..14726e7a7f66a330b28ef326734cd7fd25e8d4dd 100644
+index 492eabfa09ef6219c6e74bfc7de6d4407ed913c7..f7da2b8d77960febe07c52f770a9c4e3bbda29f0 100644
 --- a/common/string_util.c
 +++ b/common/string_util.c
 @@ -314,7 +314,7 @@ char *str_touppercase(char *s)
@@ -202,7 +197,7 @@ index 3d86eb2f2d602a5b00c29952808368dc73c0b0cd..14726e7a7f66a330b28ef326734cd7fd
      string_buffer_t *sb = (string_buffer_t*) calloc(1, sizeof(string_buffer_t));
      assert(sb != NULL);
 diff --git a/common/string_util.h b/common/string_util.h
-index 9a7cd1e4509e3eba102316330cd50df918f56aed..d0363b03f23a94c35ed3581ea40f6d8f3f2d4110 100644
+index 6b5835553617ba16fc987edee5d2bc19e2824766..765f5a1d75e9327e6005e6d04db0c1da864ae91d 100644
 --- a/common/string_util.h
 +++ b/common/string_util.h
 @@ -262,7 +262,7 @@ char *str_replace(const char *haystack, const char *needle, const char *replacem
@@ -237,10 +232,10 @@ index 7a25f424068d798a8c65c69c6c17dd05b2e2b950..f3e3b0849bb8442ca1be6fda29ffcc79
      struct timeval tv;
      gettimeofday (&tv, NULL); // blacklist-ignore
 diff --git a/common/time_util.h b/common/time_util.h
-index 207e95838126a503567bf21b4bc8a3f32774afd8..c42466b3a0acdd322bd33aa9c77df09adc6585f1 100644
+index c1840495a370e196e30d0ab432ddda12f4cc4819..58da911010f0030e81c2a33d6a5002f671d38145 100644
 --- a/common/time_util.h
 +++ b/common/time_util.h
-@@ -56,10 +56,10 @@ extern "C" {
+@@ -58,10 +58,10 @@ extern "C" {
  #endif
  
  typedef struct timeutil_rest timeutil_rest_t;
@@ -267,42 +262,17 @@ index 8016386ed214de371eae619f4e887aef01852408..197d6ceb330a4414483bd9db66e091d4
      timeprofile_t *tp = (timeprofile_t*) calloc(1, sizeof(timeprofile_t));
      tp->stamps = zarray_create(sizeof(struct timeprofile_entry));
 diff --git a/common/workerpool.c b/common/workerpool.c
-index a0170ef87978cb4cd2d3d8a198ffea471b6e40b2..23415eb5610751ae7f861f1deb7b25c8e56774a3 100644
+index 6b73541c09025ef907e2c3c598343955a14c3e7e..25dccd0a341cfb3991e735d7a3b2bd6b8f6c299a 100644
 --- a/common/workerpool.c
 +++ b/common/workerpool.c
-@@ -66,7 +66,7 @@ void *worker_thread(void *p)
+@@ -213,7 +213,7 @@ void workerpool_run(workerpool_t *wp)
- {
-     workerpool_t *wp = (workerpool_t*) p;
- 
--    int cnt = 0;
-+//    int cnt = 0;
- 
-     while (1) {
-         struct task *task;
-@@ -77,13 +77,13 @@ void *worker_thread(void *p)
- //          printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt);
-             pthread_cond_broadcast(&wp->endcond);
-             pthread_cond_wait(&wp->startcond, &wp->mutex);
--            cnt = 0;
-+//            cnt = 0;
- //            printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self());
-         }
- 
-         zarray_get_volatile(wp->tasks, wp->taskspos, &task);
-         wp->taskspos++;
--        cnt++;
-+//        cnt++;
-         pthread_mutex_unlock(&wp->mutex);
- //        pthread_yield();
-         sched_yield();
-@@ -203,7 +203,7 @@ void workerpool_run(workerpool_t *wp)
      }
  }
  
 -int workerpool_get_nprocs()
 +int workerpool_get_nprocs(void)
  {
- #ifdef WIN32
+ #ifdef _WIN32
      SYSTEM_INFO sysinfo;
 diff --git a/common/workerpool.h b/common/workerpool.h
 index 2c32ab1eb7f9fe1e803aef187d4af3aea9284238..070a983cbb0ce24450297dba2f58a903977c5a24 100644
@@ -314,126 +284,11 @@ index 2c32ab1eb7f9fe1e803aef187d4af3aea9284238..070a983cbb0ce24450297dba2f58a903
  
 -int workerpool_get_nprocs();
 +int workerpool_get_nprocs(void);
-diff --git a/common/zarray.c b/common/zarray.c
-index 43e6a7e0c6fb6d06414d6910e511d362ccc35354..fa1f8a25b6cad5661d70aa81b40b991126a9ac7c 100644
---- a/common/zarray.c
-+++ b/common/zarray.c
-@@ -41,7 +41,7 @@ int zstrcmp(const void * a_pp, const void * b_pp)
-     return strcmp(a,b);
- }
- 
--void zarray_vmap(zarray_t *za, void (*f)())
-+void zarray_vmap(zarray_t *za, void (*f)(void*))
- {
-     assert(za != NULL);
-     assert(f != NULL);
-diff --git a/common/zarray.h b/common/zarray.h
-index 22b4c2bb4773d5d80e33c14a7b4945ab18586939..411ee7b6495b4b37813d08dc42848ce96734fb39 100644
---- a/common/zarray.h
-+++ b/common/zarray.h
-@@ -355,7 +355,7 @@ static inline void zarray_map(zarray_t *za, void (*f)(void*))
-  *
-  * void map_function(element_type *element)
-  */
--    void zarray_vmap(zarray_t *za, void (*f)());
-+    void zarray_vmap(zarray_t *za, void (*f)(void*));
- 
- /**
-  * Removes all elements from the array and sets its size to zero. Pointers to
-diff --git a/common/zhash.c b/common/zhash.c
-index faf52233ed92f64b87ddbab255121c8202b13019..10eb6eb8a6e3ce70e51a117009dcbea3aa381d4c 100644
---- a/common/zhash.c
-+++ b/common/zhash.c
-@@ -352,7 +352,7 @@ void zhash_iterator_remove(zhash_iterator_t *zit)
-     zit->last_entry--;
- }
- 
--void zhash_map_keys(zhash_t *zh, void (*f)())
-+void zhash_map_keys(zhash_t *zh, void (*f)(void *))
- {
-     assert(zh != NULL);
-     if (f == NULL)
-@@ -368,7 +368,7 @@ void zhash_map_keys(zhash_t *zh, void (*f)())
-     }
- }
- 
--void zhash_vmap_keys(zhash_t * zh, void (*f)())
-+void zhash_vmap_keys(zhash_t * zh, void (*f)(void *))
- {
-     assert(zh != NULL);
-     if (f == NULL)
-@@ -385,7 +385,7 @@ void zhash_vmap_keys(zhash_t * zh, void (*f)())
-     }
- }
- 
--void zhash_map_values(zhash_t * zh, void (*f)())
-+void zhash_map_values(zhash_t * zh, void (*f)(void *))
- {
-     assert(zh != NULL);
-     if (f == NULL)
-@@ -400,7 +400,7 @@ void zhash_map_values(zhash_t * zh, void (*f)())
-     }
- }
- 
--void zhash_vmap_values(zhash_t * zh, void (*f)())
-+void zhash_vmap_values(zhash_t * zh, void (*f)(void *))
- {
-     assert(zh != NULL);
-     if (f == NULL)
-diff --git a/common/zhash.h b/common/zhash.h
-index f3dee1aa40f6f2717fa23d7e7f856bd8864a99b3..9993a66e4aed22d998d58e10a5d0db0193fcff95 100644
---- a/common/zhash.h
-+++ b/common/zhash.h
-@@ -259,7 +259,7 @@ void zhash_iterator_remove(zhash_iterator_t *zit);
-  * for the key, which the caller should not modify, as the hash table will not be
-  * re-indexed. The function may be NULL, in which case no action is taken.
-  */
--void zhash_map_keys(zhash_t *zh, void (*f)());
-+void zhash_map_keys(zhash_t *zh, void (*f)(void *));
- 
- /**
-  * Calls the supplied function with a pointer to every value in the hash table in
-@@ -267,7 +267,7 @@ void zhash_map_keys(zhash_t *zh, void (*f)());
-  * for the value, which the caller may safely modify. The function may be NULL,
-  * in which case no action is taken.
-  */
--void zhash_map_values(zhash_t *zh, void (*f)());
-+void zhash_map_values(zhash_t *zh, void (*f)(void *));
- 
- /**
-  * Calls the supplied function with a copy of every key in the hash table in
-@@ -280,7 +280,7 @@ void zhash_map_values(zhash_t *zh, void (*f)());
-  * Use with non-pointer keys (i.e. integer, double, etc.) will likely cause a
-  * segmentation fault.
-  */
--void zhash_vmap_keys(zhash_t *vh, void (*f)());
-+void zhash_vmap_keys(zhash_t *vh, void (*f)(void *));
- 
- /**
-  * Calls the supplied function with a copy of every value in the hash table in
-@@ -293,7 +293,7 @@ void zhash_vmap_keys(zhash_t *vh, void (*f)());
-  * Use with non-pointer values (i.e. integer, double, etc.) will likely cause a
-  * segmentation fault.
-  */
--void zhash_vmap_values(zhash_t *vh, void (*f)());
-+void zhash_vmap_values(zhash_t *vh, void (*f)(void *));
- 
- /**
-  * Returns an array which contains copies of all of the hash table's keys, in no
 diff --git a/common/zmaxheap.c b/common/zmaxheap.c
-index ed271420d229a98b56c7027f47d9830a78a97db1..a073ae5a17fdd5f11f4e17d56cc05548113f6a6c 100644
+index abbb549e41b6073f581b5c3216f024ed370b9212..af0cc1eedfc63670c51e5818444b6bc2d67d2c3a 100644
 --- a/common/zmaxheap.c
 +++ b/common/zmaxheap.c
-@@ -167,7 +167,7 @@ void zmaxheap_add(zmaxheap_t *heap, void *p, float v)
+@@ -357,7 +357,7 @@ void zmaxheap_iterator_finish(zmaxheap_iterator_t *it)
-     }
- }
- 
--void zmaxheap_vmap(zmaxheap_t *heap, void (*f)())
-+void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*))
- {
-     assert(heap != NULL);
-     assert(f != NULL);
-@@ -358,7 +358,7 @@ void zmaxheap_iterator_finish(zmaxheap_iterator_t *it)
          maxheapify(heap, i);
  }
  
@@ -442,19 +297,6 @@ index ed271420d229a98b56c7027f47d9830a78a97db1..a073ae5a17fdd5f11f4e17d56cc05548
  {
      int cap = 10000;
      int sz = 0;
-diff --git a/common/zmaxheap.h b/common/zmaxheap.h
-index f0020f92e7a46c4814f30d77c189931e0e276d18..af9d1458c518e365828a5dd3bb7050cf57d2ce9b 100644
---- a/common/zmaxheap.h
-+++ b/common/zmaxheap.h
-@@ -39,7 +39,7 @@ struct zmaxheap_iterator {
- 
- zmaxheap_t *zmaxheap_create(size_t el_sz);
- 
--void zmaxheap_vmap(zmaxheap_t *heap, void (*f)());
-+void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*));
- 
- void zmaxheap_destroy(zmaxheap_t *heap);
- 
 diff --git a/tag16h5.c b/tag16h5.c
 index 775f33c7e2d91af83e4a62949cadebdce34e3eb9..e38302a1d2ca2a2344a775569aed266d063a8861 100644
 --- a/tag16h5.c
@@ -508,10 +350,10 @@ index 9197c8b367d4b2047cb7d9882dfc11092f0b3dce..6e0107d28f3c3da6ae148a9005077427
  
  #ifdef __cplusplus
 diff --git a/tag36h10.c b/tag36h10.c
-index 44a129e74b135e803e2324cea179ac3001718e14..843dc4e9dcb3fbde3d86ce0a7d18703f7c358c15 100644
+index 9c02c34c5db82f693191acc8975ef514f04588d5..e103748df2ac11201959747e182a17c3f4710b11 100644
 --- a/tag36h10.c
 +++ b/tag36h10.c
-@@ -2323,7 +2323,7 @@ static uint64_t codedata[2320] = {
+@@ -2350,7 +2350,7 @@ static uint64_t codedata[2320] = {
     0x0000000447b9e7acUL,
     0x0000000d9f564f30UL,
  };

upstream_utils/apriltag_patches/0008-Remove-GCC-diagnostic-pragmas-on-windows.patch

@@ -0,0 +1,24 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Ryan Blue <ryanzblue@gmail.com>
+Date: Fri, 23 Aug 2024 02:50:24 -0400
+Subject: [PATCH 8/8] Remove GCC diagnostic pragmas on windows
+
+---
+ common/pthreads_cross.c | 3 ---
+ 1 file changed, 3 deletions(-)
+
+diff --git a/common/pthreads_cross.c b/common/pthreads_cross.c
+index d48b81a1bbafa15f1c8a5b4fec6a755847947ce6..907fa62f3cf11f8afbe51a3ca5a492111720d707 100644
+--- a/common/pthreads_cross.c
++++ b/common/pthreads_cross.c
+@@ -44,10 +44,7 @@ int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routin
+     if (thread == NULL || start_routine == NULL)
+         return 1;
+ 
+-#pragma GCC diagnostic push
+-#pragma GCC diagnostic ignored "-Wcast-function-type"
+     *thread = (HANDLE) CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)start_routine, arg, 0, NULL);
+-#pragma GCC diagnostic pop
+     if (*thread == NULL)
+         return 1;
+     return 0;