fixes #334, fixes #345

1. Fix the inconsistent image coordinate conventions used in the code: the convention that (0,0) be the left top corner of the first pixel is adopted by this PR.

2. Fix refine_edges(): Use continuous coordinates and bilinear interpolation (instead of discrete coordinates) when retrieving grayscale values from the input image. Using discrete coordinates might incur extra errors thus unstable.

apriltag.c

@@ -789,10 +789,19 @@ static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, struct qu
             // search on another pixel in the first place. Likewise,
             // for very small tags, we don't want the range to be too
             // big.
-            double range = td->quad_decimate + 1;
+
+            int range = td->quad_decimate + 1;
+
+            // To reduce the overhead of bilinear interpolation, we can
+            // reduce the number of steps per unit.
+            int steps_per_unit = 4;
+            double step_length = 1.0 / steps_per_unit;
+            int max_steps = 2 * steps_per_unit * range + 1;
+            double delta = 0.5;
 
             // XXX tunable step size.
-            for (double n = -range; n <= range; n +=  0.25) {
+            for (int step = 0; step < max_steps; ++step) {
+                double n = -range + step_length * step;
                 // Because of the guaranteed winding order of the
                 // points in the quad, we will start inside the white
                 // portion of the quad and work our way outward.
@@ -802,19 +811,35 @@ static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, struct qu
                 // gradient more precisely, but are more sensitive to
                 // noise.
                 double grange = 1;
-                int x1 = x0 + (n + grange)*nx;
-                int y1 = y0 + (n + grange)*ny;
-                if (x1 < 0 || x1 >= im_orig->width || y1 < 0 || y1 >= im_orig->height)
-                    continue;
-
-                int x2 = x0 + (n - grange)*nx;
-                int y2 = y0 + (n - grange)*ny;
-                if (x2 < 0 || x2 >= im_orig->width || y2 < 0 || y2 >= im_orig->height)
+                double x1 = x0 + (n + grange)*nx - delta;
+                double y1 = y0 + (n + grange)*ny - delta;
+                int x1i = floor(x1);
+                int y1i = floor(y1);
+                double a1 = x1 - x1i;
+                double b1 = y1 - y1i;
+
+                if (x1i < 0 || x1i + 1 >= im_orig->width || y1i < 0 || y1i + 1 >= im_orig->height)
                     continue;
 
-                int g1 = im_orig->buf[y1*im_orig->stride + x1];
-                int g2 = im_orig->buf[y2*im_orig->stride + x2];
+                double x2 = x0 + (n - grange)*nx - delta;
+                double y2 = y0 + (n - grange)*ny - delta;
+                int x2i = floor(x2);
+                int y2i = floor(y2);
+                double a2 = x2 - x2i;
+                double b2 = y2 - y2i;
 
+                if (x2i < 0 || x2i + 1 >= im_orig->width || y2i < 0 || y2i + 1 >= im_orig->height)
+                    continue;
+
+                // interpolate
+                double g1 = (1 - a1) * (1 - b1) * im_orig->buf[y1i*im_orig->stride + x1i] +
+                                  a1 * (1 - b1) * im_orig->buf[y1i*im_orig->stride + x1i + 1] +
+                            (1 - a1) *    b1    * im_orig->buf[(y1i + 1)*im_orig->stride + x1i] +
+                                  a1 *    b1    * im_orig->buf[(y1i + 1)*im_orig->stride + x1i + 1];
+                double g2 = (1 - a2) * (1 - b2) * im_orig->buf[y2i*im_orig->stride + x2i] +
+                                  a2 * (1 - b2) * im_orig->buf[y2i*im_orig->stride + x2i + 1] +
+                            (1 - a2) *    b2    * im_orig->buf[(y2i + 1)*im_orig->stride + x2i] +
+                                  a2 *    b2    * im_orig->buf[(y2i + 1)*im_orig->stride + x2i + 1];
                 if (g1 < g2) // reject points whose gradient is "backwards". They can only hurt us.
                     continue;
 
@@ -1090,13 +1115,8 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
             zarray_get_volatile(quads, i, &q);
 
             for (int j = 0; j < 4; j++) {
-                if (td->quad_decimate == 1.5) {
-                    q->p[j][0] *= td->quad_decimate;
-                    q->p[j][1] *= td->quad_decimate;
-                } else {
-                    q->p[j][0] = (q->p[j][0] - 0.5)*td->quad_decimate + 0.5;
-                    q->p[j][1] = (q->p[j][1] - 0.5)*td->quad_decimate + 0.5;
-                }
+                q->p[j][0] *= td->quad_decimate;
+                q->p[j][1] *= td->quad_decimate;
             }
         }
     }