From c7f651d98db1f0a7d3bea7b1a047cd1e4f93c7e1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 00:50:38 -0500 Subject: [PATCH] applyLeftJacobian --- gtsam/geometry/SO3.cpp | 67 +++++++++++++++++++++----------- gtsam/geometry/SO3.h | 6 +++ gtsam/geometry/tests/testSO3.cpp | 24 ++++++++++++ 3 files changed, 74 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index bc2e1e6ec..13600f884 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace gtsam { @@ -98,35 +99,47 @@ Matrix3 DexpFunctor::rightJacobian() const { } } -// Derivative of w x w x v in w: -static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) { - return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose(); +Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { + // Wv = omega x * v + const Vector3 Wv = gtsam::cross(omega, v); + if (H) { + // 1x3 Jacobian of B with respect to omega + const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); + // Apply product rule: + *H = Wv * HB - B * skewSymmetric(v); + } + return B * Wv; +} + +Vector3 DexpFunctor::doubleCross(const Vector3& v, + OptionalJacobian<3, 3> H) const { + // WWv = omega x (omega x * v) + Matrix3 doubleCrossJacobian; + const Vector3 WWv = + gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); + if (H) { + // 1x3 Jacobian of C with respect to omega + const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); + // Apply product rule: + *H = WWv * HC + C * doubleCrossJacobian; + } + return C * WWv; } // Multiplies v with left Jacobian through vector operations only. -// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once -// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - // Wv = omega x * v, with Jacobian -V in w - const Vector3 Wv = gtsam::cross(omega, v); - if (nearZero) { if (H1) *H1 = 0.5 * skewSymmetric(v); if (H2) *H2 = I_3x3 - 0.5 * W; - return v - 0.5 * Wv; + return v - 0.5 * gtsam::cross(omega, v); } else { - // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian - const Vector3 WWv = gtsam::cross(omega, Wv); - if (H1) { - // 1x3 Jacobians of B and C with respect to theta - const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); - const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); - *H1 = -Wv * HB + B * skewSymmetric(v) + - C * doubleCrossJacobian(omega, v) + WWv * HC; - } + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = cross(v, D_BWv_omega); + const Vector3 CWWv = doubleCross(v, D_CWWv_omega); + if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; if (H2) *H2 = rightJacobian(); - return v - B * Wv + C * WWv; + return v - BWv + CWWv; } } @@ -154,10 +167,18 @@ Matrix3 DexpFunctor::leftJacobian() const { Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 Jw = leftJacobian(); - if (H1) H1->setZero(); - if (H2) *H2 = Jw; - return Jw * v; + if (nearZero) { + if (H1) *H1 = - 0.5 * skewSymmetric(v); + if (H2) *H2 = I_3x3 + 0.5 * W; + return v + 0.5 * gtsam::cross(omega, v); + } else { + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = cross(v, D_BWv_omega); + const Vector3 CWWv = doubleCross(v, D_CWWv_omega); + if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + if (H2) *H2 = leftJacobian(); + return v + BWv + CWWv; + } } } // namespace so3 diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index bd54f0cc4..8766fecbe 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -160,6 +160,12 @@ class DexpFunctor : public ExpmapFunctor { const Vector3 omega; double C; // Ethan Eade's C constant + /// Computes B * (omega x v). + Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Computes C * (omega x (omega x v)). + Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + public: /// Constructor with element of Lie algebra so(3) GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 27c143d31..8773e287a 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; @@ -326,6 +327,29 @@ TEST(SO3, ApplyDexp) { } } +//****************************************************************************** +TEST(SO3, ApplyLeftJacobian) { + Matrix aH1, aH2; + for (bool nearZeroApprox : {false, true}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v); + }; + for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), + Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { + so3::DexpFunctor local(omega, nearZeroApprox); + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), + Vector3(0.4, 0.3, 0.2)}) { + CHECK(assert_equal(Vector3(local.leftJacobian() * v), + local.applyLeftJacobian(v, aH1, aH2))); + CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); + CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); + CHECK(assert_equal(local.leftJacobian(), aH2)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2;