diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index c54306ae5..6499d4139 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -87,19 +87,29 @@ SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - C = nearZero ? one_sixth : (1 - A) / theta2; - D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2; - E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; + if (!nearZero) { + C = (1 - A) / theta2; + D = (1.0 - A / (2.0 * B)) / theta2; + E = (2.0 * B - A) / theta2; + F = (3.0 * C - B) / theta2; + } else { + // Limit as theta -> 0 + // TODO(Frank): flipping signs here does not trigger any tests: harden! + C = one_sixth; + D = one_twelfth; + E = one_twelfth; + F = one_sixtieth; + } } Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x v const Vector3 Wv = gtsam::cross(omega, v); if (H) { - // Apply product rule: - // D * omega.transpose() is 1x3 Jacobian of B with respect to omega - // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v) - *H = Wv * D * omega.transpose() - B * skewSymmetric(v); + // Apply product rule to (B Wv) + // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega + // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v) + *H = - Wv * E * omega.transpose() - B * skewSymmetric(v); } return B * Wv; } @@ -111,10 +121,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v, const Vector3 WWv = gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); if (H) { - // Apply product rule: - // E * omega.transpose() is 1x3 Jacobian of C with respect to omega - // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v) - *H = WWv * E * omega.transpose() + C * doubleCrossJacobian; + // Apply product rule to (C WWv) + // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega + // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v) + *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian; } return C * WWv; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index f831aa426..488dea12e 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -157,10 +157,16 @@ struct GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; - double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 + + // Ethan's C constant used in Jacobians + double C; // (1 - A) / theta^2 or 1/6 for theta->0 + + // Constant used in inverse Jacobians + double D; // 1.0 - A / (2.0 * B)) / theta2 or 1/12 for theta->0 + // Constants used in cross and doubleCross - double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 - double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 + double F; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -179,6 +185,15 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { /// Differential of expmap == right Jacobian inline Matrix3 dexp() const { return rightJacobian(); } + /// Inverse of right Jacobian + Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; } + + // Inverse of left Jacobian + Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; } + + /// Synonym for rightJacobianInverse + inline Matrix3 invDexp() const { return rightJacobianInverse(); } + /// Computes B * (omega x v). Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; @@ -198,8 +213,8 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { OptionalJacobian<3, 3> H2 = {}) const; static constexpr double one_sixth = 1.0 / 6.0; - static constexpr double _one_twelfth = -1.0 / 12.0; - static constexpr double _one_sixtieth = -1.0 / 60.0; + static constexpr double one_twelfth = 1.0 / 12.0; + static constexpr double one_sixtieth = 1.0 / 60.0; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3ecabe84b..96872eae9 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using namespace std::placeholders; using namespace std; @@ -304,13 +305,29 @@ TEST(SO3, JacobianLogmap) { } namespace test_cases { -std::vector small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector small{{0, 0, 0}, // + {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, + {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}}; std::vector large{ {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; auto omegas = [](bool nearZero) { return nearZero ? small : large; }; std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; } // namespace test_cases +//****************************************************************************** +TEST(SO3, JacobianInverses) { + Matrix HR, HL; + for (bool nearZero : {true, false}) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + EXPECT(assert_equal(local.rightJacobian().inverse(), + local.rightJacobianInverse())); + EXPECT(assert_equal(local.leftJacobian().inverse(), + local.leftJacobianInverse())); + } + } +} + //****************************************************************************** TEST(SO3, CrossB) { Matrix aH1;