33 #ifndef _APRILTAG_MATH_H 34 #define _APRILTAG_MATH_H 53 R[4] = sqrt(A[4] - R[3]*R[3]);
56 R[7] = (A[5] - R[3]*R[6]) / R[4];
59 R[8] = sqrt(A[8] - R[6]*R[6] - R[7]*R[7]);
73 R[3] = -A[3]*R[0] / A[4];
79 R[6] = (-A[6]*R[0] - A[7]*R[3]) / A[8];
82 R[7] = -A[7]*R[4] / A[8];
101 tmp[1] = M[3]*B[0] + M[4]*B[1];
102 tmp[2] = M[6]*B[0] + M[7]*B[1] + M[8]*B[2];
104 R[0] = M[0]*tmp[0] + M[3]*tmp[1] + M[6]*tmp[2];
105 R[1] = M[4]*tmp[1] + M[7]*tmp[2];
static void mat33_sym_solve(const double *A, const double *B, double *R)
static void mat33_lower_tri_inv(const double *A, double *R)
static void mat33_chol(const double *A, double *R)