From 6bf410c0d8a9180c715d1a87789c0e20ab41bd07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 10:57:40 -0500 Subject: [PATCH] Revert "Improved CayleyChart Local" This reverts commit b5284e4b3fb6933c4443212ba695d1c151961e74. --- gtsam/geometry/Rot3M.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index eb1bd6461..02e5b771f 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,13 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - const Matrix3 P = A + I_3x3; - // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. - if (P.determinant() == 0.0) { - throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); - } - Matrix3 Pinv = (A + I_3x3).inverse(); - return SO3::Vee(Pinv * (A - I_3x3)) * 2; + // Mathematica closed form optimization (procrastination?) gone wild: + const double a = A(0, 0), b = A(0, 1), c = A(0, 2); + const double d = A(1, 0), e = A(1, 1), f = A(1, 2); + const double g = A(2, 0), h = A(2, 1), i = A(2, 2); + const double di = d * i, ce = c * e, cd = c * d, fg = f * g; + const double M = 1 + e - f * h + i + e * i; + const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } /* ************************************************************************* */