diff --git a/CMakeLists.txt b/CMakeLists.txt index c733c6c5..8d87bc6c 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/apriltag_pose.c b/apriltag_pose.c index da023aff..e66c2ba9 100644 --- a/apriltag_pose.c +++ b/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) { diff --git a/solve_square.c b/solve_square.c new file mode 100644 index 00000000..5791834b --- /dev/null +++ b/solve_square.c @@ -0,0 +1,353 @@ +#include +#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; +} diff --git a/solve_square.h b/solve_square.h new file mode 100644 index 00000000..e68b1d84 --- /dev/null +++ b/solve_square.h @@ -0,0 +1,19 @@ +#include "apriltag_pose.h" + +/* + * estimate tag pose using solve_square method. + * vector (x,y,z) is the location of the center of the tag + * vector (a,b,c) and (p,q,r) are vectors pointing from the center to the middle of two sides. + * + * abc + * tag: --------- + * | | | abc -> X + * | ----|pqr pqr -> Y + * | xyz | Z points inside + * --------- + * + * solve_square method tries to solve x y z a b c p q r from 4 corners + * and then generate a rotation matrix from (a,b,c) and (p,q,r) + */ + +double estimate_tag_pose_solve_square(apriltag_detection_info_t* info, apriltag_pose_t* pose);