From c598bd8d3d7c612eeb2b610e05ec7cdd7f4aa70f Mon Sep 17 00:00:00 2001 From: Austin Schuh Date: Fri, 29 Dec 2023 18:20:18 -0800 Subject: [PATCH 1/2] Stop adding duplicate points to the clusters. The segmentation system was adding duplicate points to the clusters list before removing them later. This has 2 problems. 1) It is more expensive to remove them later than not to add them. 2) When sorting, multiple slopes can have the same calculated slope value. Depending on the initial order, the duplicates can end up being in non-adjacent slots in the cluster, and won't get de-duplicated later. This takes my sample image on my test box from: 0 init 0.000000 ms 0.000000 ms 1 decimate 0.292000 ms 0.292000 ms 2 blur/sharp 0.000000 ms 0.292000 ms 3 threshold 0.617000 ms 0.909000 ms 4 unionfind 3.746000 ms 4.655000 ms 5 make clusters 9.461000 ms 14.116000 ms 6 fit quads to clusters 16.310000 ms 30.426000 ms 7 quads 0.087000 ms 30.513000 ms 8 decode+refinement 0.869000 ms 31.382000 ms 9 reconcile 0.001000 ms 31.383000 ms 10 debug output 0.001000 ms 31.384000 ms 11 cleanup 0.003000 ms 31.387000 ms to 0 init 0.000000 ms 0.000000 ms 1 decimate 0.303000 ms 0.303000 ms 2 blur/sharp 0.001000 ms 0.304000 ms 3 threshold 0.640000 ms 0.944000 ms 4 unionfind 3.645000 ms 4.589000 ms 5 make clusters 8.593000 ms 13.182000 ms 6 fit quads to clusters 13.935000 ms 27.117000 ms 7 quads 0.084000 ms 27.201000 ms 8 decode+refinement 0.827000 ms 28.028000 ms 9 reconcile 0.002000 ms 28.030000 ms 10 debug output 0.000000 ms 28.030000 ms 11 cleanup 0.004000 ms 28.034000 ms (across 10 runs, and picking one of the faster 10 runs for the original and the slowest for the new) Signed-off-by: Austin Schuh --- apriltag_quad_thresh.c | 53 +++++++++++++----------------------------- 1 file changed, 16 insertions(+), 37 deletions(-) diff --git a/apriltag_quad_thresh.c b/apriltag_quad_thresh.c index 589a8b44..3c0a66e0 100644 --- a/apriltag_quad_thresh.c +++ b/apriltag_quad_thresh.c @@ -846,43 +846,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]; @@ -1582,15 +1547,19 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int mem_pools[mem_pool_idx] = calloc(mem_chunk_size, sizeof(struct uint64_zarray_entry)); for (int y = y0; y < y1; y++) { + bool connected_last = false; for (int x = 1; x < w-1; x++) { uint8_t v0 = threshim->buf[y*ts + x]; - if (v0 == 127) + if (v0 == 127) { + connected_last = false; continue; + } // XXX don't query this until we know we need it? uint64_t rep0 = unionfind_get_representative(uf, y*w + x); if (unionfind_get_set_size(uf, rep0) < 25) { + connected_last = false; continue; } @@ -1614,6 +1583,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]; \ @@ -1651,6 +1621,7 @@ zarray_t* do_gradient_clusters(image_u8_t* threshim, int ts, int y0, int y1, int \ struct pt p = { .x = 2*x + dx, .y = 2*y + dy, .gx = dx*((int) v1-v0), .gy = dy*((int) v1-v0)}; \ zarray_add(entry->cluster, &p); \ + connected = true; \ } \ } \ } @@ -1660,8 +1631,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 From 37eed825c06c94a0934bd00a809741e36fb8e771 Mon Sep 17 00:00:00 2001 From: Austin Schuh Date: Mon, 1 Jan 2024 16:26:39 -0800 Subject: [PATCH 2/2] Fix perimeter threshold April tags will only have straight sides. With the deduplication happening earlier, each pixel along that boundary will now only have 2 neighbors (one above, one at 45 degrees). We should adjust the max blob length threshold accordingly. Signed-off-by: Austin Schuh --- apriltag_quad_thresh.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apriltag_quad_thresh.c b/apriltag_quad_thresh.c index 3c0a66e0..38d3e43d 100644 --- a/apriltag_quad_thresh.c +++ b/apriltag_quad_thresh.c @@ -1061,10 +1061,10 @@ static void do_quad_task(void *p) // a cluster should contain only boundary points around the // tag. it cannot be bigger than the whole screen. (Reject // large connected blobs that will be prohibitively slow to - // fit quads to.) A typical point along an edge is added three - // 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; }