diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 42b5b54f0..668c135ff 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -144,71 +144,60 @@ DexpFunctor::DexpFunctor(const Vector3& omega, double nearZeroThresholdSq, doubl DexpFunctor::DexpFunctor(const Vector3& omega) : DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {} -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 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; -} - -Vector3 DexpFunctor::doubleCrossC(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) { - // 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; -} - // Multiplies v with left Jacobian through vector operations only. -Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2) const { - Matrix3 D_BWv_w, D_CWWv_w; - const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); - const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); - if (H1) *H1 = -D_BWv_w + D_CWWv_w; +Vector3 DexpFunctor::applyRightJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Vector3 Wv = gtsam::cross(omega, v); + + Matrix3 WWv_H_w; + const Vector3 WWv = gtsam::doubleCross(omega, v, H1 ? &WWv_H_w : nullptr); + + if (H1) { + // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega + Matrix3 BWv_H_w = -Wv * E * omega.transpose() - B * skewSymmetric(v); + // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega + Matrix3 CWWv_H_w = -WWv * F * omega.transpose() + C * WWv_H_w; + *H1 = -BWv_H_w + CWWv_H_w; + } + if (H2) *H2 = rightJacobian(); - return v - BWv + CWWv; + return v - B * Wv + C * WWv; } -Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2) const { +Vector3 DexpFunctor::applyRightJacobianInverse(const Vector3& v, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { const Matrix3 invJr = rightJacobianInverse(); const Vector3 c = invJr * v; if (H1) { Matrix3 H; - applyDexp(c, H); // get derivative H of forward mapping + applyRightJacobian(c, H); // get derivative H of forward mapping *H1 = -invJr * H; } if (H2) *H2 = invJr; return c; } -Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, - OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2) const { - Matrix3 D_BWv_w, D_CWWv_w; - const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); - const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); - if (H1) *H1 = D_BWv_w + D_CWWv_w; +Vector3 so3::DexpFunctor::applyLeftJacobian(const Vector3& v, + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { + const Vector3 Wv = gtsam::cross(omega, v); + + Matrix3 WWv_H_w; + const Vector3 WWv = gtsam::doubleCross(omega, v, H1 ? &WWv_H_w : nullptr); + + if (H1) { + // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega + Matrix3 BWv_H_w = -Wv * E * omega.transpose() - B * skewSymmetric(v); + // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega + Matrix3 CWWv_H_w = -WWv * F * omega.transpose() + C * WWv_H_w; + *H1 = BWv_H_w + CWWv_H_w; + } + if (H2) *H2 = leftJacobian(); - return v + BWv + CWWv; + return v + B * Wv + C * WWv; } -Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, - OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2) const { +Vector3 so3::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) { @@ -284,34 +273,23 @@ template <> GTSAM_EXPORT SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { so3::DexpFunctor local(omega); - if (H) *H = local.dexp(); + if (H) *H = local.rightJacobian(); return SO3(local.expmap()); } template <> GTSAM_EXPORT Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { - return so3::DexpFunctor(omega).dexp(); + return so3::DexpFunctor(omega).rightJacobian(); } //****************************************************************************** -/* Right Jacobian for Log map in SO(3) - equation (10.86) and following - equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie - Groups", Volume 2, 2008. - - logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega - - where Jrinv = LogmapDerivative(omega). This maps a perturbation on the - manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv * - omega) - */ template <> GTSAM_EXPORT Matrix3 SO3::LogmapDerivative(const Vector3& omega) { return so3::DexpFunctor(omega).rightJacobianInverse(); } -//****************************************************************************** template <> GTSAM_EXPORT Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 1b3fe583b..b4d00d92c 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -192,40 +192,35 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { // Compute the left Jacobian for Exponential map in SO(3) Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } - /// 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(); } + /// Multiplies with rightJacobian(), with optional derivatives + Vector3 applyRightJacobian(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; - /// Computes B * (omega x v). - Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - - /// Computes C * (omega x (omega x v)). - Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - - /// Multiplies with dexp(), with optional derivatives - Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; - - /// Multiplies with dexp().inverse(), with optional derivatives - Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + /// Multiplies with rightJacobian().inverse(), with optional derivatives + Vector3 applyRightJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with leftJacobian(), with optional derivatives - Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + 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; + OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 + /// @deprecated: use rightJacobian + inline Matrix3 dexp() const { return rightJacobian(); } + + /// @deprecated: use rightJacobianInverse + inline Matrix3 invDexp() const { return rightJacobianInverse(); } +#endif }; } // namespace so3 diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index fe3fc1b07..922c01791 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -224,9 +224,10 @@ namespace so3 { gtsam::Matrix3 leftJacobian() const; gtsam::Matrix3 rightJacobianInverse() const; gtsam::Matrix3 leftJacobianInverse() const; - - gtsam::Matrix3 dexp() const; - gtsam::Matrix3 invDexp() const; + gtsam::Vector3 applyRightJacobian(const gtsam::Vector3& v) const; + gtsam::Vector3 applyRightJacobianInverse(const gtsam::Vector3& v) const; + gtsam::Vector3 applyLeftJacobian(const gtsam::Vector3& v) const; + gtsam::Vector3 applyLeftJacobianInverse(const gtsam::Vector3& v) const; }; } diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index feac58093..ca3e55e43 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -355,76 +355,40 @@ TEST(SO3, JacobianInverses) { } //****************************************************************************** -TEST(SO3, CrossB) { - Matrix aH1; - for (bool nearZero : {true, false}) { - std::function f = - [nearZero](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).crossB(v); - }; - for (const Vector3& omega : test_cases::omegas(nearZero)) { - so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); - for (const Vector3& v : test_cases::vs) { - local.crossB(v, aH1); - EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); - } - } - } -} - -//****************************************************************************** -TEST(SO3, DoubleCrossC) { - Matrix aH1; - for (bool nearZero : {true, false}) { - std::function f = - [nearZero](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).doubleCrossC(v); - }; - for (const Vector3& omega : test_cases::omegas(nearZero)) { - so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); - for (const Vector3& v : test_cases::vs) { - local.doubleCrossC(v, aH1); - EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); - } - } - } -} - -//****************************************************************************** -TEST(SO3, ApplyDexp) { +TEST(SO3, ApplyRightJacobian) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = - [nearZero](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyDexp(v); - }; + [nearZero](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyRightJacobian(v); + }; for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); for (const Vector3& v : test_cases::vs) { - EXPECT(assert_equal(Vector3(local.dexp() * v), - local.applyDexp(v, aH1, aH2))); + EXPECT(assert_equal(Vector3(local.rightJacobian() * v), + local.applyRightJacobian(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(local.dexp(), aH2)); + EXPECT(assert_equal(local.rightJacobian(), aH2)); } } } } //****************************************************************************** -TEST(SO3, ApplyInvDexp) { +TEST(SO3, ApplyRightJacobianInverse) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = - [nearZero](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyInvDexp(v); - }; + [nearZero](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyRightJacobianInverse(v); + }; for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); Matrix invJr = local.rightJacobianInverse(); for (const Vector3& v : test_cases::vs) { EXPECT( - assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2))); + assert_equal(Vector3(invJr * v), local.applyRightJacobianInverse(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(invJr, aH2)); @@ -438,14 +402,14 @@ TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = - [nearZero](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobian(v); - }; + [nearZero](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobian(v); + }; for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); for (const Vector3& v : test_cases::vs) { EXPECT(assert_equal(Vector3(local.leftJacobian() * v), - local.applyLeftJacobian(v, aH1, aH2))); + local.applyLeftJacobian(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(local.leftJacobian(), aH2)); @@ -459,15 +423,15 @@ TEST(SO3, ApplyLeftJacobianInverse) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = - [nearZero](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobianInverse(v); - }; + [nearZero](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobianInverse(v); + }; for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); Matrix invJl = local.leftJacobianInverse(); for (const Vector3& v : test_cases::vs) { EXPECT(assert_equal(Vector3(invJl * v), - local.applyLeftJacobianInverse(v, aH1, aH2))); + local.applyLeftJacobianInverse(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(invJl, aH2)); diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 52f730cbb..97b3c425b 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -66,7 +66,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, // Calculate exact mean propagation Matrix3 w_tangent_H_theta, invH; const Vector3 w_tangent = // angular velocity mapped back to tangent space - local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); + local.applyRightJacobianInverse(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; @@ -79,7 +79,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, if (A) { // Exact derivative of R*a with respect to theta: - const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp(); + const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.rightJacobian(); A->setIdentity(); A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta