From eeb91ba8aa01eed469cc9a1cfe02d05808a4c388 Mon Sep 17 00:00:00 2001 From: Souriya Trinh Date: Sun, 7 Jul 2024 00:30:47 +0200 Subject: [PATCH] Try to backport most of the changes from main AprilTag repo as of 188c0e0 version. --- 3rdparty/apriltag/apriltag.cpp | 61 +-- 3rdparty/apriltag/apriltag.h | 12 +- 3rdparty/apriltag/apriltag_pose.cpp | 2 - 3rdparty/apriltag/apriltag_pose.h | 19 +- 3rdparty/apriltag/apriltag_quad_thresh.cpp | 435 ++++++++++++--------- 3rdparty/apriltag/common/g2d.cpp | 4 +- 3rdparty/apriltag/common/getopt.cpp | 2 +- 3rdparty/apriltag/common/homography.cpp | 3 +- 3rdparty/apriltag/common/homography.h | 2 +- 3rdparty/apriltag/common/image_u8.cpp | 5 +- 3rdparty/apriltag/common/image_u8x3.cpp | 4 +- 3rdparty/apriltag/common/image_u8x3.h | 2 +- 3rdparty/apriltag/common/matd.cpp | 161 ++++---- 3rdparty/apriltag/common/matd.h | 6 +- 3rdparty/apriltag/common/pam.cpp | 4 +- 3rdparty/apriltag/common/pam.h | 2 +- 3rdparty/apriltag/common/pjpeg.cpp | 25 +- 3rdparty/apriltag/common/pnm.cpp | 1 - 3rdparty/apriltag/common/string_util.cpp | 28 +- 3rdparty/apriltag/common/string_util.h | 4 +- 3rdparty/apriltag/common/svd22.cpp | 5 - 3rdparty/apriltag/common/unionfind.h | 63 ++- 3rdparty/apriltag/common/workerpool.cpp | 38 +- 3rdparty/apriltag/common/zarray.cpp | 2 - 3rdparty/apriltag/common/zarray.h | 48 +-- 3rdparty/apriltag/common/zhash.cpp | 10 +- 3rdparty/apriltag/common/zmaxheap.cpp | 10 +- 27 files changed, 516 insertions(+), 442 deletions(-) diff --git a/3rdparty/apriltag/apriltag.cpp b/3rdparty/apriltag/apriltag.cpp index 633824d961..122c54e0da 100644 --- a/3rdparty/apriltag/apriltag.cpp +++ b/3rdparty/apriltag/apriltag.cpp @@ -42,18 +42,15 @@ either expressed or implied, of the Regents of The University of Michigan. #include #include #include -//#include #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; } diff --git a/3rdparty/apriltag/apriltag.h b/3rdparty/apriltag/apriltag.h index 3417341d97..108eb82149 100644 --- a/3rdparty/apriltag/apriltag.h +++ b/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 //} diff --git a/3rdparty/apriltag/apriltag_pose.cpp b/3rdparty/apriltag/apriltag_pose.cpp index 55052c2e64..e74c1fd212 100644 --- a/3rdparty/apriltag/apriltag_pose.cpp +++ b/3rdparty/apriltag/apriltag_pose.cpp @@ -2,9 +2,7 @@ #include #include "apriltag_pose.h" -#include "apriltag_math.h" #include "common/homography.h" -#include "common/image_u8x3.h" /** diff --git a/3rdparty/apriltag/apriltag_pose.h b/3rdparty/apriltag/apriltag_pose.h index 7dbff5ceea..4733556de6 100644 --- a/3rdparty/apriltag/apriltag_pose.h +++ b/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; /** diff --git a/3rdparty/apriltag/apriltag_quad_thresh.cpp b/3rdparty/apriltag/apriltag_quad_thresh.cpp index de8564381f..bd4c710a4a 100644 --- a/3rdparty/apriltag/apriltag_quad_thresh.cpp +++ b/3rdparty/apriltag/apriltag_quad_thresh.cpp @@ -40,7 +40,6 @@ either expressed or implied, of the Regents of The University of Michigan. #include "apriltag.h" #include "common/image_u8x3.h" #include "common/zarray.h" -#include "common/zhash.h" #include "common/unionfind.h" #include "common/timeprofile.h" #include "common/zmaxheap.h" @@ -114,6 +113,34 @@ struct cluster_task zarray_t* clusters; }; +struct minmax_task { + int ty; + + image_u8_t *im; + uint8_t *im_max; + uint8_t *im_min; +}; + +struct blur_task { + int ty; + + image_u8_t *im; + uint8_t *im_max; + uint8_t *im_min; + uint8_t *im_max_tmp; + uint8_t *im_min_tmp; +}; + +struct threshold_task { + int ty; + + apriltag_detector_t *td; + image_u8_t *im; + image_u8_t *threshim; + uint8_t *im_max; + uint8_t *im_min; +}; + struct remove_vertex { int i; // which vertex to remove? @@ -245,8 +272,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; - lineparm[3] = ny/length; + if (fabs(length) < 1e-12) { + lineparm[2] = lineparm[3] = 0; + } + else { + lineparm[2] = nx/length; + lineparm[3] = ny/length; + } } // sum of squared errors = @@ -413,7 +445,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); @@ -443,11 +475,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; @@ -478,7 +510,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); @@ -825,43 +857,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]; @@ -869,7 +866,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; } @@ -880,10 +877,10 @@ int fit_quad( int i0 = indices[i]; int i1 = indices[(i+1)&3]; - double err; - fit_line(lfps, sz, i0, i1, lines[i], NULL, &err); + double mse; + 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; } @@ -913,11 +910,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; @@ -990,7 +987,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; @@ -1005,7 +1002,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); @@ -1051,7 +1048,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); } } @@ -1066,26 +1063,26 @@ static void do_quad_task(void *p) for (int cidx = task->cidx0; cidx < task->cidx1; cidx++) { - zarray_t *cluster; - zarray_get(clusters, cidx, &cluster); + zarray_t **cluster; + zarray_get_volatile(clusters, cidx, &cluster); - if (zarray_size(cluster) < td->qtp.min_cluster_pixels) + if (zarray_size(*cluster) < td->qtp.min_cluster_pixels) continue; // 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 - // times (because it has 3 neighbors). The maximum perimeter - // is 2w+2h. - if (zarray_size(cluster) > 3*(2*w+2*h)) { + // fit quads to.) A typical point along an edge is added two + // times (because it has 2 unique neighbors). The maximum + // perimeter is 2w+2h. + if (zarray_size(*cluster) > 2*(2*w+2*h)) { continue; } struct quad quad; memset(&quad, 0, sizeof(struct quad)); - if (fit_quad(td, task->im, cluster, &quad, task->tag_width, task->normal_border, task->reversed_border)) { + if (fit_quad(td, task->im, *cluster, &quad, task->tag_width, task->normal_border, task->reversed_border)) { pthread_mutex_lock(&td->mutex); zarray_add(quads, &quad); pthread_mutex_unlock(&td->mutex); @@ -1093,6 +1090,122 @@ static void do_quad_task(void *p) } } +void do_minmax_task(void *p) +{ + const int tilesz = 4; + struct minmax_task* task = (struct minmax_task*) p; + int s = task->im->stride; + int ty = task->ty; + int tw = task->im->width / tilesz; + image_u8_t *im = task->im; + + for (int tx = 0; tx < tw; tx++) { + uint8_t max = 0, min = 255; + + for (int dy = 0; dy < tilesz; dy++) { + + for (int dx = 0; dx < tilesz; dx++) { + + uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx]; + if (v < min) + min = v; + if (v > max) + max = v; + } + } + + task->im_max[ty*tw+tx] = max; + task->im_min[ty*tw+tx] = min; + } +} + +void do_blur_task(void *p) +{ + const int tilesz = 4; + struct blur_task* task = (struct blur_task*) p; + int ty = task->ty; + int tw = task->im->width / tilesz; + int th = task->im->height / tilesz; + uint8_t *im_max = task->im_max; + uint8_t *im_min = task->im_min; + + for (int tx = 0; tx < tw; tx++) { + uint8_t max = 0, min = 255; + + for (int dy = -1; dy <= 1; dy++) { + if (ty+dy < 0 || ty+dy >= th) + continue; + for (int dx = -1; dx <= 1; dx++) { + if (tx+dx < 0 || tx+dx >= tw) + continue; + + uint8_t m = im_max[(ty+dy)*tw+tx+dx]; + if (m > max) + max = m; + m = im_min[(ty+dy)*tw+tx+dx]; + if (m < min) + min = m; + } + } + + task->im_max_tmp[ty*tw + tx] = max; + task->im_min_tmp[ty*tw + tx] = min; + } +} + +void do_threshold_task(void *p) +{ + const int tilesz = 4; + struct threshold_task* task = (struct threshold_task*) p; + int ty = task->ty; + int tw = task->im->width / tilesz; + int s = task->im->stride; + uint8_t *im_max = task->im_max; + uint8_t *im_min = task->im_min; + image_u8_t *im = task->im; + image_u8_t *threshim = task->threshim; + int min_white_black_diff = task->td->qtp.min_white_black_diff; + + for (int tx = 0; tx < tw; tx++) { + int min = im_min[ty*tw + tx]; + int max = im_max[ty*tw + tx]; + + // low contrast region? (no edges) + if (max - min < min_white_black_diff) { + for (int dy = 0; dy < tilesz; dy++) { + int y = ty*tilesz + dy; + + for (int dx = 0; dx < tilesz; dx++) { + int x = tx*tilesz + dx; + + threshim->buf[y*s+x] = 127; + } + } + continue; + } + + // otherwise, actually threshold this tile. + + // argument for biasing towards dark; specular highlights + // can be substantially brighter than white tag parts + uint8_t thresh = min + (max - min) / 2; + + for (int dy = 0; dy < tilesz; dy++) { + int y = ty*tilesz + dy; + + for (int dx = 0; dx < tilesz; dx++) { + int x = tx*tilesz + dx; + + uint8_t v = im->buf[y*s+x]; + if (v > thresh) + threshim->buf[y*s+x] = 255; + else + threshim->buf[y*s+x] = 0; + } + } + } +} + image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im) { int w = im->width, h = im->height, s = im->stride; @@ -1135,27 +1248,18 @@ image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im) uint8_t *im_max = (uint8_t *)calloc(tw*th, sizeof(uint8_t)); uint8_t *im_min = (uint8_t *)calloc(tw*th, sizeof(uint8_t)); + struct minmax_task *minmax_tasks = malloc(sizeof(struct minmax_task)*th); // first, collect min/max statistics for each tile for (int ty = 0; ty < th; ty++) { - for (int tx = 0; tx < tw; tx++) { - uint8_t max = 0, min = 255; + minmax_tasks[ty].im = im; + minmax_tasks[ty].im_max = im_max; + minmax_tasks[ty].im_min = im_min; + minmax_tasks[ty].ty = ty; - for (int dy = 0; dy < tilesz; dy++) { - - for (int dx = 0; dx < tilesz; dx++) { - - uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx]; - if (v < min) - min = v; - if (v > max) - max = v; - } - } - - im_max[ty*tw+tx] = max; - im_min[ty*tw+tx] = min; - } + workerpool_add_task(td->wp, do_minmax_task, &minmax_tasks[ty]); } + workerpool_run(td->wp); + free(minmax_tasks); // second, apply 3x3 max/min convolution to "blur" these values // over larger areas. This reduces artifacts due to abrupt changes @@ -1164,77 +1268,38 @@ image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im) uint8_t *im_max_tmp = (uint8_t *)calloc(tw*th, sizeof(uint8_t)); uint8_t *im_min_tmp = (uint8_t *)calloc(tw*th, sizeof(uint8_t)); + struct blur_task *blur_tasks = malloc(sizeof(struct blur_task)*th); for (int ty = 0; ty < th; ty++) { - for (int tx = 0; tx < tw; tx++) { - uint8_t max = 0, min = 255; - - for (int dy = -1; dy <= 1; dy++) { - if (ty+dy < 0 || ty+dy >= th) - continue; - for (int dx = -1; dx <= 1; dx++) { - if (tx+dx < 0 || tx+dx >= tw) - continue; - - uint8_t m = im_max[(ty+dy)*tw+tx+dx]; - if (m > max) - max = m; - m = im_min[(ty+dy)*tw+tx+dx]; - if (m < min) - min = m; - } - } - - im_max_tmp[ty*tw + tx] = max; - im_min_tmp[ty*tw + tx] = min; - } + blur_tasks[ty].im = im; + blur_tasks[ty].im_max = im_max; + blur_tasks[ty].im_min = im_min; + blur_tasks[ty].im_max_tmp = im_max_tmp; + blur_tasks[ty].im_min_tmp = im_min_tmp; + blur_tasks[ty].ty = ty; + + workerpool_add_task(td->wp, do_blur_task, &blur_tasks[ty]); } + workerpool_run(td->wp); + free(blur_tasks); free(im_max); free(im_min); im_max = im_max_tmp; im_min = im_min_tmp; } + struct threshold_task *threshold_tasks = malloc(sizeof(struct threshold_task)*th); for (int ty = 0; ty < th; ty++) { - for (int tx = 0; tx < tw; tx++) { - - int min = im_min[ty*tw + tx]; - int max = im_max[ty*tw + tx]; - - // low contrast region? (no edges) - if (max - min < td->qtp.min_white_black_diff) { - for (int dy = 0; dy < tilesz; dy++) { - int y = ty*tilesz + dy; - - for (int dx = 0; dx < tilesz; dx++) { - int x = tx*tilesz + dx; - - threshim->buf[y*s+x] = 127; - } - } - continue; - } - - // otherwise, actually threshold this tile. - - // argument for biasing towards dark; specular highlights - // can be substantially brighter than white tag parts - uint8_t thresh = min + (max - min) / 2; - - for (int dy = 0; dy < tilesz; dy++) { - int y = ty*tilesz + dy; - - for (int dx = 0; dx < tilesz; dx++) { - int x = tx*tilesz + dx; - - uint8_t v = im->buf[y*s+x]; - if (v > thresh) - threshim->buf[y*s+x] = 255; - else - threshim->buf[y*s+x] = 0; - } - } - } + threshold_tasks[ty].im = im; + threshold_tasks[ty].threshim = threshim; + threshold_tasks[ty].im_max = im_max; + threshold_tasks[ty].im_min = im_min; + threshold_tasks[ty].ty = ty; + threshold_tasks[ty].td = td; + + workerpool_add_task(td->wp, do_threshold_task, &threshold_tasks[ty]); } + workerpool_run(td->wp); + free(threshold_tasks); // we skipped over the non-full-sized tiles above. Fix those now. if (1) { @@ -1278,7 +1343,7 @@ image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im) // this is a dilate/erode deglitching scheme that does not improve // anything as far as I can tell. - if (0 || td->qtp.deglitch) { + if (td->qtp.deglitch) { image_u8_t *tmp = image_u8_create(w, h); for (int y = 1; y + 1 < h; y++) { @@ -1440,12 +1505,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); @@ -1475,7 +1540,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); @@ -1493,15 +1558,19 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int mem_pools[mem_pool_idx] = (struct uint64_zarray_entry *)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; } @@ -1525,6 +1594,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]; \ @@ -1562,6 +1632,7 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int \ struct pt p = { (uint16_t)(2*x + dx), (uint16_t)(2*y + dy), (int16_t)(dx*((int) v1-v0)), (int16_t)(dy*((int) v1-v0)), 0.f}; \ zarray_add(entry->cluster, &p); \ + connected = true; \ } \ } \ } @@ -1571,8 +1642,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 @@ -1592,14 +1671,14 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int int n = end - start; for (int j = 0; j < n - 1; j++) { for (int k = 0; k < n - j - 1; k++) { - struct cluster_hash* hash1; - struct cluster_hash* hash2; - zarray_get(clusters, start + k, &hash1); - zarray_get(clusters, start + k + 1, &hash2); - if (hash1->id > hash2->id) { - struct cluster_hash tmp = *hash2; - *hash2 = *hash1; - *hash1 = tmp; + struct cluster_hash** hash1; + struct cluster_hash** hash2; + zarray_get_volatile(clusters, start + k, &hash1); + zarray_get_volatile(clusters, start + k + 1, &hash2); + if ((*hash1)->id > (*hash2)->id) { + struct cluster_hash tmp = **hash2; + **hash2 = **hash1; + **hash1 = tmp; } } } @@ -1630,38 +1709,29 @@ zarray_t* merge_clusters(zarray_t* c1, zarray_t* c2) { int l2 = zarray_size(c2); while (i1 < l1 && i2 < l2) { - struct cluster_hash* h1; - struct cluster_hash* h2; - zarray_get(c1, i1, &h1); - zarray_get(c2, i2, &h2); - - if (h1->hash == h2->hash && h1->id == h2->id) { - zarray_add_all(h1->data, h2->data); - zarray_add(ret, &h1); + struct cluster_hash** h1; + struct cluster_hash** h2; + zarray_get_volatile(c1, i1, &h1); + zarray_get_volatile(c2, i2, &h2); + + if ((*h1)->hash == (*h2)->hash && (*h1)->id == (*h2)->id) { + zarray_add_range((*h1)->data, (*h2)->data, 0, zarray_size((*h2)->data)); + zarray_add(ret, h1); i1++; i2++; - zarray_destroy(h2->data); - free(h2); - } else if (h2->hash < h1->hash || (h2->hash == h1->hash && h2->id < h1->id)) { - zarray_add(ret, &h2); + zarray_destroy((*h2)->data); + free(*h2); + } else if ((*h2)->hash < (*h1)->hash || ((*h2)->hash == (*h1)->hash && (*h2)->id < (*h1)->id)) { + zarray_add(ret, h2); i2++; } else { - zarray_add(ret, &h1); + zarray_add(ret, h1); i1++; } } - for (; i1 < l1; i1++) { - struct cluster_hash* h1; - zarray_get(c1, i1, &h1); - zarray_add(ret, &h1); - } - - for (; i2 < l2; i2++) { - struct cluster_hash* h2; - zarray_get(c2, i2, &h2); - zarray_add(ret, &h2); - } + zarray_add_range(ret, c1, i1, l1); + zarray_add_range(ret, c2, i2, l2); zarray_destroy(c1); zarray_destroy(c2); @@ -1674,7 +1744,7 @@ zarray_t* gradient_clusters(apriltag_detector_t *td, image_u8_t* threshim, int w int nclustermap = 0.2*w*h; int sz = h - 1; - int chunksize = 1 + sz / td->nthreads; + int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads); struct cluster_task *tasks = (struct cluster_task *)malloc(sizeof(struct cluster_task)*(sz / chunksize + 1)); int ntasks = 0; @@ -1720,10 +1790,10 @@ zarray_t* gradient_clusters(apriltag_detector_t *td, image_u8_t* threshim, int w clusters = zarray_create(sizeof(zarray_t*)); zarray_ensure_capacity(clusters, zarray_size(clusters_list[0])); for (int i = 0; i < zarray_size(clusters_list[0]); i++) { - struct cluster_hash* hash; - zarray_get(clusters_list[0], i, &hash); - zarray_add(clusters, &hash->data); - free(hash); + struct cluster_hash** hash; + zarray_get_volatile(clusters_list[0], i, &hash); + zarray_add(clusters, &(*hash)->data); + free(*hash); } zarray_destroy(clusters_list[0]); free(clusters_list); @@ -1746,7 +1816,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; } @@ -1808,7 +1879,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]; diff --git a/3rdparty/apriltag/common/g2d.cpp b/3rdparty/apriltag/common/g2d.cpp index 880a748131..abefbc99f3 100644 --- a/3rdparty/apriltag/common/g2d.cpp +++ b/3rdparty/apriltag/common/g2d.cpp @@ -127,7 +127,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]; @@ -329,7 +329,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++) { diff --git a/3rdparty/apriltag/common/getopt.cpp b/3rdparty/apriltag/common/getopt.cpp index e64508978d..7613b69c34 100644 --- a/3rdparty/apriltag/common/getopt.cpp +++ b/3rdparty/apriltag/common/getopt.cpp @@ -241,7 +241,7 @@ int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors) if (!strncmp(tok,"-",1) && strncmp(tok,"--",2)) { size_t len = strlen(tok); - int pos; + size_t pos; for (pos = 1; pos < len; pos++) { char sopt[2]; sopt[0] = tok[pos]; diff --git a/3rdparty/apriltag/common/homography.cpp b/3rdparty/apriltag/common/homography.cpp index 840273bbe0..dc10019baa 100644 --- a/3rdparty/apriltag/common/homography.cpp +++ b/3rdparty/apriltag/common/homography.cpp @@ -26,7 +26,6 @@ either expressed or implied, of the Regents of The University of Michigan. */ #include -#include #include "common/matd.h" #include "common/zarray.h" @@ -254,7 +253,7 @@ matd_t *homography_compute(zarray_t *correspondences, int flags) // And that the homography is equal to the projection matrix times the // model matrix, recover the model matrix (which is returned). Note // that the third column of the model matrix is missing in the -// expresison below, reflecting the fact that the homography assumes +// expression below, reflecting the fact that the homography assumes // all points are at z=0 (i.e., planar) and that the element of z is // thus omitted. (3x1 instead of 4x1). // diff --git a/3rdparty/apriltag/common/homography.h b/3rdparty/apriltag/common/homography.h index 49701ceb3f..10863c78cd 100644 --- a/3rdparty/apriltag/common/homography.h +++ b/3rdparty/apriltag/common/homography.h @@ -148,7 +148,7 @@ static inline void homography_project(const matd_t *H, double x, double y, doubl // // And that the homography is equal to the projection matrix times the model matrix, // recover the model matrix (which is returned). Note that the third column of the model -// matrix is missing in the expresison below, reflecting the fact that the homography assumes +// matrix is missing in the expression below, reflecting the fact that the homography assumes // all points are at z=0 (i.e., planar) and that the element of z is thus omitted. // (3x1 instead of 4x1). // diff --git a/3rdparty/apriltag/common/image_u8.cpp b/3rdparty/apriltag/common/image_u8.cpp index cacfaad303..fc4d9fbb6d 100644 --- a/3rdparty/apriltag/common/image_u8.cpp +++ b/3rdparty/apriltag/common/image_u8.cpp @@ -212,7 +212,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; } @@ -266,6 +266,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 diff --git a/3rdparty/apriltag/common/image_u8x3.cpp b/3rdparty/apriltag/common/image_u8x3.cpp index 1f1d6d49ce..f39607ced2 100644 --- a/3rdparty/apriltag/common/image_u8x3.cpp +++ b/3rdparty/apriltag/common/image_u8x3.cpp @@ -149,7 +149,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; @@ -167,7 +167,7 @@ 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]) { double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0)); double delta = 0.5 / dist; diff --git a/3rdparty/apriltag/common/image_u8x3.h b/3rdparty/apriltag/common/image_u8x3.h index 004ce87ca3..de1d1d3e9d 100644 --- a/3rdparty/apriltag/common/image_u8x3.h +++ b/3rdparty/apriltag/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 //} diff --git a/3rdparty/apriltag/common/matd.cpp b/3rdparty/apriltag/common/matd.cpp index c5e85c0594..9b1dd5a772 100644 --- a/3rdparty/apriltag/common/matd.cpp +++ b/3rdparty/apriltag/common/matd.cpp @@ -108,20 +108,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); @@ -130,9 +128,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; @@ -167,12 +163,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(c0 >= 0 && c0 < a->ncols); + assert(r0 < a->nrows); + assert(c0 < a->ncols); int nrows = r1 - r0 + 1; int ncols = c1 - c0 + 1; @@ -195,8 +191,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 (int j = 0; j < m->ncols; j++) { + for (unsigned int i = 0; i < m->nrows; i++) { + for (unsigned int j = 0; j < m->ncols; j++) { printf(fmt, MATD_EL(m, i, j)); } printf("\n"); @@ -213,8 +209,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 (int i = 0; i < m->nrows; i++) { + for (unsigned int j = 0; j < m->ncols; j++) { + for (unsigned int i = 0; i < m->nrows; i++) { printf(fmt, MATD_EL(m, i, j)); } printf("\n"); @@ -245,10 +241,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 (int j = 0; j < m->ncols; j++) { + for (unsigned int i = 0; i < m->nrows; i++) { + 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; @@ -267,8 +263,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 (int j = 0; j < m->ncols; j++) { + for (unsigned int i = 0; i < m->nrows; i++) { + for (unsigned int j = 0; j < m->ncols; j++) { MATD_EL(m, i, j) = s * MATD_EL(a, i, j); } } @@ -285,8 +281,8 @@ void matd_scale_inplace(matd_t *a, double s) return; } - for (int i = 0; i < a->nrows; i++) { - for (int j = 0; j < a->ncols; j++) { + for (unsigned int i = 0; i < a->nrows; i++) { + for (unsigned int j = 0; j < a->ncols; j++) { MATD_EL(a, i, j) *= s; } } @@ -304,8 +300,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 (int j = 0; j < m->ncols; j++) { + for (unsigned int i = 0; i < m->nrows; i++) { + for (unsigned int j = 0; j < m->ncols; j++) { MATD_EL(m, i, j) = MATD_EL(a, i, j) + MATD_EL(b, i, j); } } @@ -325,8 +321,8 @@ void matd_add_inplace(matd_t *a, const matd_t *b) return; } - for (int i = 0; i < a->nrows; i++) { - for (int j = 0; j < a->ncols; j++) { + for (unsigned int i = 0; i < a->nrows; i++) { + for (unsigned int j = 0; j < a->ncols; j++) { MATD_EL(a, i, j) += MATD_EL(b, i, j); } } @@ -345,8 +341,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 (int j = 0; j < m->ncols; j++) { + for (unsigned int i = 0; i < m->nrows; i++) { + for (unsigned int j = 0; j < m->ncols; j++) { MATD_EL(m, i, j) = MATD_EL(a, i, j) - MATD_EL(b, i, j); } } @@ -366,8 +362,8 @@ void matd_subtract_inplace(matd_t *a, const matd_t *b) return; } - for (int i = 0; i < a->nrows; i++) { - for (int j = 0; j < a->ncols; j++) { + for (unsigned int i = 0; i < a->nrows; i++) { + for (unsigned int j = 0; j < a->ncols; j++) { MATD_EL(a, i, j) -= MATD_EL(b, i, j); } } @@ -383,8 +379,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 (int j = 0; j < a->ncols; j++) { + for (unsigned int i = 0; i < a->nrows; i++) { + for (unsigned int j = 0; j < a->ncols; j++) { MATD_EL(m, j, i) = MATD_EL(a, i, j); } } @@ -402,7 +398,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); } @@ -880,6 +876,8 @@ double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n) int lenb = b->nrows*b->ncols; assert(n <= lena && n <= lenb); + (void)lena; + (void)lenb; double mag = 0.0; for (int i = 0; i < n; i++) @@ -914,6 +912,7 @@ double matd_vec_dot_product(const matd_t *a, const matd_t *b) int adim = a->ncols*a->nrows; int bdim = b->ncols*b->nrows; assert(adim == bdim); + (void)bdim; double acc = 0; for (int i = 0; i < adim; i++) { @@ -962,8 +961,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 (int j = 0; j < a->ncols; j++) { + for (unsigned int i = 0; i < a->nrows; i++) { + for (unsigned int j = 0; j < a->ncols; j++) { TYPE av = MATD_EL(a, i, j); TYPE bv = MATD_EL(b, i, j); @@ -1007,7 +1006,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 @@ -1067,7 +1066,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]; @@ -1076,7 +1075,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]; @@ -1125,7 +1124,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]; @@ -1134,7 +1133,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]; @@ -1163,11 +1162,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 = (int *)malloc(sizeof(int)*B->ncols); - int lastmaxi, lastmaxj; + unsigned int *maxrowidx = (unsigned int *)malloc(sizeof(int)*B->ncols); + 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 @@ -1200,7 +1199,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. @@ -1273,8 +1272,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 (int j = 0; j < B->ncols; j++) { + for (unsigned int i = 0; i < B->ncols; i++) { + for (unsigned int j = 0; j < B->ncols; j++) { if (i == j) continue; @@ -1339,7 +1338,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); @@ -1348,7 +1347,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); @@ -1358,7 +1357,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); @@ -1367,7 +1366,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); @@ -1390,7 +1389,7 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags) // U/LS. int *idxs = (int *)malloc(sizeof(int)*A->ncols); double *vals = (double *)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); } @@ -1400,7 +1399,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]; @@ -1418,7 +1417,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; @@ -1446,8 +1445,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 (int j = 0; j < B->ncols; j++) { + for (unsigned int i = 0; i < B->nrows; i++) { + for (unsigned int j = 0; j < B->ncols; j++) { if (i != j) MATD_EL(B, i, j) = 0; } @@ -1524,11 +1523,11 @@ matd_plu_t *matd_plu(const matd_t *a) matd_plu_t *mlu = (matd_plu_t *)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 (int i = 0; i < a->nrows; i++) { + for (unsigned int j = 0; j < a->ncols; j++) { + 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) @@ -1540,9 +1539,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; } @@ -1581,7 +1580,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; } } @@ -1607,7 +1606,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); } @@ -1619,7 +1618,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; } @@ -1631,10 +1630,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); } } @@ -1647,8 +1646,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 (int j = 0; j < lu->ncols; j++) { + for (unsigned int i = 0; i < lu->ncols; i++) { + for (unsigned int j = 0; j < lu->ncols; j++) { if (i <= j) MATD_EL(U, i, j) = MATD_EL(lu, i, j); } @@ -1666,14 +1665,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 (int i = k+1; i < mlu->lu->nrows; i++) { + for (unsigned int k = 0; k < mlu->lu->nrows; k++) { + 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; } } @@ -1681,12 +1680,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; } } @@ -1914,7 +1913,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); } } @@ -1944,7 +1943,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; @@ -1961,17 +1960,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 (int j = 0; j < i; j++) { + for (unsigned int i = 0; i < u->nrows; i++) { + 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); } } @@ -1979,12 +1978,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; } } @@ -2020,8 +2019,8 @@ matd_t *matd_chol_inverse(matd_t *a) double matd_max(matd_t *m) { double d = -DBL_MAX; - for(int x=0; xnrows; x++) { - for(int y=0; yncols; y++) { + for(unsigned int x=0; xnrows; x++) { + for(unsigned int y=0; yncols; y++) { if(MATD_EL(m, x, y) > d) d = MATD_EL(m, x, y); } diff --git a/3rdparty/apriltag/common/matd.h b/3rdparty/apriltag/common/matd.h index b4f33a4e99..f1a8a7f216 100644 --- a/3rdparty/apriltag/common/matd.h +++ b/3rdparty/apriltag/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 diff --git a/3rdparty/apriltag/common/pam.cpp b/3rdparty/apriltag/common/pam.cpp index 0fa4aabf5c..0d582d14df 100644 --- a/3rdparty/apriltag/common/pam.cpp +++ b/3rdparty/apriltag/common/pam.cpp @@ -64,7 +64,7 @@ pam_t *pam_create_from_file(const char *inpath) continue; size_t linelen = strlen(line); - for (int idx = 0; idx < linelen; idx++) { + for (size_t idx = 0; idx < linelen; idx++) { if (line[idx] == ' ') { line[idx] = 0; if (tok1) { @@ -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; diff --git a/3rdparty/apriltag/common/pam.h b/3rdparty/apriltag/common/pam.h index 67fff6748d..5e03a1308f 100644 --- a/3rdparty/apriltag/common/pam.h +++ b/3rdparty/apriltag/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; }; diff --git a/3rdparty/apriltag/common/pjpeg.cpp b/3rdparty/apriltag/common/pjpeg.cpp index befb354f14..bed1fb74b3 100644 --- a/3rdparty/apriltag/common/pjpeg.cpp +++ b/3rdparty/apriltag/common/pjpeg.cpp @@ -95,7 +95,7 @@ static uint8_t mjpeg_dht[] = { // header 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B, ///////////////////////////////////////////////////////////// - // chrominance DC coefficents + // chrominance DC coefficients // DC table 1 0x01, // code lengths @@ -422,7 +422,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++; @@ -752,8 +752,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 (int x = 0; x < pj->width; x++) { + for (uint32_t y = 0; y < pj->height; y++) { + 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; @@ -768,8 +768,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 (int bx = 0; bx < pj->width / Cb_factor_x; bx++) { + for (uint32_t by = 0; by < pj->height / Cb_factor_y; by++) { + 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; @@ -799,8 +799,8 @@ image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj) } } else { - for (int y = 0; y < pj->height; y++) { - for (int x = 0; x < pj->width; x++) { + for (uint32_t y = 0; y < pj->height; y++) { + 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; @@ -823,7 +823,7 @@ image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj) // returns NULL if file loading fails. pjpeg_t *pjpeg_create_from_file(const char *path, uint32_t flags, int *error) { - FILE *f = fopen(path, "r"); + FILE *f = fopen(path, "rb"); if (f == NULL) return NULL; @@ -833,6 +833,12 @@ pjpeg_t *pjpeg_create_from_file(const char *path, uint32_t flags, int *error) uint8_t *buf = (uint8_t *)malloc(buflen); fseek(f, 0, SEEK_SET); int res = fread(buf, 1, buflen, f); + + if ( ferror(f) ){ + debug_print ("Read failed"); + clearerr(f); + } + fclose(f); if (res != buflen) { free(buf); @@ -857,6 +863,7 @@ pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int pjd.inlen = sizeof(mjpeg_dht); int result = pjpeg_decode_buffer(&pjd); assert(result == 0); + (void)result; } pjd.in = buf; diff --git a/3rdparty/apriltag/common/pnm.cpp b/3rdparty/apriltag/common/pnm.cpp index cebbb0c89c..d3812ace78 100644 --- a/3rdparty/apriltag/common/pnm.cpp +++ b/3rdparty/apriltag/common/pnm.cpp @@ -26,7 +26,6 @@ either expressed or implied, of the Regents of The University of Michigan. */ #include -#include #include #include diff --git a/3rdparty/apriltag/common/string_util.cpp b/3rdparty/apriltag/common/string_util.cpp index 7a53b5b4cc..d1a852879f 100644 --- a/3rdparty/apriltag/common/string_util.cpp +++ b/3rdparty/apriltag/common/string_util.cpp @@ -39,7 +39,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 }; @@ -86,7 +86,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); @@ -249,7 +249,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'; } @@ -262,7 +262,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'); } @@ -455,14 +455,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 = (char *)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; } @@ -478,7 +477,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) @@ -506,9 +504,10 @@ 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); assert(c == str[i]); + (void)c; } } @@ -576,15 +575,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(endidx < 0 || endidx >= startidx); - assert(endidx < 0 || endidx <= strlen(str)+1); - - if (endidx < 0) - endidx = (long) strlen(str); + assert(startidx <= strlen(str)+1); + assert(endidx >= startidx); + assert(endidx <= strlen(str)+1); size_t blen = endidx - startidx; // not counting \0 char *b = (char *)malloc(blen + 1); @@ -603,7 +599,7 @@ char *str_replace(const char *haystack, const char *needle, const char *replacem size_t haystack_len = strlen(haystack); size_t needle_len = strlen(needle); - int pos = 0; + size_t pos = 0; while (pos < haystack_len) { if (needle_len > 0 && str_starts_with(&haystack[pos], needle)) { string_buffer_append_string(sb, replacement); diff --git a/3rdparty/apriltag/common/string_util.h b/3rdparty/apriltag/common/string_util.h index 152b3ae720..d2ba85560e 100644 --- a/3rdparty/apriltag/common/string_util.h +++ b/3rdparty/apriltag/common/string_util.h @@ -68,7 +68,7 @@ int str_diff_idx(const char * a, const char * b); * contain the delimiter. The original string will remain unchanged. * If str is composed of all delimiters, an empty array will be returned. * - * It is the caller's responsibilty to free the returned zarray, as well as + * It is the caller's responsibility to free the returned zarray, as well as * the strings contained within it, e.g.: * * zarray_t *za = str_split("this is a haystack", " "); @@ -184,7 +184,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 diff --git a/3rdparty/apriltag/common/svd22.cpp b/3rdparty/apriltag/common/svd22.cpp index 2675b1cf86..ba7486ddf4 100644 --- a/3rdparty/apriltag/common/svd22.cpp +++ b/3rdparty/apriltag/common/svd22.cpp @@ -26,11 +26,6 @@ either expressed or implied, of the Regents of The University of Michigan. */ #include -#include -#include -#include - -#include "common/doubles.h" /** SVD 2x2. diff --git a/3rdparty/apriltag/common/unionfind.h b/3rdparty/apriltag/common/unionfind.h index ea6ee196bd..6423512e8b 100644 --- a/3rdparty/apriltag/common/unionfind.h +++ b/3rdparty/apriltag/common/unionfind.h @@ -27,6 +27,7 @@ either expressed or implied, of the Regents of The University of Michigan. #pragma once +#include #include #include @@ -35,35 +36,28 @@ typedef struct unionfind unionfind_t; struct unionfind { uint32_t maxid; - struct ufrec *data; -}; -struct ufrec -{ - // the parent of this node. If a node's parent is its own index, - // then it is a root. - uint32_t parent; + // Parent node for each. Initialized to 0xffffffff + uint32_t *parent; - // for the root of a connected component, the number of components - // connected to it. For intermediate values, it's not meaningful. - uint32_t size; + // The size of the tree excluding the root + uint32_t *size; }; static inline unionfind_t *unionfind_create(uint32_t maxid) { unionfind_t *uf = (unionfind_t*) calloc(1, sizeof(unionfind_t)); uf->maxid = maxid; - uf->data = (struct ufrec*) malloc((maxid+1) * sizeof(struct ufrec)); - for (int i = 0; i <= maxid; i++) { - uf->data[i].size = 1; - uf->data[i].parent = i; - } + uf->parent = (uint32_t *) malloc((maxid+1) * sizeof(uint32_t) * 2); + memset(uf->parent, 0xff, (maxid+1) * sizeof(uint32_t)); + uf->size = uf->parent + (maxid+1); + memset(uf->size, 0, (maxid+1) * sizeof(uint32_t)); return uf; } static inline void unionfind_destroy(unionfind_t *uf) { - free(uf->data); + free(uf->parent); free(uf); } @@ -71,14 +65,14 @@ static inline void unionfind_destroy(unionfind_t *uf) static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) { // base case: a node is its own parent - if (uf->data[id].parent == id) + if (uf->parent[id] == id) return id; // otherwise, recurse - uint32_t root = unionfind_get_representative(uf, uf->data[id].parent); + uint32_t root = unionfind_get_representative(uf, uf->parent[id]); // short circuit the path. [XXX This write prevents tail recursion] - uf->data[id].parent = root; + uf->parent[id] = root; return root; } @@ -88,17 +82,22 @@ static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id // version above. static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) { - uint32_t root = id; + uint32_t root = uf->parent[id]; + // uninitialized node, so set to self + if (root == 0xffffffff) { + uf->parent[id] = id; + return id; + } // chase down the root - while (uf->data[root].parent != root) { - root = uf->data[root].parent; + while (uf->parent[root] != root) { + root = uf->parent[root]; } // go back and collapse the tree. - while (uf->data[id].parent != root) { - uint32_t tmp = uf->data[id].parent; - uf->data[id].parent = root; + while (uf->parent[id] != root) { + uint32_t tmp = uf->parent[id]; + uf->parent[id] = root; id = tmp; } @@ -108,7 +107,7 @@ static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id) { uint32_t repid = unionfind_get_representative(uf, id); - return uf->data[repid].size; + return uf->size[repid] + 1; } static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid) @@ -126,8 +125,8 @@ static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t // for rank. In my testing, it's often *faster* to use size than // rank, perhaps because the rank of the tree isn't that critical // if there are very few nodes in it. - uint32_t asize = uf->data[aroot].size; - uint32_t bsize = uf->data[broot].size; + uint32_t asize = uf->size[aroot] + 1; + uint32_t bsize = uf->size[broot] + 1; // optimization idea: We could shortcut some or all of the tree // that is grafted onto the other tree. Pro: those nodes were just @@ -135,12 +134,12 @@ static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t // wasted effort -- the tree might be grafted onto another tree in // a moment! if (asize > bsize) { - uf->data[broot].parent = aroot; - uf->data[aroot].size += bsize; + uf->parent[broot] = aroot; + uf->size[aroot] += bsize; return aroot; } else { - uf->data[aroot].parent = broot; - uf->data[broot].size += asize; + uf->parent[aroot] = broot; + uf->size[broot] += asize; return broot; } } diff --git a/3rdparty/apriltag/common/workerpool.cpp b/3rdparty/apriltag/common/workerpool.cpp index 9b8ca93207..e4a3db8119 100644 --- a/3rdparty/apriltag/common/workerpool.cpp +++ b/3rdparty/apriltag/common/workerpool.cpp @@ -29,6 +29,7 @@ either expressed or implied, of the Regents of The University of Michigan. #include #include #include +#include #include #include //#include @@ -53,6 +54,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? @@ -68,26 +70,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. @@ -107,6 +102,7 @@ workerpool_t *workerpool_create(int nthreads) workerpool_t *wp = (workerpool_t *)calloc(1, sizeof(workerpool_t)); wp->nthreads = nthreads; wp->tasks = zarray_create(sizeof(struct task)); + wp->start_predicate = false; if (nthreads > 1) { wp->threads = (pthread_t *)calloc(wp->nthreads, sizeof(pthread_t)); @@ -122,6 +118,13 @@ workerpool_t *workerpool_create(int nthreads) exit(-1); } } + + // 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; @@ -138,6 +141,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); @@ -165,7 +169,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) @@ -183,9 +193,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) { @@ -193,9 +203,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); @@ -206,7 +216,7 @@ void workerpool_run(workerpool_t *wp) int workerpool_get_nprocs() { -#ifdef WIN32 +#ifdef _WIN32 SYSTEM_INFO sysinfo; GetSystemInfo(&sysinfo); return sysinfo.dwNumberOfProcessors; diff --git a/3rdparty/apriltag/common/zarray.cpp b/3rdparty/apriltag/common/zarray.cpp index efb4449883..9528bf22f3 100644 --- a/3rdparty/apriltag/common/zarray.cpp +++ b/3rdparty/apriltag/common/zarray.cpp @@ -25,8 +25,6 @@ of the authors and should not be interpreted as representing official policies, either expressed or implied, of the Regents of The University of Michigan. */ -#include -#include #include #include diff --git a/3rdparty/apriltag/common/zarray.h b/3rdparty/apriltag/common/zarray.h index 8b89d68a3e..c31ff852f9 100644 --- a/3rdparty/apriltag/common/zarray.h +++ b/3rdparty/apriltag/common/zarray.h @@ -229,27 +229,6 @@ inline static void zarray_truncate(zarray_t *za, int sz) za->size = sz; } -/** - * Copies the memory array used internally by zarray to store its owned - * elements to the address pointed by 'buffer'. It is the caller's responsibility - * to allocate zarray_size()*el_sz bytes for the copy to be stored and - * to free the memory when no longer needed. The memory allocated at 'buffer' - * and the internal zarray storage must not overlap. 'buffer_bytes' should be - * the size of the 'buffer' memory space, in bytes, and must be at least - * zarray_size()*el_sz. - * - * Returns the number of bytes copied into 'buffer'. - */ -static inline size_t zarray_copy_data(const zarray_t *za, void *buffer, size_t buffer_bytes) -{ - assert(za != NULL); - assert(buffer != NULL); - assert(buffer_bytes >= za->el_sz * za->size); - memcpy(buffer, za->data, za->el_sz * za->size); - (void)buffer_bytes; // To avoid a warning on iOS - return za->el_sz * za->size; -} - /** * Removes the entry at index 'idx'. * If shuffle is true, the last element in the array will be placed in @@ -460,23 +439,26 @@ static inline int zarray_index_of(const zarray_t *za, const void *p) } /** - * Add all elements from 'source' into 'dest'. el_size must be the same - * for both lists + * Add elements from start up to and excluding end from 'source' into 'dest'. + * el_sz must be the same for both lists **/ -static inline void zarray_add_all(zarray_t *dest, const zarray_t *source) +static inline void zarray_add_range(zarray_t *dest, const zarray_t *source, int start, int end) { assert(dest->el_sz == source->el_sz); - - // Don't allocate on stack because el_sz could be larger than ~8 MB - // stack size - char *tmp = (char *)calloc(1, dest->el_sz); - - for (int i = 0; i < zarray_size(source); i++) { - zarray_get(source, i, tmp); - zarray_add(dest, tmp); + assert(dest != NULL); + assert(source != NULL); + assert(start >= 0); + assert(end <= source->size); + if (start == end) { + return; } + assert(start < end); + + int count = end - start; + zarray_ensure_capacity(dest, dest->size + count); - free(tmp); + memcpy(&dest->data[dest->size*dest->el_sz], &source->data[source->el_sz*start], dest->el_sz*count); + dest->size += count; } #ifdef __cplusplus diff --git a/3rdparty/apriltag/common/zhash.cpp b/3rdparty/apriltag/common/zhash.cpp index d15b8eeee2..16faac15cd 100644 --- a/3rdparty/apriltag/common/zhash.cpp +++ b/3rdparty/apriltag/common/zhash.cpp @@ -84,7 +84,6 @@ zhash_t *zhash_create_capacity(size_t keysz, size_t valuesz, zh->entrysz = static_cast(1 + zh->keysz + zh->valuesz); zh->entries = (char *)calloc(zh->nentries, zh->entrysz); - zh->nentries = nentries; return zh; } @@ -540,14 +539,15 @@ uint32_t zhash_str_hash(const void *_a) char *a = * (char**)_a; - int32_t hash = 0; + uint32_t hash = 0; while (*a != 0) { - hash = (hash << 7) + (hash >> 23); - hash += *a; + // optimization of hash x FNV_prime + hash += (hash << 1) + (hash << 4) + (hash << 7) + (hash << 8) + (hash << 24); + hash ^= *a; a++; } - return (uint32_t) hash; + return hash; } diff --git a/3rdparty/apriltag/common/zmaxheap.cpp b/3rdparty/apriltag/common/zmaxheap.cpp index 71475bca23..ec95246dcb 100644 --- a/3rdparty/apriltag/common/zmaxheap.cpp +++ b/3rdparty/apriltag/common/zmaxheap.cpp @@ -321,7 +321,7 @@ static void maxheapify(zmaxheap_t *heap, int parent) if (betterchild != parent) { heap->swap(heap, parent, betterchild); - return maxheapify(heap, betterchild); + maxheapify(heap, betterchild); } } @@ -362,7 +362,7 @@ void zmaxheap_test() { int cap = 10000; int sz = 0; - int32_t *vals = (int32_t *)calloc(sizeof(int32_t), cap); + int32_t *vals = (int32_t *)calloc(cap, sizeof(int32_t)); zmaxheap_t *heap = zmaxheap_create(sizeof(int32_t)); @@ -376,7 +376,6 @@ void zmaxheap_test() // 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,11 +394,12 @@ void zmaxheap_test() } - int32_t outv; - float outfv; + int32_t outv = 0; + float outfv = 0; int res = zmaxheap_remove_max(heap, &outv, &outfv); if (sz == 0) { assert(res == 0); + (void)res; } else { // printf("%d %d %d %f\n", sz, maxv, outv, outfv); assert(outv == outfv);