From d9ee45f178e52799eabbdeb888cfd5705948f7c9 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 | 72 ++-- 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 | 42 +- 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, 526 insertions(+), 447 deletions(-) diff --git a/3rdparty/apriltag/apriltag.cpp b/3rdparty/apriltag/apriltag.cpp index 633824d961..5f6229dec5 100644 --- a/3rdparty/apriltag/apriltag.cpp +++ b/3rdparty/apriltag/apriltag.cpp @@ -40,20 +40,17 @@ either expressed or implied, of the Regents of The University of Michigan. #include #include #include -#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" @@ -228,13 +225,16 @@ static void quick_decode_init(apriltag_family_t *family, int maxhamming) qd->entries = (quick_decode_entry *)calloc(qd->nentries, sizeof(struct quick_decode_entry)); if (qd->entries == NULL) { printf("apriltag.c: failed to allocate hamming decode table. Reduce max hamming size.\n"); - exit(-1); + // errno already set to ENOMEM (Error No MEMory) by calloc() failure + return; } 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) @@ -263,6 +263,9 @@ static void quick_decode_init(apriltag_family_t *family, int maxhamming) if (maxhamming > 3) { printf("apriltag.c: maxhamming beyond 3 not supported\n"); + // set errno to Error INvalid VALue + errno = EINVAL; + return; } } @@ -299,7 +302,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 +370,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 +379,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 +451,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 +512,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 +632,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 +693,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 +726,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 +885,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 +914,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 +1018,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); @@ -1333,8 +1350,7 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig) uint8_t data_[] = { (uint8_t)rgb[0], (uint8_t)rgb[1], (uint8_t)rgb[2] }; image_u8x3_draw_line(out, det->p[j][0], det->p[j][1], det->p[k][0], det->p[k][1], - data_, - 1); + data_); } } @@ -1455,10 +1471,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 +1491,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..3134ec0c33 100644 --- a/3rdparty/apriltag/common/workerpool.cpp +++ b/3rdparty/apriltag/common/workerpool.cpp @@ -24,11 +24,13 @@ The views and conclusions contained in the software and documentation are those of the authors and should not be interpreted as representing official policies, either expressed or implied, of the Regents of The University of Michigan. */ +#include #define __USE_GNU #include #include #include +#include #include #include //#include @@ -53,6 +55,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 +71,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 +103,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)); @@ -119,9 +116,17 @@ workerpool_t *workerpool_create(int nthreads) int res = pthread_create(&wp->threads[i], NULL, worker_thread, wp); if (res != 0) { perror("pthread_create"); - exit(-1); + // errno already set to EAGAIN by pthread_create() failure + 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; @@ -138,6 +143,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 +171,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 +195,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 +205,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 +218,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);