From 0c1f087dba7374e13440461e260ee47086d39c31 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 00:10:26 -0500 Subject: [PATCH] Final touches --- gtsam/geometry/Pose3.cpp | 7 ++----- gtsam/geometry/SO3.cpp | 25 ++++++++++++------------- gtsam/geometry/SO3.h | 27 ++++++++++++--------------- 3 files changed, 26 insertions(+), 33 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f317cd4e8..5f0e8e3ce 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -240,12 +240,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { /* ************************************************************************* */ namespace pose3 { -class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { - protected: +struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { // Constant used in computeQ double F; // (B - 0.5) / theta2 or -1/24 for theta->0 - public: ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) : so3::DexpFunctor(omega, nearZeroApprox) { F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2; @@ -253,7 +251,7 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand - // how to compute mess below from its derivatives in w and v. + // how to compute mess below from applyLeftJacobian derivatives in w and v. Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; @@ -261,7 +259,6 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); } - protected: static constexpr double _one_twenty_fourth = - 1.0 / 24.0; }; } // namespace pose3 diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index ef1fa374b..c54306ae5 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -26,7 +26,6 @@ #include #include -#include #include namespace gtsam { @@ -94,7 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) } Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { - // Wv = omega x * v + // Wv = omega x v const Vector3 Wv = gtsam::cross(omega, v); if (H) { // Apply product rule: @@ -123,10 +122,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v, // 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_omega, D_CWWv_omega; - const Vector3 BWv = crossB(v, D_BWv_omega); - const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); - if (H1) *H1 = -D_BWv_omega + D_CWWv_omega; + 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; if (H2) *H2 = rightJacobian(); return v - BWv + CWWv; } @@ -136,9 +135,9 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, const Matrix3 invDexp = rightJacobian().inverse(); const Vector3 c = invDexp * v; if (H1) { - Matrix3 D_dexpv_omega; - applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_omega; + Matrix3 D_dexpv_w; + applyDexp(c, D_dexpv_w); // get derivative H of forward mapping + *H1 = -invDexp * D_dexpv_w; } if (H2) *H2 = invDexp; return c; @@ -147,10 +146,10 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = crossB(v, D_BWv_omega); - const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); - if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + 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; if (H2) *H2 = leftJacobian(); return v + BWv + CWWv; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index efb947e73..f831aa426 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -132,18 +132,15 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map -class GTSAM_EXPORT ExpmapFunctor { - protected: +struct GTSAM_EXPORT ExpmapFunctor { const double theta2, theta; const Matrix3 W, WW; bool nearZero; + // Ethan Eade's constants: - double A; // A = sin(theta) / theta or 1 for theta->0 - double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 + double A; // A = sin(theta) / theta or 1 for theta->0 + double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 - void init(bool nearZeroApprox = false); - - public: /// Constructor with element of Lie algebra so(3) explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -152,33 +149,34 @@ class GTSAM_EXPORT ExpmapFunctor { /// Rodrigues formula SO3 expmap() const; + + protected: + void init(bool nearZeroApprox = false); }; /// Functor that implements Exponential map *and* its derivatives -class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { - protected: +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 // 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 - public: /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, // Information Theory, and Lie Groups", Volume 2, 2008. - // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) - // This maps a perturbation v in the tangent space to - // a perturbation on the manifold Expmap(dexp * v) + // Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi) + // This maps a perturbation dxi=(w,v) in the tangent space to + // a perturbation on the manifold Expmap(dexp * xi) Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } // 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 + /// Differential of expmap == right Jacobian inline Matrix3 dexp() const { return rightJacobian(); } /// Computes B * (omega x v). @@ -199,7 +197,6 @@ class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; - protected: 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;