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 <[email protected]>

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