diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 6499d4139..93bf072c5 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -142,14 +142,14 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = rightJacobian().inverse(); - const Vector3 c = invDexp * v; + const Matrix3 invJr = rightJacobianInverse(); + const Vector3 c = invJr * v; if (H1) { - Matrix3 D_dexpv_w; - applyDexp(c, D_dexpv_w); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_w; + Matrix3 H; + applyDexp(c, H); // get derivative H of forward mapping + *H1 = -invJr * H; } - if (H2) *H2 = invDexp; + if (H2) *H2 = invJr; return c; } @@ -164,6 +164,20 @@ Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, return v + BWv + CWWv; } +Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 invJl = leftJacobianInverse(); + const Vector3 c = invJl * v; + if (H1) { + Matrix3 H; + applyLeftJacobian(c, H); // get derivative H of forward mapping + *H1 = -invJl * H; + } + if (H2) *H2 = invJl; + return c; +} + } // namespace so3 //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 488dea12e..2845fcce7 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -162,7 +162,7 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { 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 + double D; // (1 - A/2B) / theta2 or 1/12 for theta->0 // Constants used in cross and doubleCross double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 @@ -212,6 +212,11 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + /// Multiplies with leftJacobianInverse(), with optional derivatives + Vector3 applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + 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; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 96872eae9..d3313b7f1 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -385,6 +385,28 @@ TEST(SO3, ApplyDexp) { } } +//****************************************************************************** +TEST(SO3, ApplyInvDexp) { + Matrix aH1, aH2; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + Matrix invJr = local.rightJacobianInverse(); + for (const Vector3& v : test_cases::vs) { + EXPECT( + assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(invJr, aH2)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; @@ -407,22 +429,22 @@ TEST(SO3, ApplyLeftJacobian) { } //****************************************************************************** -TEST(SO3, ApplyInvDexp) { +TEST(SO3, ApplyLeftJacobianInverse) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); + return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - Matrix invDexp = local.dexp().inverse(); + Matrix invJl = local.leftJacobianInverse(); for (const Vector3& v : test_cases::vs) { - EXPECT(assert_equal(Vector3(invDexp * v), - local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(Vector3(invJl * v), + local.applyLeftJacobianInverse(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(invDexp, aH2)); + EXPECT(assert_equal(invJl, aH2)); } } }