propose another method for estimating tag position.

CMakeLists.txt

@@ -5,7 +5,7 @@ find_package(OpenCV QUIET)
 
 include_directories(.)
 aux_source_directory(common COMMON_SRC)
-set(APRILTAG_SRCS apriltag.c apriltag_pose.c apriltag_quad_thresh.c)
+set(APRILTAG_SRCS apriltag.c apriltag_pose.c apriltag_quad_thresh.c solve_square.c)
 
 set(CMAKE_BUILD_TYPE Release)
 

apriltag_pose.c

@@ -6,6 +6,7 @@
 #include "common/homography.h"
 #include "common/image_u8x3.h"
 
+#include "solve_square.h"
 
 /**
  * Calculate projection operator from image points.
@@ -524,7 +525,9 @@ double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose)
     double err1, err2;
     apriltag_pose_t pose1, pose2;
     estimate_tag_pose_orthogonal_iteration(info, &err1, &pose1, &err2, &pose2, 50);
-    if (err1 <= err2) {
+    if (!pose2.R) {
+        return estimate_tag_pose_solve_square(info, pose);
+    } else if (err1 <= err2) {
         pose->R = pose1.R;
         pose->t = pose1.t;
         if (pose2.R) {

solve_square.c

@@ -0,0 +1,353 @@
+#include <math.h>
+#include "apriltag_pose.h"
+
+double solve_double_quadratic(
+    double a1, double b1, double c1, double d1, double e1, double f1,
+    double a2, double b2, double c2, double d2, double e2, double f2,
+    double x0, double y0,
+    double* pansx, double* pansy)
+{
+    double x = x0;
+    double y = y0;
+    double lastx = x;
+    double lasty = y;
+    double F1 = a1*x*x + b1*y*y + c1*x*y + d1*x + e1*y + f1;
+    double F2 = a2*x*x + b2*y*y + c2*x*y + d2*x + e2*y + f2;
+
+    double F1x, F1y, F2x, F2y, Fx, Fy, L, dx, dy, dF, rate;
+
+    double threshold = 0.0000000001;
+    int count = 0;
+
+    while (! ((F1 >= -threshold) && (F1 <= threshold) &&
+              (F2 >= -threshold) && (F2 <= threshold)    )
+          )
+    {
+        // find the next (x,y) based on gradient of F1 and F2
+        double stepx = 0;
+        double stepy = 0;
+        // F1
+        Fx = 2*a1*x+c1*y+d1;
+        Fy = 2*b1*y+c1*x+e1;
+        L = 1/sqrt(Fx*Fx + Fy*Fy);
+        dx = Fx * L;
+        dy = Fy * L;
+        dF = Fx * dx + Fy * dy;
+        rate = (0-F1) / dF;
+        stepx += rate * dx;
+        stepy += rate * dy;
+        // F2
+        Fx = 2*a2*x+c2*y+d2;
+        Fy = 2*b2*y+c2*x+e2;
+        L = 1/sqrt(Fx*Fx + Fy*Fy);
+        dx = Fx * L;
+        dy = Fy * L;
+        dF = Fx * dx + Fy * dy;
+        rate = (0-F2) / dF;
+        stepx += rate * dx;
+        stepy += rate * dy;
+        // check and add to (x,y)
+        if ((x + stepx == lastx) && (y + stepy == lasty)) break;
+        lastx = x;
+        lasty = y;
+        x += stepx;
+        y += stepy;
+        F1 = a1*x*x + b1*y*y + c1*x*y + d1*x + e1*y + f1;
+        F2 = a2*x*x + b2*y*y + c2*x*y + d2*x + e2*y + f2;
+
+        count++;
+        if (count > 1000) break;
+    }
+
+
+    if ((F1 >= -threshold) && (F1 <= threshold) &&
+        (F2 >= -threshold) && (F2 <= threshold)    )
+    {
+        *pansx = x; *pansy = y; return F1*F1 + F2*F2;
+    }
+
+    // if not finish, then it means (x,y) goes into an oscillation, use F=F1^2 + F2^2 to approach
+    threshold *= threshold;
+    double F = F1*F1 + F2*F2;
+
+    while (F > threshold)
+    {
+        // find the next (x,y) based on gradient of F = F1^2 + F^2
+        F1x = 2*a1*x+c1*y+d1;
+        F1y = 2*b1*y+c1*x+e1;
+
+        F2x = 2*a2*x+c2*y+d2;
+        F2y = 2*b2*y+c2*x+e2;
+
+        Fx = 2*F1 * F1x + 2*F2 * F2x;
+        Fy = 2*F1 * F1y + 2*F2 * F2y;
+
+        L = 1/sqrt(Fx*Fx + Fy*Fy);
+        dx = Fx * L;
+        dy = Fy * L;
+        dF = Fx * dx + Fy * dy;
+        rate = (0-F) / dF;
+        x += rate * dx;
+        y += rate * dy;
+
+        F1 = a1*x*x + b1*y*y + c1*x*y + d1*x + e1*y + f1;
+        F2 = a2*x*x + b2*y*y + c2*x*y + d2*x + e2*y + f2;
+        F = F1*F1 + F2*F2;
+
+        count++;
+        if (count > 2000) break;
+    }
+
+    *pansx = x; *pansy = y;
+    return F;
+}
+
+double solve_square(
+    apriltag_detection_info_t* info,
+    matd_t *xyz, matd_t *abc, matd_t *pqr)
+{
+    double u1, v1, u2, v2, u3, v3, u4, v4;
+    double ku, kv, u0, v0;
+    double L;
+
+    u1 = info->det->p[0][0]; v1 = info->det->p[0][1];
+    u2 = info->det->p[1][0]; v2 = info->det->p[1][1];
+    u3 = info->det->p[2][0]; v3 = info->det->p[2][1];
+    u4 = info->det->p[3][0]; v4 = info->det->p[3][1];
+    ku = info->fx; kv = info->fy; u0 = info->cx; v0 = info->cy;
+    L = info->tagsize;
+
+    /*       image  u         camera / z
+     *          ---------           /
+     *        v |       |          ------------- x
+     *          |       |          |
+     *          ---------          | y
+     *       tag
+     *             abc
+     *          ---------
+     *          |   |   |       abc -> X
+     *          |   ----|pqr    pqr -> Y
+     *          |  xyz  |       Z points inside
+     *          ---------
+     */
+
+    // solve linar equation
+    double c0 = -(u1*v2 - u2*v1 - u1*v3 + u3*v1 + u2*v4 - u4*v2 - u3*v4 + u4*v3)/
+            (u1*v2 - u2*v1 - u1*v4 + u2*v3 - u3*v2 + u4*v1 + u3*v4 - u4*v3);
+    double r0 = -(  u1*v3 - u3*v1 - u1*v4 - u2*v3 + u3*v2 + u4*v1 + u2*v4 - u4*v2)/
+            (   u1*v2 - u2*v1 - u1*v4 + u2*v3 - u3*v2 + u4*v1 + u3*v4 - u4*v3);
+        // got a c/z and r/z proximately, for quadric solving later as a starting point
+
+    // a b p q x y c r z
+    // 6 rows left
+    double Ks[6] = {-0.5/ku,-0.5/kv,0.5/ku,0.5/kv,-0.5/ku,-0.5/kv};
+    double Cs[6] = { u3+u2-u0-u0,   v3+v2-v0-v0,
+                     u2-u1,         v2-v1,
+                    -u3+u1,        -v3+v1
+                  };
+    double Rs[6] = { u3-u2,         v3-v2,
+                    -u2-u1+u0+u0,  -v2-v1+v0+v0,
+                    -u3+u1,        -v3+v1
+                  };
+    double Zs[6] = {-u3+u2,        -v3+v2,
+                    u2-u1,         v2-v1,
+                    u3+u1-u0-u0,   v3+v1-v0-v0,
+                  };
+    double ac,ar,az,bc,br,bz,xc,xr,xz;
+    double pc,pr,pz,qc,qr,qz,yc,yr,yz;
+
+    ac = -Cs[0]*Ks[0];  ar = -Rs[0]*Ks[0];  az = -Zs[0]*Ks[0];
+    bc = -Cs[1]*Ks[1];  br = -Rs[1]*Ks[1];  bz = -Zs[1]*Ks[1];
+    pc = -Cs[2]*Ks[2];  pr = -Rs[2]*Ks[2];  pz = -Zs[2]*Ks[2];
+    qc = -Cs[3]*Ks[3];  qr = -Rs[3]*Ks[3];  qz = -Zs[3]*Ks[3];
+    xc = -Cs[4]*Ks[4];  xr = -Rs[4]*Ks[4];  xz = -Zs[4]*Ks[4];
+    yc = -Cs[5]*Ks[5];  yr = -Rs[5]*Ks[5];  yz = -Zs[5]*Ks[5];
+
+    /* now we have the rate of x,y,a,b,p,q to c r z
+     * solve double quadric equation next
+     * we have two constrains:
+     *      ap + bq + cr == 0 and
+     *      a^2 + b^2 + c^2 == p^2 + q^2 + r^2 == hL^2
+     * expressed by c r z, we have two equations like:
+     *      Ac^2 + Br^2 + Ccr + Dcz + Erz + Fz^2 == 0
+     * have c = cz * z,  r = rz * z, and eliminate z
+     * we have two:
+     *      A1c^2 + B1r^2 + C1cr + D1c + E1r + F1 == 0
+     *      A2c^2 + B2r^2 + C2cr + D2c + E2r + F2 == 0
+     */
+    double a1, b1, c1, d1, e1, f1;
+    double a2, b2, c2, d2, e2, f2;
+    //c^2 ap            bq              cr
+    a1 =  ac*pc         +bc*qc             ;
+    //r^2
+    b1 =  ar*pr         +br*qr             ;
+    //cr
+    c1 =  ac*pr+ar*pc   +bc*qr+br*qc    +1 ;
+    //cz
+    d1 =  ac*pz+az*pc   +bc*qz+bz*qc       ;
+    //rz
+    e1 =  ar*pz+az*pr   +br*qz+bz*qr       ;
+    //z^2
+    f1 =  az*pz         +bz*qz             ;
+
+    double a3, b3, c3, d3, e3, f3;
+    double a4, b4, c4, d4, e4, f4;
+    //c^2 aa            bb              cc
+    a3 = ac*ac          +bc*bc          +1 ;
+    //r^2
+    b3 = ar*ar          +br*br             ;
+    //cr
+    c3 = ac*ar+ar*ac    +bc*br+br*bc       ;
+    //cz
+    d3 = ac*az+az*ac    +bc*bz+bz*bc       ;
+    //rz
+    e3 = ar*az+az*ar    +br*bz+bz*br       ;
+    //z^2
+    f3 = az*az          +bz*bz             ;
+
+    //c^2 pp            qq              rr
+    a4 = pc*pc          +qc*qc             ;
+    //r^2
+    b4 = pr*pr          +qr*qr          +1 ;
+    //cr
+    c4 = pc*pr+pr*pc    +qc*qr+qr*qc       ;
+    //cz
+    d4 = pc*pz+pz*pc    +qc*qz+qz*qc       ;
+    //rz
+    e4 = pr*pz+pz*pr    +qr*qz+qz*qr       ;
+    //z^2
+    f4 = pz*pz          +qz*qz             ;
+
+    a2 = a3-a4;
+    b2 = b3-b4;
+    c2 = c3-c4;
+    d2 = d3-d4;
+    e2 = e3-e4;
+    f2 = f3-f4;
+
+    double cz, rz;
+
+    double ret = solve_double_quadratic(
+        a1, b1, c1, d1, e1, f1,
+        a2, b2, c2, d2, e2, f2,
+        c0, r0,
+        &cz, &rz);
+
+    double a5,b5,c5,d5,e5,f5;
+    //c^2
+    a5 = a3 * cz * cz;
+    //r^2
+    b5 = b3 * rz * rz;
+    //cr
+    c5 = c3 * cz * rz;
+    //cz
+    d5 = d3 * cz;
+    //rz
+    e5 = e3 * rz;
+    //z^2
+    f5 = f3;
+
+    double x,y,z,a,b,c,p,q,r;
+    double hL = L / 2;
+    z = sqrt(hL * hL / (a5+b5+c5+d5+e5+f5));
+    c = cz * z;
+    r = rz * z;
+    x = xc * c + xr * r + xz * z;
+    y = yc * c + yr * r + yz * z;
+    a = ac * c + ar * r + az * z;
+    b = bc * c + br * r + bz * z;
+    p = pc * c + pr * r + pz * z;
+    q = qc * c + qr * r + qz * z;
+
+    // got x y z a b c p q r
+    matd_put(xyz, 0, 0, x);
+    matd_put(xyz, 1, 0, y);
+    matd_put(xyz, 2, 0, z);
+    matd_put(abc, 0, 0, a);
+    matd_put(abc, 1, 0, b);
+    matd_put(abc, 2, 0, c);
+    matd_put(pqr, 0, 0, p);
+    matd_put(pqr, 1, 0, q);
+    matd_put(pqr, 2, 0, r);
+
+    return ret;
+}
+
+matd_t* create_rotation_matrix(double th, matd_t *axis)
+{
+    double x = matd_get(axis, 0, 0);
+    double y = matd_get(axis, 1, 0);
+    double z = matd_get(axis, 2, 0);
+
+    matd_t *r = matd_create(3, 3);
+
+    matd_put(r, 0, 0, cos(th) + (1 - cos(th))*x*x);
+    matd_put(r, 0, 1, (1 - cos(th))*x*y - sin(th)*z);
+    matd_put(r, 0, 2, (1 - cos(th))*x*z + sin(th)*y);
+
+    matd_put(r, 1, 0, (1 - cos(th))*y*x + sin(th)*z);
+    matd_put(r, 1, 1, cos(th) + (1 - cos(th))*y*y);
+    matd_put(r, 1, 2, (1 - cos(th))*y*z - sin(th)*x);
+
+    matd_put(r, 2, 0, (1 - cos(th))*z*x - sin(th)*y);
+    matd_put(r, 2, 1, (1 - cos(th))*z*y + sin(th)*x);
+    matd_put(r, 2, 2, cos(th) + (1 - cos(th))*z*z);
+
+    return r;
+}
+
+matd_t* create_square_rotation(matd_t *abc, matd_t *pqr)
+{
+    matd_t *X = matd_create(3, 1);
+    matd_put(X, 0, 0, 1);
+    matd_put(X, 1, 0, 0);
+    matd_put(X, 2, 0, 0);
+    matd_t *Y = matd_create(3, 1);
+    matd_put(Y, 0, 0, 0);
+    matd_put(Y, 1, 0, 1);
+    matd_put(Y, 2, 0, 0);
+    matd_scale_inplace(abc, 1/matd_vec_mag(abc));
+    matd_scale_inplace(pqr, 1/matd_vec_mag(pqr));
+
+    double th1 = acos(matd_vec_dot_product(X, abc));
+    matd_t* axis1 = matd_crossproduct(X, abc);
+    matd_t* R1;
+    if (matd_vec_mag(axis1) == 0)
+        R1 = create_rotation_matrix(0, X);
+    else {
+        matd_scale_inplace(axis1, 1/matd_vec_mag(axis1));
+        R1 = create_rotation_matrix(th1, axis1);
+    }
+
+    matd_t* Y_rotated = matd_multiply(R1, Y);
+
+    double th2 = acos(matd_vec_dot_product(Y_rotated, pqr));
+    matd_t* axis2 = matd_crossproduct(Y_rotated, pqr);
+    matd_scale_inplace(axis2, 1/matd_vec_mag(axis2));
+    if (matd_vec_dist(axis2, abc) > 1) matd_put(X, 0, 0, -1);
+    matd_t *R2 = create_rotation_matrix(th2, X);
+
+    matd_t *rotation = matd_multiply(R1, R2);
+
+    matd_destroy(X);
+    matd_destroy(Y);
+    matd_destroy(axis1);
+    matd_destroy(R1);
+    matd_destroy(Y_rotated);
+    matd_destroy(axis2);
+    matd_destroy(R2);
+
+    return rotation;
+}
+
+double estimate_tag_pose_solve_square(apriltag_detection_info_t* info, apriltag_pose_t* pose)
+{
+    pose->t = matd_create(3, 1);
+    matd_t *abc = matd_create(3, 1);
+    matd_t *pqr = matd_create(3, 1);
+    double err = solve_square(info, pose->t, abc, pqr);
+    pose->R = create_square_rotation(abc, pqr);
+    matd_destroy(abc);
+    matd_destroy(pqr);
+    return err;
+}