Squashed 'third_party/apriltag/' content from commit 3e8e974d0
git-subtree-dir: third_party/apriltag
git-subtree-split: 3e8e974d0d8d6ab318abf56d87506d15d7f2cc35
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I04ba3cb2106b6813a1013d57aa8074c26f856598
diff --git a/common/homography.c b/common/homography.c
new file mode 100644
index 0000000..48e7f02
--- /dev/null
+++ b/common/homography.c
@@ -0,0 +1,479 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson@umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <math.h>
+
+#include "common/matd.h"
+#include "common/zarray.h"
+#include "common/homography.h"
+#include "common/math_util.h"
+
+// correspondences is a list of float[4]s, consisting of the points x
+// and y concatenated. We will compute a homography such that y = Hx
+matd_t *homography_compute(zarray_t *correspondences, int flags)
+{
+ // compute centroids of both sets of points (yields a better
+ // conditioned information matrix)
+ double x_cx = 0, x_cy = 0;
+ double y_cx = 0, y_cy = 0;
+
+ for (int i = 0; i < zarray_size(correspondences); i++) {
+ float *c;
+ zarray_get_volatile(correspondences, i, &c);
+
+ x_cx += c[0];
+ x_cy += c[1];
+ y_cx += c[2];
+ y_cy += c[3];
+ }
+
+ int sz = zarray_size(correspondences);
+ x_cx /= sz;
+ x_cy /= sz;
+ y_cx /= sz;
+ y_cy /= sz;
+
+ // NB We don't normalize scale; it seems implausible that it could
+ // possibly make any difference given the dynamic range of IEEE
+ // doubles.
+
+ matd_t *A = matd_create(9,9);
+ for (int i = 0; i < zarray_size(correspondences); i++) {
+ float *c;
+ zarray_get_volatile(correspondences, i, &c);
+
+ // (below world is "x", and image is "y")
+ double worldx = c[0] - x_cx;
+ double worldy = c[1] - x_cy;
+ double imagex = c[2] - y_cx;
+ double imagey = c[3] - y_cy;
+
+ double a03 = -worldx;
+ double a04 = -worldy;
+ double a05 = -1;
+ double a06 = worldx*imagey;
+ double a07 = worldy*imagey;
+ double a08 = imagey;
+
+ MATD_EL(A, 3, 3) += a03*a03;
+ MATD_EL(A, 3, 4) += a03*a04;
+ MATD_EL(A, 3, 5) += a03*a05;
+ MATD_EL(A, 3, 6) += a03*a06;
+ MATD_EL(A, 3, 7) += a03*a07;
+ MATD_EL(A, 3, 8) += a03*a08;
+ MATD_EL(A, 4, 4) += a04*a04;
+ MATD_EL(A, 4, 5) += a04*a05;
+ MATD_EL(A, 4, 6) += a04*a06;
+ MATD_EL(A, 4, 7) += a04*a07;
+ MATD_EL(A, 4, 8) += a04*a08;
+ MATD_EL(A, 5, 5) += a05*a05;
+ MATD_EL(A, 5, 6) += a05*a06;
+ MATD_EL(A, 5, 7) += a05*a07;
+ MATD_EL(A, 5, 8) += a05*a08;
+ MATD_EL(A, 6, 6) += a06*a06;
+ MATD_EL(A, 6, 7) += a06*a07;
+ MATD_EL(A, 6, 8) += a06*a08;
+ MATD_EL(A, 7, 7) += a07*a07;
+ MATD_EL(A, 7, 8) += a07*a08;
+ MATD_EL(A, 8, 8) += a08*a08;
+
+ double a10 = worldx;
+ double a11 = worldy;
+ double a12 = 1;
+ double a16 = -worldx*imagex;
+ double a17 = -worldy*imagex;
+ double a18 = -imagex;
+
+ MATD_EL(A, 0, 0) += a10*a10;
+ MATD_EL(A, 0, 1) += a10*a11;
+ MATD_EL(A, 0, 2) += a10*a12;
+ MATD_EL(A, 0, 6) += a10*a16;
+ MATD_EL(A, 0, 7) += a10*a17;
+ MATD_EL(A, 0, 8) += a10*a18;
+ MATD_EL(A, 1, 1) += a11*a11;
+ MATD_EL(A, 1, 2) += a11*a12;
+ MATD_EL(A, 1, 6) += a11*a16;
+ MATD_EL(A, 1, 7) += a11*a17;
+ MATD_EL(A, 1, 8) += a11*a18;
+ MATD_EL(A, 2, 2) += a12*a12;
+ MATD_EL(A, 2, 6) += a12*a16;
+ MATD_EL(A, 2, 7) += a12*a17;
+ MATD_EL(A, 2, 8) += a12*a18;
+ MATD_EL(A, 6, 6) += a16*a16;
+ MATD_EL(A, 6, 7) += a16*a17;
+ MATD_EL(A, 6, 8) += a16*a18;
+ MATD_EL(A, 7, 7) += a17*a17;
+ MATD_EL(A, 7, 8) += a17*a18;
+ MATD_EL(A, 8, 8) += a18*a18;
+
+ double a20 = -worldx*imagey;
+ double a21 = -worldy*imagey;
+ double a22 = -imagey;
+ double a23 = worldx*imagex;
+ double a24 = worldy*imagex;
+ double a25 = imagex;
+
+ MATD_EL(A, 0, 0) += a20*a20;
+ MATD_EL(A, 0, 1) += a20*a21;
+ MATD_EL(A, 0, 2) += a20*a22;
+ MATD_EL(A, 0, 3) += a20*a23;
+ MATD_EL(A, 0, 4) += a20*a24;
+ MATD_EL(A, 0, 5) += a20*a25;
+ MATD_EL(A, 1, 1) += a21*a21;
+ MATD_EL(A, 1, 2) += a21*a22;
+ MATD_EL(A, 1, 3) += a21*a23;
+ MATD_EL(A, 1, 4) += a21*a24;
+ MATD_EL(A, 1, 5) += a21*a25;
+ MATD_EL(A, 2, 2) += a22*a22;
+ MATD_EL(A, 2, 3) += a22*a23;
+ MATD_EL(A, 2, 4) += a22*a24;
+ MATD_EL(A, 2, 5) += a22*a25;
+ MATD_EL(A, 3, 3) += a23*a23;
+ MATD_EL(A, 3, 4) += a23*a24;
+ MATD_EL(A, 3, 5) += a23*a25;
+ MATD_EL(A, 4, 4) += a24*a24;
+ MATD_EL(A, 4, 5) += a24*a25;
+ MATD_EL(A, 5, 5) += a25*a25;
+ }
+
+ // make symmetric
+ for (int i = 0; i < 9; i++)
+ for (int j = i+1; j < 9; j++)
+ MATD_EL(A, j, i) = MATD_EL(A, i, j);
+
+ matd_t *H = matd_create(3,3);
+
+ if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) {
+ // compute singular vector by (carefully) inverting the rank-deficient matrix.
+
+ if (1) {
+ matd_t *Ainv = matd_inverse(A);
+ double scale = 0;
+
+ for (int i = 0; i < 9; i++)
+ scale += sq(MATD_EL(Ainv, i, 0));
+ scale = sqrt(scale);
+
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
+
+ matd_destroy(Ainv);
+ } else {
+
+ matd_t *b = matd_create_data(9, 1, (double[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 });
+ matd_t *Ainv = NULL;
+
+ if (0) {
+ matd_plu_t *lu = matd_plu(A);
+ Ainv = matd_plu_solve(lu, b);
+ matd_plu_destroy(lu);
+ } else {
+ matd_chol_t *chol = matd_chol(A);
+ Ainv = matd_chol_solve(chol, b);
+ matd_chol_destroy(chol);
+ }
+
+ double scale = 0;
+
+ for (int i = 0; i < 9; i++)
+ scale += sq(MATD_EL(Ainv, i, 0));
+ scale = sqrt(scale);
+
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
+
+ matd_destroy(b);
+ matd_destroy(Ainv);
+ }
+
+ } else {
+ // compute singular vector using SVD. A bit slower, but more accurate.
+ matd_svd_t svd = matd_svd_flags(A, MATD_SVD_NO_WARNINGS);
+
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8);
+
+ matd_destroy(svd.U);
+ matd_destroy(svd.S);
+ matd_destroy(svd.V);
+
+ }
+
+ matd_t *Tx = matd_identity(3);
+ MATD_EL(Tx,0,2) = -x_cx;
+ MATD_EL(Tx,1,2) = -x_cy;
+
+ matd_t *Ty = matd_identity(3);
+ MATD_EL(Ty,0,2) = y_cx;
+ MATD_EL(Ty,1,2) = y_cy;
+
+ matd_t *H2 = matd_op("M*M*M", Ty, H, Tx);
+
+ matd_destroy(A);
+ matd_destroy(Tx);
+ matd_destroy(Ty);
+ matd_destroy(H);
+
+ return H2;
+}
+
+
+// assuming that the projection matrix is:
+// [ fx 0 cx 0 ]
+// [ 0 fy cy 0 ]
+// [ 0 0 1 0 ]
+//
+// And that the homography is equal to the projection matrix times the
+// model matrix, recover the model matrix (which is returned). Note
+// that the third column of the model matrix is missing in the
+// expresison below, reflecting the fact that the homography assumes
+// all points are at z=0 (i.e., planar) and that the element of z is
+// thus omitted. (3x1 instead of 4x1).
+//
+// [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ]
+// [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ]
+// [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ]
+// [ 0 0 1 ]
+//
+// fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.)
+// fx*R01 + cx*R21 = H01
+// fx*TX + cx*TZ = H02
+// fy*R10 + cy*R20 = H10
+// fy*R11 + cy*R21 = H11
+// fy*TY + cy*TZ = H12
+// R20 = H20
+// R21 = H21
+// TZ = H22
+
+matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy)
+{
+ // Note that every variable that we compute is proportional to the scale factor of H.
+ double R20 = MATD_EL(H, 2, 0);
+ double R21 = MATD_EL(H, 2, 1);
+ double TZ = MATD_EL(H, 2, 2);
+ double R00 = (MATD_EL(H, 0, 0) - cx*R20) / fx;
+ double R01 = (MATD_EL(H, 0, 1) - cx*R21) / fx;
+ double TX = (MATD_EL(H, 0, 2) - cx*TZ) / fx;
+ double R10 = (MATD_EL(H, 1, 0) - cy*R20) / fy;
+ double R11 = (MATD_EL(H, 1, 1) - cy*R21) / fy;
+ double TY = (MATD_EL(H, 1, 2) - cy*TZ) / fy;
+
+ // compute the scale by requiring that the rotation columns are unit length
+ // (Use geometric average of the two length vectors we have)
+ double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
+ double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
+ double s = 1.0 / sqrtf(length1 * length2);
+
+ // get sign of S by requiring the tag to be in front the camera;
+ // we assume camera looks in the -Z direction.
+ if (TZ > 0)
+ s *= -1;
+
+ R20 *= s;
+ R21 *= s;
+ TZ *= s;
+ R00 *= s;
+ R01 *= s;
+ TX *= s;
+ R10 *= s;
+ R11 *= s;
+ TY *= s;
+
+ // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns.
+ double R02 = R10*R21 - R20*R11;
+ double R12 = R20*R01 - R00*R21;
+ double R22 = R00*R11 - R10*R01;
+
+ // Improve rotation matrix by applying polar decomposition.
+ if (1) {
+ // do polar decomposition. This makes the rotation matrix
+ // "proper", but probably increases the reprojection error. An
+ // iterative alignment step would be superior.
+
+ matd_t *R = matd_create_data(3, 3, (double[]) { R00, R01, R02,
+ R10, R11, R12,
+ R20, R21, R22 });
+
+ matd_svd_t svd = matd_svd(R);
+ matd_destroy(R);
+
+ R = matd_op("M*M'", svd.U, svd.V);
+
+ matd_destroy(svd.U);
+ matd_destroy(svd.S);
+ matd_destroy(svd.V);
+
+ R00 = MATD_EL(R, 0, 0);
+ R01 = MATD_EL(R, 0, 1);
+ R02 = MATD_EL(R, 0, 2);
+ R10 = MATD_EL(R, 1, 0);
+ R11 = MATD_EL(R, 1, 1);
+ R12 = MATD_EL(R, 1, 2);
+ R20 = MATD_EL(R, 2, 0);
+ R21 = MATD_EL(R, 2, 1);
+ R22 = MATD_EL(R, 2, 2);
+
+ matd_destroy(R);
+ }
+
+ return matd_create_data(4, 4, (double[]) { R00, R01, R02, TX,
+ R10, R11, R12, TY,
+ R20, R21, R22, TZ,
+ 0, 0, 0, 1 });
+}
+
+// Similar to above
+// Recover the model view matrix assuming that the projection matrix is:
+//
+// [ F 0 A 0 ] (see glFrustrum)
+// [ 0 G B 0 ]
+// [ 0 0 C D ]
+// [ 0 0 -1 0 ]
+
+matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D)
+{
+ // Note that every variable that we compute is proportional to the scale factor of H.
+ double R20 = -MATD_EL(H, 2, 0);
+ double R21 = -MATD_EL(H, 2, 1);
+ double TZ = -MATD_EL(H, 2, 2);
+ double R00 = (MATD_EL(H, 0, 0) - A*R20) / F;
+ double R01 = (MATD_EL(H, 0, 1) - A*R21) / F;
+ double TX = (MATD_EL(H, 0, 2) - A*TZ) / F;
+ double R10 = (MATD_EL(H, 1, 0) - B*R20) / G;
+ double R11 = (MATD_EL(H, 1, 1) - B*R21) / G;
+ double TY = (MATD_EL(H, 1, 2) - B*TZ) / G;
+
+ // compute the scale by requiring that the rotation columns are unit length
+ // (Use geometric average of the two length vectors we have)
+ double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
+ double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
+ double s = 1.0 / sqrtf(length1 * length2);
+
+ // get sign of S by requiring the tag to be in front of the camera
+ // (which is Z < 0) for our conventions.
+ if (TZ > 0)
+ s *= -1;
+
+ R20 *= s;
+ R21 *= s;
+ TZ *= s;
+ R00 *= s;
+ R01 *= s;
+ TX *= s;
+ R10 *= s;
+ R11 *= s;
+ TY *= s;
+
+ // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns.
+ double R02 = R10*R21 - R20*R11;
+ double R12 = R20*R01 - R00*R21;
+ double R22 = R00*R11 - R10*R01;
+
+ // TODO XXX: Improve rotation matrix by applying polar decomposition.
+
+ return matd_create_data(4, 4, (double[]) { R00, R01, R02, TX,
+ R10, R11, R12, TY,
+ R20, R21, R22, TZ,
+ 0, 0, 0, 1 });
+}
+
+// Only uses the upper 3x3 matrix.
+/*
+static void matrix_to_quat(const matd_t *R, double q[4])
+{
+ // see: "from quaternion to matrix and back"
+
+ // trace: get the same result if R is 4x4 or 3x3:
+ double T = MATD_EL(R, 0, 0) + MATD_EL(R, 1, 1) + MATD_EL(R, 2, 2) + 1;
+ double S = 0;
+
+ double m0 = MATD_EL(R, 0, 0);
+ double m1 = MATD_EL(R, 1, 0);
+ double m2 = MATD_EL(R, 2, 0);
+ double m4 = MATD_EL(R, 0, 1);
+ double m5 = MATD_EL(R, 1, 1);
+ double m6 = MATD_EL(R, 2, 1);
+ double m8 = MATD_EL(R, 0, 2);
+ double m9 = MATD_EL(R, 1, 2);
+ double m10 = MATD_EL(R, 2, 2);
+
+ if (T > 0.0000001) {
+ S = sqrtf(T) * 2;
+ q[1] = -( m9 - m6 ) / S;
+ q[2] = -( m2 - m8 ) / S;
+ q[3] = -( m4 - m1 ) / S;
+ q[0] = 0.25 * S;
+ } else if ( m0 > m5 && m0 > m10 ) { // Column 0:
+ S = sqrtf( 1.0 + m0 - m5 - m10 ) * 2;
+ q[1] = -0.25 * S;
+ q[2] = -(m4 + m1 ) / S;
+ q[3] = -(m2 + m8 ) / S;
+ q[0] = (m9 - m6 ) / S;
+ } else if ( m5 > m10 ) { // Column 1:
+ S = sqrtf( 1.0 + m5 - m0 - m10 ) * 2;
+ q[1] = -(m4 + m1 ) / S;
+ q[2] = -0.25 * S;
+ q[3] = -(m9 + m6 ) / S;
+ q[0] = (m2 - m8 ) / S;
+ } else {
+ // Column 2:
+ S = sqrtf( 1.0 + m10 - m0 - m5 ) * 2;
+ q[1] = -(m2 + m8 ) / S;
+ q[2] = -(m9 + m6 ) / S;
+ q[3] = -0.25 * S;
+ q[0] = (m4 - m1 ) / S;
+ }
+
+ double mag2 = 0;
+ for (int i = 0; i < 4; i++)
+ mag2 += q[i]*q[i];
+ double norm = 1.0 / sqrtf(mag2);
+ for (int i = 0; i < 4; i++)
+ q[i] *= norm;
+}
+*/
+
+// overwrites upper 3x3 area of matrix M. Doesn't touch any other elements of M.
+void quat_to_matrix(const double q[4], matd_t *M)
+{
+ double w = q[0], x = q[1], y = q[2], z = q[3];
+
+ MATD_EL(M, 0, 0) = w*w + x*x - y*y - z*z;
+ MATD_EL(M, 0, 1) = 2*x*y - 2*w*z;
+ MATD_EL(M, 0, 2) = 2*x*z + 2*w*y;
+
+ MATD_EL(M, 1, 0) = 2*x*y + 2*w*z;
+ MATD_EL(M, 1, 1) = w*w - x*x + y*y - z*z;
+ MATD_EL(M, 1, 2) = 2*y*z - 2*w*x;
+
+ MATD_EL(M, 2, 0) = 2*x*z - 2*w*y;
+ MATD_EL(M, 2, 1) = 2*y*z + 2*w*x;
+ MATD_EL(M, 2, 2) = w*w - x*x - y*y + z*z;
+}