From 2a22123d5d92572631b6e7fa1c9fff2f835f6c8a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:46:25 -0400 Subject: [PATCH] Add AdjointMap into traits --- gtsam/base/Lie.h | 29 +++++++++++++++++++---------- gtsam/base/VectorSpace.h | 33 ++++++++++++++++++--------------- gtsam/navigation/InvariantEKF.h | 6 +++--- 3 files changed, 40 insertions(+), 28 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 4248f16b2..17c35446e 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -169,32 +169,34 @@ namespace internal { /// To use this for your gtsam type, define: /// template<> struct traits : public internal::LieGroupTraits {}; /// Assumes existence of: identity, dimension, localCoordinates, retract, -/// and additionally Logmap, Expmap, compose, between, and inverse +/// and additionally Logmap, Expmap, AdjointMap, compose, between, and inverse template -struct LieGroupTraits: public GetDimensionImpl { +struct LieGroupTraits : public GetDimensionImpl { using structure_category = lie_group_tag; /// @name Group /// @{ using group_flavor = multiplicative_group_tag; - static Class Identity() { return Class::Identity();} + static Class Identity() { return Class::Identity(); } /// @} /// @name Manifold /// @{ using ManifoldType = Class; + // Note: Class::dimension can be an int or Eigen::Dynamic. + // GetDimensionImpl handles resolving this to a static value or providing GetDimension(obj). inline constexpr static auto dimension = Class::dimension; using TangentVector = Eigen::Matrix; using ChartJacobian = OptionalJacobian; static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { - return origin.localCoordinates(other, Horigin, Hother); + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + return origin.localCoordinates(other, H1, H2); } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian Horigin = {}, ChartJacobian Hv = {}) { - return origin.retract(v, Horigin, Hv); + ChartJacobian H = {}, ChartJacobian Hv = {}) { + return origin.retract(v, H, Hv); } /// @} @@ -209,19 +211,26 @@ struct LieGroupTraits: public GetDimensionImpl { } static Class Compose(const Class& m1, const Class& m2, // - ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.compose(m2, H1, H2); } static Class Between(const Class& m1, const Class& m2, // - ChartJacobian H1 = {}, ChartJacobian H2 = {}) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.between(m2, H1, H2); } static Class Inverse(const Class& m, // - ChartJacobian H = {}) { + ChartJacobian H = {}) { return m.inverse(H); } + + static Eigen::Matrix AdjointMap(const Class& m) { + // This assumes that the Class itself provides a member function `AdjointMap()` + // For dynamically-sized types (dimension == Eigen::Dynamic), + // m.AdjointMap() must return a gtsam::Matrix of the correct runtime dimensions. + return m.AdjointMap(); + } /// @} }; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 1a486886d..b81b19ac3 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -35,7 +35,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); - Class v = other-origin; + Class v = other - origin; return v.vector(); } @@ -82,12 +82,12 @@ struct VectorSpaceImpl { return -v; } - static LieAlgebra Hat(const TangentVector& v) { - return v; - } + static LieAlgebra Hat(const TangentVector& v) { return v; } - static TangentVector Vee(const LieAlgebra& X) { - return X; + static TangentVector Vee(const LieAlgebra& X) { return X; } + + static Jacobian AdjointMap(const Class& /*m*/) { + return Jacobian::Identity(); } /// @} }; @@ -118,7 +118,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Eye(origin); if (H2) *H2 = Eye(other); - Class v = other-origin; + Class v = other - origin; return v.vector(); } @@ -141,8 +141,7 @@ struct VectorSpaceImpl { static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { Class result(v); - if (Hv) - *Hv = Eye(v); + if (Hv) *Hv = Eye(v); return result; } @@ -165,6 +164,7 @@ struct VectorSpaceImpl { return -v; } + static Eigen::MatrixXd AdjointMap(const Class& m) { return Eye(m); } /// @} }; @@ -273,8 +273,8 @@ struct ScalarTraits : VectorSpaceImpl { if (H) (*H)[0] = 1.0; return v[0]; } + // AdjointMap for ScalarTraits is inherited from VectorSpaceImpl /// @} - }; } // namespace internal @@ -352,6 +352,9 @@ struct traits > : if (H) *H = Jacobian::Identity(); return m + Eigen::Map(v.data()); } + + // AdjointMap for fixed-size Eigen matrices is inherited from + // internal::VectorSpaceImpl< Eigen::Matrix , M*N > /// @} }; @@ -404,7 +407,7 @@ struct DynamicTraits { static TangentVector Local(const Dynamic& m, const Dynamic& other, // ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = -Eye(m); - if (H2) *H2 = Eye(m); + if (H2) *H2 = Eye(m); TangentVector v(GetDimension(m)); Eigen::Map(v.data(), m.rows(), m.cols()) = other - m; return v; @@ -425,7 +428,7 @@ struct DynamicTraits { static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) { if (H) *H = Eye(m); TangentVector result(GetDimension(m)); - Eigen::Map(result.data(), m.cols(), m.rows()) = m; + Eigen::Map(result.data(), m.rows(), m.cols()) = m; return result; } @@ -460,7 +463,8 @@ struct DynamicTraits { static TangentVector Vee(const LieAlgebra& X) { return X; } - + + static Jacobian AdjointMap(const Dynamic& m) { return Eye(m); } /// @} }; @@ -505,5 +509,4 @@ private: T p, q, r; }; -} // namespace gtsam - +} // namespace gtsam diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 08ba24def..20d74285f 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -77,9 +77,9 @@ namespace gtsam { * @param Q Process noise covariance. */ void predict(const G& U, const Covariance& Q) { - this->X_ = this->X_.compose(U); - // TODO(dellaert): traits::AdjointMap should exist - const Jacobian A = traits::Inverse(U).AdjointMap(); + this->X_ = traits::Compose(this->X_, U); + const G U_inv = traits::Inverse(U); + const Jacobian A = traits::AdjointMap(U_inv); // P_ is Covariance. A is Jacobian. Q is Covariance. // All are Eigen::Matrix. this->P_ = A * this->P_ * A.transpose() + Q;