From 0476d5200e98e6f3ebc49874c344afb45a37b4a5 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Mon, 6 Apr 2026 02:04:47 -0400 Subject: [PATCH 01/21] extend plg --- gtsam/base/ProductLieGroup-inl.h | 531 ++++++++++++++++++------------- gtsam/base/ProductLieGroup.h | 156 ++++++++- tests/testProductLieGroup.cpp | 257 +++++++++++++++ 3 files changed, 719 insertions(+), 225 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 42a0681459..276eeb24f9 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -20,162 +20,224 @@ namespace gtsam { -template -ProductLieGroup ProductLieGroup::operator*( +template +ProductLieGroup ProductLieGroup::operator*( const ProductLieGroup& other) const { checkMatchingDimensions(other, "operator*"); - return ProductLieGroup(traits::Compose(this->first, other.first), - traits::Compose(this->second, other.second)); + if constexpr (isDirectProduct) { + return ProductLieGroup(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); + } else { + const Action action{}; + const H actedSecond = action(this->first, other.second); + return ProductLieGroup(traits::Compose(this->first, other.first), + traits::Compose(this->second, actedSecond)); + } } -template -ProductLieGroup ProductLieGroup::inverse() const { - return ProductLieGroup(traits::Inverse(this->first), - traits::Inverse(this->second)); +template +ProductLieGroup ProductLieGroup::inverse() const { + if constexpr (isDirectProduct) { + return ProductLieGroup(traits::Inverse(this->first), + traits::Inverse(this->second)); + } else { + const Action action{}; + const G gInv = traits::Inverse(this->first); + const H hInv = traits::Inverse(this->second); + return ProductLieGroup(gInv, action(gInv, hInv)); + } } -template -ProductLieGroup ProductLieGroup::retract(const TangentVector& v, - ChartJacobian H1, - ChartJacobian H2) const { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - if (static_cast(v.size()) != - productDimension) { - throw std::invalid_argument( - "ProductLieGroup::retract tangent dimension does not match product " - "dimension"); - } - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - G g = traits::Retract(this->first, tangentSegment(v, 0, firstDimension), - H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - H h = traits::Retract( - this->second, tangentSegment(v, firstDimension, secondDimension), - H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return ProductLieGroup(g, h); -} - -template -typename ProductLieGroup::TangentVector -ProductLieGroup::localCoordinates(const ProductLieGroup& g, - ChartJacobian H1, - ChartJacobian H2) const { +template +ProductLieGroup ProductLieGroup::retract( + const TangentVector& v, ChartJacobian H1, ChartJacobian H2) const { + if constexpr (isDirectProduct) { + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + if (static_cast(v.size()) != productDimension) { + throw std::invalid_argument( + "ProductLieGroup::retract tangent dimension does not match product " + "dimension"); + } + Jacobian1 D_g_first; + Jacobian1 D_g_second; + Jacobian2 D_h_first; + Jacobian2 D_h_second; + G g = traits::Retract(this->first, + tangentSegment(v, 0, firstDimension), + H1 ? &D_g_first : nullptr, + H2 ? &D_g_second : nullptr); + H h = traits::Retract( + this->second, tangentSegment(v, firstDimension, secondDimension), + H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_first; + } + if (H2) { + *H2 = zeroJacobian(productDimension); + H2->block(0, 0, firstDimension, firstDimension) = D_g_second; + H2->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return ProductLieGroup(g, h); + } else { + const size_t productDimension = + combinedDimension(firstDim(), secondDim()); + if (static_cast(v.size()) != productDimension) { + throw std::invalid_argument( + "ProductLieGroup::retract tangent dimension does not match product " + "dimension"); + } + Jacobian D_g_v; + ProductLieGroup g = + ProductLieGroup::Expmap(v, H2 ? OptionalJacobian(&D_g_v) + : ChartJacobian()); + ProductLieGroup h = compose(g); + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = D_g_v; + return h; + } +} + +template +typename ProductLieGroup::TangentVector +ProductLieGroup::localCoordinates(const ProductLieGroup& g, + ChartJacobian H1, + ChartJacobian H2) const { checkMatchingDimensions(g, "localCoordinates"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - typename traits::TangentVector v2 = traits::Local( - this->second, g.second, H1 ? &D_h_first : nullptr, - H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return makeTangentVector(v1, v2, firstDimension, secondDimension); -} - -template -ProductLieGroup ProductLieGroup::compose( + if constexpr (isDirectProduct) { + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian1 D_g_second; + Jacobian2 D_h_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, + H2 ? &D_g_second : nullptr); + typename traits::TangentVector v2 = traits::Local( + this->second, g.second, H1 ? &D_h_first : nullptr, + H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_first; + } + if (H2) { + *H2 = zeroJacobian(productDimension); + H2->block(0, 0, firstDimension, firstDimension) = D_g_second; + H2->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return makeTangentVector(v1, v2, firstDimension, secondDimension); + } else { + ProductLieGroup h = between(g); + Jacobian D_v_h; + TangentVector v = ProductLieGroup::Logmap( + h, H1 || H2 ? OptionalJacobian(&D_v_h) + : ChartJacobian()); + if (H1) *H1 = -D_v_h * h.inverse().AdjointMap(); + if (H2) *H2 = D_v_h; + return v; + } +} + +template +ProductLieGroup ProductLieGroup::compose( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "compose"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Compose(this->first, other.first, H1 ? &D_g_first : nullptr); - H h = traits::Compose(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); -} - -template -ProductLieGroup ProductLieGroup::between( + const size_t productDimension = combinedDimension(firstDim(), secondDim()); + if constexpr (isDirectProduct) { + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Compose(this->first, other.first, + H1 ? &D_g_first : nullptr); + H h = traits::Compose(this->second, other.second, + H1 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + if (H2) *H2 = identityJacobian(productDimension); + return ProductLieGroup(g, h); + } else { + ProductLieGroup result = (*this) * other; + if (H1) *H1 = other.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(productDimension); + return result; + } +} + +template +ProductLieGroup ProductLieGroup::between( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "between"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Between(this->first, other.first, H1 ? &D_g_first : nullptr); - H h = traits::Between(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); -} - -template -ProductLieGroup ProductLieGroup::inverse(ChartJacobian D) const { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Inverse(this->first, D ? &D_g_first : nullptr); - H h = traits::Inverse(this->second, D ? &D_h_second : nullptr); - if (D) { - *D = zeroJacobian(productDimension); - D->block(0, 0, firstDimension, firstDimension) = D_g_first; - D->block(firstDimension, firstDimension, secondDimension, secondDimension) = - D_h_second; + const size_t productDimension = combinedDimension(firstDim(), secondDim()); + if constexpr (isDirectProduct) { + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Between(this->first, other.first, + H1 ? &D_g_first : nullptr); + H h = traits::Between(this->second, other.second, + H1 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + if (H2) *H2 = identityJacobian(productDimension); + return ProductLieGroup(g, h); + } else { + ProductLieGroup result = this->inverse() * other; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(productDimension); + return result; } - return ProductLieGroup(g, h); } -template -ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, - ChartJacobian Hv) { +template +ProductLieGroup ProductLieGroup::inverse( + ChartJacobian D) const { + if constexpr (isDirectProduct) { + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Inverse(this->first, D ? &D_g_first : nullptr); + H h = traits::Inverse(this->second, D ? &D_h_second : nullptr); + if (D) { + *D = zeroJacobian(productDimension); + D->block(0, 0, firstDimension, firstDimension) = D_g_first; + D->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return ProductLieGroup(g, h); + } else { + if (D) *D = -AdjointMap(); + return inverse(); + } +} + +template +ProductLieGroup ProductLieGroup::Expmap( + const TangentVector& v, ChartJacobian Hv) { size_t firstDimension = 0; size_t secondDimension = 0; if constexpr (firstDynamic && secondDynamic) { @@ -216,12 +278,10 @@ ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, Matrix D_h_second; const auto v1 = tangentSegment(v, 0, firstDimension); const auto v2 = tangentSegment(v, firstDimension, secondDimension); - ProductLieGroup result = - Expmap(v1, v2, - Hv ? OptionalJacobian(D_g_first) - : OptionalJacobian(), - Hv ? OptionalJacobian(D_h_second) - : OptionalJacobian()); + ProductLieGroup result = Expmap(v1, v2, Hv ? DynamicJacobian(D_g_first) + : DynamicJacobian(), + Hv ? DynamicJacobian(D_h_second) + : DynamicJacobian()); if (Hv) { const size_t productDimension = combinedDimension(firstDimension, secondDimension); @@ -233,76 +293,107 @@ ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, return result; } -template -ProductLieGroup ProductLieGroup::Expmap( +template +ProductLieGroup ProductLieGroup::Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, - OptionalJacobian H1, - OptionalJacobian H2) { - const size_t firstDimension = static_cast(v1.size()); - const size_t secondDimension = static_cast(v2.size()); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); - H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = Matrix::Zero(productDimension, firstDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - } - if (H2) { - *H2 = Matrix::Zero(productDimension, secondDimension); - H2->block(firstDimension, 0, secondDimension, secondDimension) = D_h_second; - } - return ProductLieGroup(g, h); -} - -template -typename ProductLieGroup::TangentVector ProductLieGroup::Logmap( + DynamicJacobian H1, DynamicJacobian H2) { + if constexpr (isDirectProduct) { + const size_t firstDimension = static_cast(v1.size()); + const size_t secondDimension = static_cast(v2.size()); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); + H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = Matrix::Zero(productDimension, firstDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + } + if (H2) { + *H2 = Matrix::Zero(productDimension, secondDimension); + H2->block(firstDimension, 0, secondDimension, secondDimension) = + D_h_second; + } + return ProductLieGroup(g, h); + } else { + static_assert( + product_lie_group_detail::HasSemidirectExpmap::value, + "Semidirect ProductLieGroup actions must provide " + "template static Product Expmap(v1, v2, H1, H2)."); + return Action::template Expmap(v1, v2, H1, H2); + } +} + +template +typename ProductLieGroup::TangentVector +ProductLieGroup::Logmap( const ProductLieGroup& p, ChartJacobian Hp) { - const size_t firstDimension = p.firstDim(); - const size_t secondDimension = p.secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Logmap(p.first, Hp ? &D_g_first : nullptr); - typename traits::TangentVector v2 = - traits::Logmap(p.second, Hp ? &D_h_second : nullptr); - TangentVector v = makeTangentVector(v1, v2, firstDimension, secondDimension); - if (Hp) { - *Hp = zeroJacobian(productDimension); - Hp->block(0, 0, firstDimension, firstDimension) = D_g_first; - Hp->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; + if constexpr (isDirectProduct) { + const size_t firstDimension = p.firstDim(); + const size_t secondDimension = p.secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Logmap(p.first, Hp ? &D_g_first : nullptr); + typename traits::TangentVector v2 = + traits::Logmap(p.second, Hp ? &D_h_second : nullptr); + TangentVector v = + makeTangentVector(v1, v2, firstDimension, secondDimension); + if (Hp) { + *Hp = zeroJacobian(productDimension); + Hp->block(0, 0, firstDimension, firstDimension) = D_g_first; + Hp->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return v; + } else { + static_assert( + product_lie_group_detail::HasSemidirectLogmap::value, + "Semidirect ProductLieGroup actions must provide " + "template static typename Product::TangentVector " + "Logmap(const Product&, ChartJacobian)."); + return Action::template Logmap(p, Hp); } - return v; } -template -ProductLieGroup ProductLieGroup::expmap( +template +ProductLieGroup ProductLieGroup::expmap( const TangentVector& v) const { return compose(ProductLieGroup::Expmap(v)); } -template -typename ProductLieGroup::Jacobian ProductLieGroup::AdjointMap() - const { - const auto adjG = traits::AdjointMap(this->first); - const auto adjH = traits::AdjointMap(this->second); - const size_t d1 = static_cast(adjG.rows()); - const size_t d2 = static_cast(adjH.rows()); - Jacobian adj = zeroJacobian(d1 + d2); - adj.block(0, 0, d1, d1) = adjG; - adj.block(d1, d1, d2, d2) = adjH; - return adj; +template +typename ProductLieGroup::Jacobian +ProductLieGroup::AdjointMap() const { + if constexpr (isDirectProduct) { + const auto adjG = traits::AdjointMap(this->first); + const auto adjH = traits::AdjointMap(this->second); + const size_t d1 = static_cast(adjG.rows()); + const size_t d2 = static_cast(adjH.rows()); + Jacobian adj = zeroJacobian(d1 + d2); + adj.block(0, 0, d1, d1) = adjG; + adj.block(d1, d1, d2, d2) = adjH; + return adj; + } else { + static_assert( + product_lie_group_detail::HasSemidirectAdjointMap::value, + "Semidirect ProductLieGroup actions must provide " + "template static typename Product::Jacobian " + "AdjointMap(const Product&)."); + return Action::template AdjointMap(*this); + } } -template +template template -T ProductLieGroup::defaultIdentity() { +T ProductLieGroup::defaultIdentity() { if constexpr (traits::dimension == Eigen::Dynamic) { return T(); } else { @@ -310,9 +401,10 @@ T ProductLieGroup::defaultIdentity() { } } -template +template template -typename traits::TangentVector ProductLieGroup::tangentSegment( +typename traits::TangentVector +ProductLieGroup::tangentSegment( const TangentVector& v, size_t start, size_t runtimeDimension) { const int startIndex = static_cast(start); const int runtimeIndex = static_cast(runtimeDimension); @@ -324,9 +416,9 @@ typename traits::TangentVector ProductLieGroup::tangentSegment( } } -template -typename ProductLieGroup::TangentVector -ProductLieGroup::makeTangentVector( +template +typename ProductLieGroup::TangentVector +ProductLieGroup::makeTangentVector( const typename traits::TangentVector& v1, const typename traits::TangentVector& v2, size_t firstDimension, size_t secondDimension) { @@ -346,8 +438,9 @@ ProductLieGroup::makeTangentVector( } } -template -typename ProductLieGroup::Jacobian ProductLieGroup::zeroJacobian( +template +typename ProductLieGroup::Jacobian +ProductLieGroup::zeroJacobian( size_t productDimension) { if constexpr (dimension == Eigen::Dynamic) { return Jacobian::Zero(productDimension, productDimension); @@ -357,9 +450,9 @@ typename ProductLieGroup::Jacobian ProductLieGroup::zeroJacobian( } } -template -typename ProductLieGroup::Jacobian -ProductLieGroup::identityJacobian(size_t productDimension) { +template +typename ProductLieGroup::Jacobian +ProductLieGroup::identityJacobian(size_t productDimension) { if constexpr (dimension == Eigen::Dynamic) { return Jacobian::Identity(productDimension, productDimension); } else { @@ -368,8 +461,8 @@ ProductLieGroup::identityJacobian(size_t productDimension) { } } -template -void ProductLieGroup::checkMatchingDimensions( +template +void ProductLieGroup::checkMatchingDimensions( const ProductLieGroup& other, const char* operation) const { if (firstDim() != other.firstDim() || secondDim() != other.secondDim()) { throw std::invalid_argument(std::string("ProductLieGroup::") + operation + @@ -377,8 +470,8 @@ void ProductLieGroup::checkMatchingDimensions( } } -template -void ProductLieGroup::print(const std::string& s) const { +template +void ProductLieGroup::print(const std::string& s) const { std::cout << s << "ProductLieGroup" << std::endl; traits::Print(this->first, " first"); traits::Print(this->second, " second"); diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 297652204a..d3b1de8363 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -26,16 +27,145 @@ #include #include #include +#include #include // pair #include namespace gtsam { +template +struct DirectProductAction; + +template +class ProductLieGroup; + +namespace product_lie_group_detail { + +template +struct IsStatelessLeftActionPolicy : std::false_type {}; + +template +struct IsStatelessLeftActionPolicy> + : std::bool_constant< + std::is_empty_v && std::is_default_constructible_v && + std::is_base_of_v, Action> && + Action::type == ActionType::Left && + std::is_invocable_r_v< + H, const Action&, const G&, const H&, + OptionalJacobian::dimension, traits::dimension>, + OptionalJacobian::dimension, traits::dimension>>> { +}; + +template +using ProductChartJacobian = std::conditional_t< + traits::dimension == Eigen::Dynamic || traits::dimension == Eigen::Dynamic, + OptionalJacobian, + OptionalJacobian::dimension + traits::dimension, + traits::dimension + traits::dimension>>; + +template +using ProductTangentVector = std::conditional_t< + traits::dimension == Eigen::Dynamic || traits::dimension == Eigen::Dynamic, + Vector, + Eigen::Matrix::dimension + traits::dimension, 1>>; + +template +using ProductJacobian = std::conditional_t< + traits::dimension == Eigen::Dynamic || traits::dimension == Eigen::Dynamic, + Matrix, + Eigen::Matrix::dimension + traits::dimension, + traits::dimension + traits::dimension>>; + +template +struct HasSemidirectExpmap : std::false_type {}; + +template +struct HasSemidirectExpmap< + Product, Action, G, H, + std::void_t( + std::declval::TangentVector&>(), + std::declval::TangentVector&>(), + std::declval>(), + std::declval>()))>> + : std::bool_constant( + std::declval::TangentVector&>(), + std::declval::TangentVector&>(), + std::declval>(), + std::declval>())), + Product>> {}; + +template +struct HasSemidirectLogmap : std::false_type {}; + +template +struct HasSemidirectLogmap< + Product, Action, G, H, + std::void_t( + std::declval(), + std::declval>()))>> + : std::bool_constant( + std::declval(), + std::declval>())), + ProductTangentVector>> {}; + +template +struct HasSemidirectAdjointMap : std::false_type {}; + +template +struct HasSemidirectAdjointMap< + Product, Action, G, H, + std::void_t( + std::declval()))>> + : std::bool_constant( + std::declval())), + ProductJacobian>> {}; + +} // namespace product_lie_group_detail + +template +struct DirectProductAction + : public GroupAction, G, H> { + static constexpr ActionType type = ActionType::Left; + + H operator()(const G& g, const H& h, + OptionalJacobian::dimension, traits::dimension> Hg = + {}, + OptionalJacobian::dimension, traits::dimension> Hh = + {}) const { + if (Hg) { + if constexpr (traits::dimension == Eigen::Dynamic || + traits::dimension == Eigen::Dynamic) { + Hg->setZero(static_cast(traits::GetDimension(h)), + static_cast(traits::GetDimension(g))); + } else { + Hg->setZero(); + } + } + if (Hh) { + if constexpr (traits::dimension == Eigen::Dynamic) { + const Eigen::Index hDim = + static_cast(traits::GetDimension(h)); + Hh->setIdentity(hDim, hDim); + } else { + Hh->setIdentity(); + } + } + return h; + } +}; + /** * @brief Template to construct the product Lie group of two other Lie groups * Assumes Lie group structure for G and H */ -template +template > class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsLieGroup); GTSAM_CONCEPT_ASSERT(IsLieGroup); @@ -43,15 +173,24 @@ class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsTestable); public: + using This = ProductLieGroup; + using FirstFactor = G; + using SecondFactor = H; + using ActionPolicy = Action; + /// Base pair type typedef std::pair Base; protected: + using DefaultAction = DirectProductAction; + /// Dimensions of the two subgroups inline constexpr static int dimension1 = traits::dimension; inline constexpr static int dimension2 = traits::dimension; inline constexpr static bool firstDynamic = dimension1 == Eigen::Dynamic; inline constexpr static bool secondDynamic = dimension2 == Eigen::Dynamic; + inline constexpr static bool isDirectProduct = + std::is_same_v; public: /// Manifold dimension @@ -68,6 +207,8 @@ class ProductLieGroup : public std::pair { OptionalJacobian, OptionalJacobian>; + using DynamicJacobian = OptionalJacobian; + /// Jacobian types for internal use using Jacobian = std::conditional_t { using Jacobian1 = typename traits::Jacobian; using Jacobian2 = typename traits::Jacobian; + static_assert(product_lie_group_detail::IsStatelessLeftActionPolicy< + Action, G, H>::value, + "ProductLieGroup action must be a stateless left " + "GroupAction policy on H."); public: /// @name Standard Constructors /// @{ @@ -157,8 +302,7 @@ class ProductLieGroup : public std::pair { static ProductLieGroup Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, - OptionalJacobian H1 = {}, - OptionalJacobian H2 = {}); + DynamicJacobian H1 = {}, DynamicJacobian H2 = {}); /// Logarithmic map static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}); @@ -523,9 +667,9 @@ class PowerLieGroup }; /// Traits specialization for ProductLieGroup -template -struct traits> - : internal::LieGroup> {}; +template +struct traits> + : internal::LieGroup> {}; /// Traits specialization for PowerLieGroup template diff --git a/tests/testProductLieGroup.cpp b/tests/testProductLieGroup.cpp index 78eb97a2dd..5dd3c2c38a 100644 --- a/tests/testProductLieGroup.cpp +++ b/tests/testProductLieGroup.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -32,8 +33,54 @@ using namespace gtsam; constexpr double kTol = 1e-9; using Product = ProductLieGroup; +using ProductExplicitDirect = + ProductLieGroup>; using ProductVR = ProductLieGroup; using ProductVV = ProductLieGroup; + +struct Rot3VectorAction : public GroupAction { + static constexpr ActionType type = ActionType::Left; + + Vector3 operator()(const Rot3& R, const Vector3& t, + OptionalJacobian<3, 3> HR = {}, + OptionalJacobian<3, 3> Ht = {}) const { + return R.rotate(t, HR, Ht); + } + + template + static Product Expmap( + const Eigen::Ref:: + TangentVector>& w, + const Eigen::Ref:: + TangentVector>& rho, + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) { + Matrix6 Hpose; + Vector6 xi; + xi << w, rho; + const Pose3 pose = Pose3::Expmap(xi, H1 || H2 ? &Hpose : nullptr); + if (H1) *H1 = Hpose.leftCols<3>(); + if (H2) *H2 = Hpose.rightCols<3>(); + return Product(pose.rotation(), pose.translation()); + } + + template + static typename Product::TangentVector Logmap( + const Product& p, typename Product::ChartJacobian H = {}) { + Matrix6 Hpose; + const Vector6 xi = + Pose3::Logmap(Pose3(p.first, p.second), H ? &Hpose : nullptr); + if (H) *H = Hpose; + return xi; + } + + template + static typename Product::Jacobian AdjointMap(const Product& p) { + return Pose3(p.first, p.second).AdjointMap(); + } +}; + +using Semidirect = ProductLieGroup; constexpr int kPowerComponents = 2; using Power = PowerLieGroup; using PowerTangent = Power::TangentVector; @@ -118,6 +165,37 @@ Product expmapProductProxy(const Vector5& vec) { return Product::Expmap(vec); } Vector5 logmapProductProxy(const Product& p) { return Product::Logmap(p); } +Semidirect composeSemidirectProxy(const Semidirect& A, const Semidirect& B) { + return A.compose(B); +} + +Semidirect betweenSemidirectProxy(const Semidirect& A, const Semidirect& B) { + return A.between(B); +} + +Semidirect inverseSemidirectProxy(const Semidirect& A) { return A.inverse(); } + +Semidirect expmapSemidirectProxy(const Vector6& vec) { + return Semidirect::Expmap(vec); +} + +Vector6 logmapSemidirectProxy(const Semidirect& p) { + return Semidirect::Logmap(p); +} + +Semidirect retractSemidirectProxy(const Semidirect& X, const Vector6& v) { + return X.retract(v); +} + +Vector6 localCoordinatesSemidirectProxy(const Semidirect& X, + const Semidirect& Y) { + return X.localCoordinates(Y); +} + +Pose3 asPose3(const Semidirect& state) { + return Pose3(state.first, state.second); +} + ProductVR composeProductVRProxy(const ProductVR& A, const ProductVR& B) { return A.compose(B); } @@ -530,6 +608,185 @@ TEST(Lie, Interpolate) { EXPECT(assert_equal(numericH3, actH3, kTol)); } +/* ************************************************************************* */ +TEST(Lie, ProductLieGroupExplicitDirectAction) { + const Product state(Point2(1, 2), Pose2(3, 4, 5)); + const Product other(Point2(-0.5, 0.25), Pose2(-1, 2, -0.4)); + const ProductExplicitDirect explicitState(state.first, state.second); + const ProductExplicitDirect explicitOther(other.first, other.second); + + const Product defaultComposed = state.compose(other); + const ProductExplicitDirect explicitComposed = explicitState.compose(explicitOther); + EXPECT(assert_equal(defaultComposed.first, explicitComposed.first, kTol)); + EXPECT(assert_equal(defaultComposed.second, explicitComposed.second, kTol)); + + const Product defaultBetween = state.between(other); + const ProductExplicitDirect explicitBetween = explicitState.between(explicitOther); + EXPECT(assert_equal(defaultBetween.first, explicitBetween.first, kTol)); + EXPECT(assert_equal(defaultBetween.second, explicitBetween.second, kTol)); + + const Product defaultInverse = state.inverse(); + const ProductExplicitDirect explicitInverse = explicitState.inverse(); + EXPECT(assert_equal(defaultInverse.first, explicitInverse.first, kTol)); + EXPECT(assert_equal(defaultInverse.second, explicitInverse.second, kTol)); + + Vector5 xi; + xi << 0.1, -0.2, 0.05, 0.1, -0.15; + const Product defaultExp = Product::Expmap(xi); + const ProductExplicitDirect explicitExp = ProductExplicitDirect::Expmap(xi); + EXPECT(assert_equal(defaultExp.first, explicitExp.first, kTol)); + EXPECT(assert_equal(defaultExp.second, explicitExp.second, kTol)); + EXPECT(assert_equal(Product::Logmap(defaultExp), + ProductExplicitDirect::Logmap(explicitExp), kTol)); + EXPECT(assert_equal(state.AdjointMap(), explicitState.AdjointMap(), kTol)); +} + +/* ************************************************************************* */ +TEST(Lie, ProductLieGroupSemidirect) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + const Rot3VectorAction action; + const Rot3 R1 = Rot3::RzRyRx(0.1, -0.2, 0.3); + const Rot3 R2 = Rot3::RzRyRx(-0.2, 0.05, 0.1); + const Vector3 t = Vector3(0.4, -0.5, 0.6); + EXPECT_LEFT_ACTION(action, R1, R2, t); + + const Semidirect identity; + Vector6 xi; + xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; + const Semidirect actual = identity.expmap(xi); + EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); + EXPECT(assert_equal(xi, identity.logmap(actual), kTol)); + + const Semidirect a(Rot3::RzRyRx(0.1, 0.2, -0.1), + Vector3(1.0, -2.0, 0.5)); + const Semidirect b(Rot3::RzRyRx(-0.3, 0.15, 0.2), + Vector3(-0.25, 0.4, 1.5)); + const Semidirect c(Rot3::RzRyRx(0.2, -0.1, 0.05), + Vector3(0.3, -0.6, 0.8)); + EXPECT(assert_equal(asPose3((a * b) * c), asPose3(a * (b * c)), kTol)); + EXPECT(assert_equal(asPose3(a * a.inverse()), Pose3(), kTol)); + EXPECT(assert_equal(asPose3(a * b), asPose3(a) * asPose3(b), kTol)); +} + +/* ************************************************************************* */ +TEST(testProductSemidirect, compose) { + const Semidirect state1(Rot3::RzRyRx(0.1, 0.2, -0.3), + Vector3(1.0, -0.5, 0.25)); + const Semidirect state2(Rot3::RzRyRx(-0.2, 0.1, 0.15), + Vector3(-0.75, 0.4, 1.2)); + + Matrix actH1, actH2; + const Semidirect actual = state1.compose(state2, actH1, actH2); + const Matrix numericH1 = + numericalDerivative21(composeSemidirectProxy, state1, state2); + const Matrix numericH2 = + numericalDerivative22(composeSemidirectProxy, state1, state2); + + EXPECT(assert_equal(asPose3(actual), asPose3(state1) * asPose3(state2), kTol)); + EXPECT(assert_equal(numericH1, actH1, 1e-6)); + EXPECT(assert_equal(numericH2, actH2, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testProductSemidirect, between) { + const Semidirect state1(Rot3::RzRyRx(0.1, 0.2, -0.3), + Vector3(1.0, -0.5, 0.25)); + const Semidirect state2(Rot3::RzRyRx(-0.2, 0.1, 0.15), + Vector3(-0.75, 0.4, 1.2)); + + Matrix actH1, actH2; + const Semidirect actual = state1.between(state2, actH1, actH2); + const Matrix numericH1 = + numericalDerivative21(betweenSemidirectProxy, state1, state2); + const Matrix numericH2 = + numericalDerivative22(betweenSemidirectProxy, state1, state2); + + EXPECT(assert_equal(asPose3(actual), asPose3(state1).between(asPose3(state2)), + kTol)); + EXPECT(assert_equal(numericH1, actH1, 1e-6)); + EXPECT(assert_equal(numericH2, actH2, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testProductSemidirect, inverse) { + const Semidirect state(Rot3::RzRyRx(0.2, -0.1, 0.05), + Vector3(0.5, -1.2, 0.8)); + + Matrix actH; + const Semidirect actual = state.inverse(actH); + const Matrix numericH = numericalDerivative11(inverseSemidirectProxy, state); + + EXPECT(assert_equal(asPose3(actual), asPose3(state).inverse(), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testProductSemidirect, Expmap) { + Vector6 xi; + xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; + + Matrix actH; + const Semidirect actual = Semidirect::Expmap(xi, actH); + const Matrix numericH = numericalDerivative11(expmapSemidirectProxy, xi); + + EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testProductSemidirect, Logmap) { + const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), + Vector3(0.4, -0.1, 0.2)); + + Matrix actH; + const Vector6 actual = Semidirect::Logmap(state, actH); + const Matrix numericH = numericalDerivative11(logmapSemidirectProxy, state); + + EXPECT(assert_equal(Pose3::Logmap(asPose3(state)), actual, kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testProductSemidirect, AdjointMap) { + const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), + Vector3(0.4, -0.1, 0.2)); + + EXPECT(assert_equal(asPose3(state).AdjointMap(), state.AdjointMap(), kTol)); +} + +/* ************************************************************************* */ +TEST(testProductSemidirect, retractAndLocalCoordinates) { + const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), + Vector3(0.4, -0.1, 0.2)); + Vector6 delta; + delta << 0.05, -0.04, 0.03, 0.1, -0.2, 0.05; + + Matrix retractH1, retractH2, localH1, localH2; + const Semidirect updated = state.retract(delta, retractH1, retractH2); + const Vector6 recovered = + state.localCoordinates(updated, localH1, localH2); + + EXPECT(assert_equal(asPose3(updated), asPose3(state).retract(delta), kTol)); + EXPECT(assert_equal(delta, recovered, kTol)); + + const Matrix numericRetractH1 = + numericalDerivative21(retractSemidirectProxy, state, delta); + const Matrix numericRetractH2 = + numericalDerivative22(retractSemidirectProxy, state, delta); + const Matrix numericLocalH1 = + numericalDerivative21(localCoordinatesSemidirectProxy, state, updated); + const Matrix numericLocalH2 = + numericalDerivative22(localCoordinatesSemidirectProxy, state, updated); + + EXPECT(assert_equal(numericRetractH1, retractH1, 1e-6)); + EXPECT(assert_equal(numericRetractH2, retractH2, 1e-6)); + EXPECT(assert_equal(numericLocalH1, localH1, 1e-6)); + EXPECT(assert_equal(numericLocalH2, localH2, 1e-6)); +} + /* ************************************************************************* */ TEST(Lie, PowerLieGroup) { GTSAM_CONCEPT_ASSERT(IsGroup); From 6f211812fea161605f4a746ed42ea48ad78f3025 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 02:18:24 -0400 Subject: [PATCH 02/21] [skip ci] make separate class --- gtsam/base/ActionProductLieGroup-inl.h | 281 +++++++++++++ gtsam/base/ActionProductLieGroup.h | 282 +++++++++++++ gtsam/base/ProductLieGroup-inl.h | 531 ++++++++++--------------- gtsam/base/ProductLieGroup.h | 157 +------- tests/testActionProductLieGroup.cpp | 304 ++++++++++++++ tests/testProductLieGroup.cpp | 257 ------------ 6 files changed, 1092 insertions(+), 720 deletions(-) create mode 100644 gtsam/base/ActionProductLieGroup-inl.h create mode 100644 gtsam/base/ActionProductLieGroup.h create mode 100644 tests/testActionProductLieGroup.cpp diff --git a/gtsam/base/ActionProductLieGroup-inl.h b/gtsam/base/ActionProductLieGroup-inl.h new file mode 100644 index 0000000000..b4f0a63141 --- /dev/null +++ b/gtsam/base/ActionProductLieGroup-inl.h @@ -0,0 +1,281 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ActionProductLieGroup-inl.h + * @date April, 2026 + * @author Frank Dellaert + * @brief Internals for ActionProductLieGroup.h, not for general consumption + */ + +#pragma once + +namespace gtsam { + +template +ActionProductLieGroup +ActionProductLieGroup::operator*(const This& other) const { + checkMatchingDimensions(other, "operator*"); + if constexpr (isDirectProduct) { + return This(Base::operator*(other)); + } else { + const Action action{}; + const H actedSecond = action(this->first, other.second); + return This(traits::Compose(this->first, other.first), + traits::Compose(this->second, actedSecond)); + } +} + +template +ActionProductLieGroup +ActionProductLieGroup::inverse() const { + if constexpr (isDirectProduct) { + return This(Base::inverse()); + } else { + const Action action{}; + const G gInv = traits::Inverse(this->first); + const H hInv = traits::Inverse(this->second); + return This(gInv, action(gInv, hInv)); + } +} + +template +ActionProductLieGroup +ActionProductLieGroup::retract(const TangentVector& v, + ChartJacobian H1, + ChartJacobian H2) const { + if constexpr (isDirectProduct) { + return This(Base::retract(v, H1, H2)); + } else { + const size_t productDimension = + combinedDimension(this->firstDim(), this->secondDim()); + if (static_cast(v.size()) != productDimension) { + throw std::invalid_argument( + "ActionProductLieGroup::retract tangent dimension does not match " + "product dimension"); + } + Jacobian D_g_v; + This g = + This::Expmap(v, H2 ? OptionalJacobian(&D_g_v) + : ChartJacobian()); + This h = compose(g); + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = D_g_v; + return h; + } +} + +template +typename ActionProductLieGroup::TangentVector +ActionProductLieGroup::localCoordinates(const This& g, + ChartJacobian H1, + ChartJacobian H2) const { + checkMatchingDimensions(g, "localCoordinates"); + if constexpr (isDirectProduct) { + return Base::localCoordinates(g, H1, H2); + } else { + This h = between(g); + Jacobian D_v_h; + TangentVector v = This::Logmap( + h, H1 || H2 ? OptionalJacobian(&D_v_h) + : ChartJacobian()); + if (H1) *H1 = -D_v_h * h.inverse().AdjointMap(); + if (H2) *H2 = D_v_h; + return v; + } +} + +template +ActionProductLieGroup +ActionProductLieGroup::compose(const This& other, + ChartJacobian H1, + ChartJacobian H2) const { + checkMatchingDimensions(other, "compose"); + if constexpr (isDirectProduct) { + return This(Base::compose(other, H1, H2)); + } else { + const size_t productDimension = + combinedDimension(this->firstDim(), this->secondDim()); + This result = (*this) * other; + if (H1) *H1 = other.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(productDimension); + return result; + } +} + +template +ActionProductLieGroup +ActionProductLieGroup::between(const This& other, + ChartJacobian H1, + ChartJacobian H2) const { + checkMatchingDimensions(other, "between"); + if constexpr (isDirectProduct) { + return This(Base::between(other, H1, H2)); + } else { + const size_t productDimension = + combinedDimension(this->firstDim(), this->secondDim()); + This result = this->inverse() * other; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(productDimension); + return result; + } +} + +template +ActionProductLieGroup +ActionProductLieGroup::inverse(ChartJacobian D) const { + if constexpr (isDirectProduct) { + return This(Base::inverse(D)); + } else { + if (D) *D = -AdjointMap(); + return inverse(); + } +} + +template +ActionProductLieGroup +ActionProductLieGroup::Expmap(const TangentVector& v, + ChartJacobian Hv) { + if constexpr (isDirectProduct) { + return This(Base::Expmap(v, Hv)); + } else { + size_t firstDimension = 0; + size_t secondDimension = 0; + if constexpr (firstDynamic && secondDynamic) { + if (v.size() == 0) { + if (Hv) *Hv = Matrix::Zero(0, 0); + return This(); + } + throw std::invalid_argument( + "ActionProductLieGroup::Expmap requires split tangent vectors when " + "both factors are dynamic"); + } else if constexpr (firstDynamic) { + if (v.size() < dimension2) { + throw std::invalid_argument( + "ActionProductLieGroup::Expmap tangent dimension is too small for " + "the fixed second factor"); + } + firstDimension = static_cast(v.size() - dimension2); + secondDimension = static_cast(dimension2); + } else if constexpr (secondDynamic) { + if (v.size() < dimension1) { + throw std::invalid_argument( + "ActionProductLieGroup::Expmap tangent dimension is too small for " + "the fixed first factor"); + } + firstDimension = static_cast(dimension1); + secondDimension = static_cast(v.size() - dimension1); + } else { + firstDimension = static_cast(dimension1); + secondDimension = static_cast(dimension2); + } + + if (static_cast(v.size()) != + combinedDimension(firstDimension, secondDimension)) { + throw std::invalid_argument( + "ActionProductLieGroup::Expmap tangent dimension does not match " + "product dimension"); + } + + Matrix D_g_first; + Matrix D_h_second; + const auto v1 = tangentSegment(v, 0, firstDimension); + const auto v2 = tangentSegment(v, firstDimension, secondDimension); + This result = Expmap(v1, v2, Hv ? DynamicJacobian(D_g_first) + : DynamicJacobian(), + Hv ? DynamicJacobian(D_h_second) + : DynamicJacobian()); + if (Hv) { + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + *Hv = zeroJacobian(productDimension); + Hv->block(0, 0, productDimension, firstDimension) = D_g_first; + Hv->block(0, firstDimension, productDimension, secondDimension) = + D_h_second; + } + return result; + } +} + +template +ActionProductLieGroup +ActionProductLieGroup::Expmap( + const Eigen::Ref::TangentVector>& v1, + const Eigen::Ref::TangentVector>& v2, + DynamicJacobian H1, DynamicJacobian H2) { + if constexpr (isDirectProduct) { + return This(Base::Expmap(v1, v2, H1, H2)); + } else { + static_assert(action_product_lie_group_detail::HasSemidirectExpmap< + This, Action, G, H>::value, + "ActionProductLieGroup semidirect actions must provide " + "template static Product Expmap(v1, v2, H1, " + "H2)."); + return Action::template Expmap(v1, v2, H1, H2); + } +} + +template +typename ActionProductLieGroup::TangentVector +ActionProductLieGroup::Logmap(const This& p, + ChartJacobian Hp) { + if constexpr (isDirectProduct) { + return Base::Logmap(p, Hp); + } else { + static_assert(action_product_lie_group_detail::HasSemidirectLogmap< + This, Action, G, H>::value, + "ActionProductLieGroup semidirect actions must provide " + "template static typename Product::" + "TangentVector Logmap(const Product&, ChartJacobian)."); + return Action::template Logmap(p, Hp); + } +} + +template +ActionProductLieGroup +ActionProductLieGroup::expmap(const TangentVector& v) const { + return compose(This::Expmap(v)); +} + +template +typename ActionProductLieGroup::Jacobian +ActionProductLieGroup::AdjointMap() const { + if constexpr (isDirectProduct) { + return Base::AdjointMap(); + } else { + static_assert(action_product_lie_group_detail::HasSemidirectAdjointMap< + This, Action, G, H>::value, + "ActionProductLieGroup semidirect actions must provide " + "template static typename Product::Jacobian " + "AdjointMap(const Product&)."); + return Action::template AdjointMap(*this); + } +} + +template +void ActionProductLieGroup::checkMatchingDimensions( + const This& other, const char* operation) const { + if (this->firstDim() != other.firstDim() || + this->secondDim() != other.secondDim()) { + throw std::invalid_argument(std::string("ActionProductLieGroup::") + + operation + + " requires matching component dimensions"); + } +} + +template +void ActionProductLieGroup::print(const std::string& s) const { + std::cout << s << "ActionProductLieGroup" << std::endl; + traits::Print(this->first, " first"); + traits::Print(this->second, " second"); +} + +} // namespace gtsam diff --git a/gtsam/base/ActionProductLieGroup.h b/gtsam/base/ActionProductLieGroup.h new file mode 100644 index 0000000000..0b1bee0c8d --- /dev/null +++ b/gtsam/base/ActionProductLieGroup.h @@ -0,0 +1,282 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ActionProductLieGroup.h + * @date April, 2026 + * @author Frank Dellaert + * @brief Product Lie group parameterized by a left group action + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +template +struct DirectProductAction; + +namespace action_product_lie_group_detail { + +template +struct IsStatelessLeftActionPolicy : std::false_type {}; + +template +struct IsStatelessLeftActionPolicy> + : std::bool_constant< + std::is_empty_v && std::is_default_constructible_v && + std::is_base_of_v, Action> && + Action::type == ActionType::Left && + std::is_invocable_r_v< + H, const Action&, const G&, const H&, + OptionalJacobian::dimension, traits::dimension>, + OptionalJacobian::dimension, traits::dimension>>> { +}; + +template +using ProductChartJacobian = std::conditional_t< + traits::dimension == Eigen::Dynamic || + traits::dimension == Eigen::Dynamic, + OptionalJacobian, + OptionalJacobian::dimension + traits::dimension, + traits::dimension + traits::dimension>>; + +template +using ProductTangentVector = std::conditional_t< + traits::dimension == Eigen::Dynamic || + traits::dimension == Eigen::Dynamic, + Vector, + Eigen::Matrix::dimension + traits::dimension, 1>>; + +template +using ProductJacobian = std::conditional_t< + traits::dimension == Eigen::Dynamic || + traits::dimension == Eigen::Dynamic, + Matrix, + Eigen::Matrix::dimension + traits::dimension, + traits::dimension + traits::dimension>>; + +template +struct HasSemidirectExpmap : std::false_type {}; + +template +struct HasSemidirectExpmap< + Product, Action, G, H, + std::void_t( + std::declval::TangentVector&>(), + std::declval::TangentVector&>(), + std::declval>(), + std::declval>()))>> + : std::bool_constant( + std::declval::TangentVector&>(), + std::declval::TangentVector&>(), + std::declval>(), + std::declval>())), + Product>> {}; + +template +struct HasSemidirectLogmap : std::false_type {}; + +template +struct HasSemidirectLogmap< + Product, Action, G, H, + std::void_t( + std::declval(), + std::declval>()))>> + : std::bool_constant( + std::declval(), + std::declval>())), + ProductTangentVector>> {}; + +template +struct HasSemidirectAdjointMap : std::false_type {}; + +template +struct HasSemidirectAdjointMap< + Product, Action, G, H, + std::void_t( + std::declval()))>> + : std::bool_constant( + std::declval())), + ProductJacobian>> {}; + +} // namespace action_product_lie_group_detail + +template +struct DirectProductAction + : public GroupAction, G, H> { + static constexpr ActionType type = ActionType::Left; + + H operator()(const G& g, const H& h, + OptionalJacobian::dimension, traits::dimension> Hg = + {}, + OptionalJacobian::dimension, traits::dimension> Hh = + {}) const { + if (Hg) { + if constexpr (traits::dimension == Eigen::Dynamic || + traits::dimension == Eigen::Dynamic) { + Hg->setZero(static_cast(traits::GetDimension(h)), + static_cast(traits::GetDimension(g))); + } else { + Hg->setZero(); + } + } + if (Hh) { + if constexpr (traits::dimension == Eigen::Dynamic) { + const Eigen::Index hDim = + static_cast(traits::GetDimension(h)); + Hh->setIdentity(hDim, hDim); + } else { + Hh->setIdentity(); + } + } + return h; + } +}; + +/** + * @brief Product Lie group extended with an optional left action. + * If Action is omitted the group reduces to the direct product G x H. + * If Action is provided the group becomes the semidirect product G ⋉ H. + */ +template > +class ActionProductLieGroup : public ProductLieGroup { + GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsTestable); + GTSAM_CONCEPT_ASSERT(IsTestable); + + public: + using This = ActionProductLieGroup; + using FirstFactor = G; + using SecondFactor = H; + using ActionPolicy = Action; + using Base = ProductLieGroup; + using PairBase = typename Base::Base; + using typename Base::ChartJacobian; + using typename Base::Jacobian; + using typename Base::Jacobian1; + using typename Base::Jacobian2; + using typename Base::TangentVector; + + protected: + inline constexpr static int dimension1 = Base::dimension1; + inline constexpr static int dimension2 = Base::dimension2; + inline constexpr static bool firstDynamic = Base::firstDynamic; + inline constexpr static bool secondDynamic = Base::secondDynamic; + inline constexpr static bool isDirectProduct = + std::is_same_v>; + + public: + inline constexpr static int dimension = Base::dimension; + inline constexpr static int manifoldDimension = dimension; + using DynamicJacobian = OptionalJacobian; + + static_assert(action_product_lie_group_detail::IsStatelessLeftActionPolicy< + Action, G, H>::value, + "ActionProductLieGroup action must be a stateless left " + "GroupAction policy on H."); + + typedef multiplicative_group_tag group_flavor; + + ActionProductLieGroup() : Base() {} + ActionProductLieGroup(const G& g, const H& h) : Base(g, h) {} + ActionProductLieGroup(const PairBase& base) : Base(base) {} + ActionProductLieGroup(const Base& base) : Base(base) {} + + static This Identity() { return This(); } + static constexpr int Dim() { return manifoldDimension; } + size_t dim() const { return Base::dim(); } + + This operator*(const This& other) const; + This inverse() const; + + This compose(const This& g) const { return (*this) * g; } + This between(const This& g) const { return this->inverse() * g; } + + This retract(const TangentVector& v, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const; + TangentVector localCoordinates(const This& g, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const; + + This compose(const This& other, ChartJacobian H1, + ChartJacobian H2 = {}) const; + This between(const This& other, ChartJacobian H1, + ChartJacobian H2 = {}) const; + This inverse(ChartJacobian D) const; + + static This Expmap(const TangentVector& v, ChartJacobian Hv = {}); + static This Expmap( + const Eigen::Ref::TangentVector>& v1, + const Eigen::Ref::TangentVector>& v2, + DynamicJacobian H1 = {}, DynamicJacobian H2 = {}); + static TangentVector Logmap(const This& p, ChartJacobian Hp = {}); + static TangentVector LocalCoordinates(const This& p, ChartJacobian Hp = {}) { + return Logmap(p, Hp); + } + + This expmap(const TangentVector& v) const; + TangentVector logmap(const This& g) const { return This::Logmap(between(g)); } + Jacobian AdjointMap() const; + + void print(const std::string& s = "") const; + bool equals(const This& other, double tol = 1e-9) const { + return Base::equals(other, tol); + } + + protected: + static size_t combinedDimension(size_t d1, size_t d2) { + return Base::combinedDimension(d1, d2); + } + + template ::dimension> + static typename traits::TangentVector tangentSegment( + const TangentVector& v, size_t start, size_t runtimeDimension) { + return Base::template tangentSegment(v, start, runtimeDimension); + } + + static TangentVector makeTangentVector( + const typename traits::TangentVector& v1, + const typename traits::TangentVector& v2, size_t firstDimension, + size_t secondDimension) { + return Base::makeTangentVector(v1, v2, firstDimension, secondDimension); + } + + static Jacobian zeroJacobian(size_t productDimension) { + return Base::zeroJacobian(productDimension); + } + + static Jacobian identityJacobian(size_t productDimension) { + return Base::identityJacobian(productDimension); + } + + void checkMatchingDimensions(const This& other, const char* operation) const; +}; + +template +struct traits> + : internal::LieGroup> {}; + +} // namespace gtsam + +#include diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 276eeb24f9..42a0681459 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -20,224 +20,162 @@ namespace gtsam { -template -ProductLieGroup ProductLieGroup::operator*( +template +ProductLieGroup ProductLieGroup::operator*( const ProductLieGroup& other) const { checkMatchingDimensions(other, "operator*"); - if constexpr (isDirectProduct) { - return ProductLieGroup(traits::Compose(this->first, other.first), - traits::Compose(this->second, other.second)); - } else { - const Action action{}; - const H actedSecond = action(this->first, other.second); - return ProductLieGroup(traits::Compose(this->first, other.first), - traits::Compose(this->second, actedSecond)); - } + return ProductLieGroup(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); } -template -ProductLieGroup ProductLieGroup::inverse() const { - if constexpr (isDirectProduct) { - return ProductLieGroup(traits::Inverse(this->first), - traits::Inverse(this->second)); - } else { - const Action action{}; - const G gInv = traits::Inverse(this->first); - const H hInv = traits::Inverse(this->second); - return ProductLieGroup(gInv, action(gInv, hInv)); - } +template +ProductLieGroup ProductLieGroup::inverse() const { + return ProductLieGroup(traits::Inverse(this->first), + traits::Inverse(this->second)); } -template -ProductLieGroup ProductLieGroup::retract( - const TangentVector& v, ChartJacobian H1, ChartJacobian H2) const { - if constexpr (isDirectProduct) { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - if (static_cast(v.size()) != productDimension) { - throw std::invalid_argument( - "ProductLieGroup::retract tangent dimension does not match product " - "dimension"); - } - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - G g = traits::Retract(this->first, - tangentSegment(v, 0, firstDimension), - H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - H h = traits::Retract( - this->second, tangentSegment(v, firstDimension, secondDimension), - H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return ProductLieGroup(g, h); - } else { - const size_t productDimension = - combinedDimension(firstDim(), secondDim()); - if (static_cast(v.size()) != productDimension) { - throw std::invalid_argument( - "ProductLieGroup::retract tangent dimension does not match product " - "dimension"); - } - Jacobian D_g_v; - ProductLieGroup g = - ProductLieGroup::Expmap(v, H2 ? OptionalJacobian(&D_g_v) - : ChartJacobian()); - ProductLieGroup h = compose(g); - if (H1) *H1 = g.inverse().AdjointMap(); - if (H2) *H2 = D_g_v; - return h; - } -} - -template -typename ProductLieGroup::TangentVector -ProductLieGroup::localCoordinates(const ProductLieGroup& g, - ChartJacobian H1, - ChartJacobian H2) const { - checkMatchingDimensions(g, "localCoordinates"); - if constexpr (isDirectProduct) { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - typename traits::TangentVector v2 = traits::Local( - this->second, g.second, H1 ? &D_h_first : nullptr, - H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return makeTangentVector(v1, v2, firstDimension, secondDimension); - } else { - ProductLieGroup h = between(g); - Jacobian D_v_h; - TangentVector v = ProductLieGroup::Logmap( - h, H1 || H2 ? OptionalJacobian(&D_v_h) - : ChartJacobian()); - if (H1) *H1 = -D_v_h * h.inverse().AdjointMap(); - if (H2) *H2 = D_v_h; - return v; +template +ProductLieGroup ProductLieGroup::retract(const TangentVector& v, + ChartJacobian H1, + ChartJacobian H2) const { + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + if (static_cast(v.size()) != + productDimension) { + throw std::invalid_argument( + "ProductLieGroup::retract tangent dimension does not match product " + "dimension"); } -} - -template -ProductLieGroup ProductLieGroup::compose( + Jacobian1 D_g_first; + Jacobian1 D_g_second; + Jacobian2 D_h_first; + Jacobian2 D_h_second; + G g = traits::Retract(this->first, tangentSegment(v, 0, firstDimension), + H1 ? &D_g_first : nullptr, + H2 ? &D_g_second : nullptr); + H h = traits::Retract( + this->second, tangentSegment(v, firstDimension, secondDimension), + H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_first; + } + if (H2) { + *H2 = zeroJacobian(productDimension); + H2->block(0, 0, firstDimension, firstDimension) = D_g_second; + H2->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return ProductLieGroup(g, h); +} + +template +typename ProductLieGroup::TangentVector +ProductLieGroup::localCoordinates(const ProductLieGroup& g, + ChartJacobian H1, + ChartJacobian H2) const { + checkMatchingDimensions(g, "localCoordinates"); + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian1 D_g_second; + Jacobian2 D_h_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, + H2 ? &D_g_second : nullptr); + typename traits::TangentVector v2 = traits::Local( + this->second, g.second, H1 ? &D_h_first : nullptr, + H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_first; + } + if (H2) { + *H2 = zeroJacobian(productDimension); + H2->block(0, 0, firstDimension, firstDimension) = D_g_second; + H2->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return makeTangentVector(v1, v2, firstDimension, secondDimension); +} + +template +ProductLieGroup ProductLieGroup::compose( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "compose"); - const size_t productDimension = combinedDimension(firstDim(), secondDim()); - if constexpr (isDirectProduct) { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Compose(this->first, other.first, - H1 ? &D_g_first : nullptr); - H h = traits::Compose(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); - } else { - ProductLieGroup result = (*this) * other; - if (H1) *H1 = other.inverse().AdjointMap(); - if (H2) *H2 = identityJacobian(productDimension); - return result; - } -} - -template -ProductLieGroup ProductLieGroup::between( + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Compose(this->first, other.first, H1 ? &D_g_first : nullptr); + H h = traits::Compose(this->second, other.second, + H1 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + if (H2) *H2 = identityJacobian(productDimension); + return ProductLieGroup(g, h); +} + +template +ProductLieGroup ProductLieGroup::between( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "between"); - const size_t productDimension = combinedDimension(firstDim(), secondDim()); - if constexpr (isDirectProduct) { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Between(this->first, other.first, - H1 ? &D_g_first : nullptr); - H h = traits::Between(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); - } else { - ProductLieGroup result = this->inverse() * other; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = identityJacobian(productDimension); - return result; - } -} - -template -ProductLieGroup ProductLieGroup::inverse( - ChartJacobian D) const { - if constexpr (isDirectProduct) { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Inverse(this->first, D ? &D_g_first : nullptr); - H h = traits::Inverse(this->second, D ? &D_h_second : nullptr); - if (D) { - *D = zeroJacobian(productDimension); - D->block(0, 0, firstDimension, firstDimension) = D_g_first; - D->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return ProductLieGroup(g, h); - } else { - if (D) *D = -AdjointMap(); - return inverse(); + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Between(this->first, other.first, H1 ? &D_g_first : nullptr); + H h = traits::Between(this->second, other.second, + H1 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + if (H2) *H2 = identityJacobian(productDimension); + return ProductLieGroup(g, h); +} + +template +ProductLieGroup ProductLieGroup::inverse(ChartJacobian D) const { + const size_t firstDimension = firstDim(); + const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Inverse(this->first, D ? &D_g_first : nullptr); + H h = traits::Inverse(this->second, D ? &D_h_second : nullptr); + if (D) { + *D = zeroJacobian(productDimension); + D->block(0, 0, firstDimension, firstDimension) = D_g_first; + D->block(firstDimension, firstDimension, secondDimension, secondDimension) = + D_h_second; } + return ProductLieGroup(g, h); } -template -ProductLieGroup ProductLieGroup::Expmap( - const TangentVector& v, ChartJacobian Hv) { +template +ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, + ChartJacobian Hv) { size_t firstDimension = 0; size_t secondDimension = 0; if constexpr (firstDynamic && secondDynamic) { @@ -278,10 +216,12 @@ ProductLieGroup ProductLieGroup::Expmap( Matrix D_h_second; const auto v1 = tangentSegment(v, 0, firstDimension); const auto v2 = tangentSegment(v, firstDimension, secondDimension); - ProductLieGroup result = Expmap(v1, v2, Hv ? DynamicJacobian(D_g_first) - : DynamicJacobian(), - Hv ? DynamicJacobian(D_h_second) - : DynamicJacobian()); + ProductLieGroup result = + Expmap(v1, v2, + Hv ? OptionalJacobian(D_g_first) + : OptionalJacobian(), + Hv ? OptionalJacobian(D_h_second) + : OptionalJacobian()); if (Hv) { const size_t productDimension = combinedDimension(firstDimension, secondDimension); @@ -293,107 +233,76 @@ ProductLieGroup ProductLieGroup::Expmap( return result; } -template -ProductLieGroup ProductLieGroup::Expmap( +template +ProductLieGroup ProductLieGroup::Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, - DynamicJacobian H1, DynamicJacobian H2) { - if constexpr (isDirectProduct) { - const size_t firstDimension = static_cast(v1.size()); - const size_t secondDimension = static_cast(v2.size()); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); - H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = Matrix::Zero(productDimension, firstDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - } - if (H2) { - *H2 = Matrix::Zero(productDimension, secondDimension); - H2->block(firstDimension, 0, secondDimension, secondDimension) = - D_h_second; - } - return ProductLieGroup(g, h); - } else { - static_assert( - product_lie_group_detail::HasSemidirectExpmap::value, - "Semidirect ProductLieGroup actions must provide " - "template static Product Expmap(v1, v2, H1, H2)."); - return Action::template Expmap(v1, v2, H1, H2); - } -} - -template -typename ProductLieGroup::TangentVector -ProductLieGroup::Logmap( + OptionalJacobian H1, + OptionalJacobian H2) { + const size_t firstDimension = static_cast(v1.size()); + const size_t secondDimension = static_cast(v2.size()); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); + H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = Matrix::Zero(productDimension, firstDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + } + if (H2) { + *H2 = Matrix::Zero(productDimension, secondDimension); + H2->block(firstDimension, 0, secondDimension, secondDimension) = D_h_second; + } + return ProductLieGroup(g, h); +} + +template +typename ProductLieGroup::TangentVector ProductLieGroup::Logmap( const ProductLieGroup& p, ChartJacobian Hp) { - if constexpr (isDirectProduct) { - const size_t firstDimension = p.firstDim(); - const size_t secondDimension = p.secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Logmap(p.first, Hp ? &D_g_first : nullptr); - typename traits::TangentVector v2 = - traits::Logmap(p.second, Hp ? &D_h_second : nullptr); - TangentVector v = - makeTangentVector(v1, v2, firstDimension, secondDimension); - if (Hp) { - *Hp = zeroJacobian(productDimension); - Hp->block(0, 0, firstDimension, firstDimension) = D_g_first; - Hp->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return v; - } else { - static_assert( - product_lie_group_detail::HasSemidirectLogmap::value, - "Semidirect ProductLieGroup actions must provide " - "template static typename Product::TangentVector " - "Logmap(const Product&, ChartJacobian)."); - return Action::template Logmap(p, Hp); + const size_t firstDimension = p.firstDim(); + const size_t secondDimension = p.secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Logmap(p.first, Hp ? &D_g_first : nullptr); + typename traits::TangentVector v2 = + traits::Logmap(p.second, Hp ? &D_h_second : nullptr); + TangentVector v = makeTangentVector(v1, v2, firstDimension, secondDimension); + if (Hp) { + *Hp = zeroJacobian(productDimension); + Hp->block(0, 0, firstDimension, firstDimension) = D_g_first; + Hp->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; } + return v; } -template -ProductLieGroup ProductLieGroup::expmap( +template +ProductLieGroup ProductLieGroup::expmap( const TangentVector& v) const { return compose(ProductLieGroup::Expmap(v)); } -template -typename ProductLieGroup::Jacobian -ProductLieGroup::AdjointMap() const { - if constexpr (isDirectProduct) { - const auto adjG = traits::AdjointMap(this->first); - const auto adjH = traits::AdjointMap(this->second); - const size_t d1 = static_cast(adjG.rows()); - const size_t d2 = static_cast(adjH.rows()); - Jacobian adj = zeroJacobian(d1 + d2); - adj.block(0, 0, d1, d1) = adjG; - adj.block(d1, d1, d2, d2) = adjH; - return adj; - } else { - static_assert( - product_lie_group_detail::HasSemidirectAdjointMap::value, - "Semidirect ProductLieGroup actions must provide " - "template static typename Product::Jacobian " - "AdjointMap(const Product&)."); - return Action::template AdjointMap(*this); - } +template +typename ProductLieGroup::Jacobian ProductLieGroup::AdjointMap() + const { + const auto adjG = traits::AdjointMap(this->first); + const auto adjH = traits::AdjointMap(this->second); + const size_t d1 = static_cast(adjG.rows()); + const size_t d2 = static_cast(adjH.rows()); + Jacobian adj = zeroJacobian(d1 + d2); + adj.block(0, 0, d1, d1) = adjG; + adj.block(d1, d1, d2, d2) = adjH; + return adj; } -template +template template -T ProductLieGroup::defaultIdentity() { +T ProductLieGroup::defaultIdentity() { if constexpr (traits::dimension == Eigen::Dynamic) { return T(); } else { @@ -401,10 +310,9 @@ T ProductLieGroup::defaultIdentity() { } } -template +template template -typename traits::TangentVector -ProductLieGroup::tangentSegment( +typename traits::TangentVector ProductLieGroup::tangentSegment( const TangentVector& v, size_t start, size_t runtimeDimension) { const int startIndex = static_cast(start); const int runtimeIndex = static_cast(runtimeDimension); @@ -416,9 +324,9 @@ ProductLieGroup::tangentSegment( } } -template -typename ProductLieGroup::TangentVector -ProductLieGroup::makeTangentVector( +template +typename ProductLieGroup::TangentVector +ProductLieGroup::makeTangentVector( const typename traits::TangentVector& v1, const typename traits::TangentVector& v2, size_t firstDimension, size_t secondDimension) { @@ -438,9 +346,8 @@ ProductLieGroup::makeTangentVector( } } -template -typename ProductLieGroup::Jacobian -ProductLieGroup::zeroJacobian( +template +typename ProductLieGroup::Jacobian ProductLieGroup::zeroJacobian( size_t productDimension) { if constexpr (dimension == Eigen::Dynamic) { return Jacobian::Zero(productDimension, productDimension); @@ -450,9 +357,9 @@ ProductLieGroup::zeroJacobian( } } -template -typename ProductLieGroup::Jacobian -ProductLieGroup::identityJacobian(size_t productDimension) { +template +typename ProductLieGroup::Jacobian +ProductLieGroup::identityJacobian(size_t productDimension) { if constexpr (dimension == Eigen::Dynamic) { return Jacobian::Identity(productDimension, productDimension); } else { @@ -461,8 +368,8 @@ ProductLieGroup::identityJacobian(size_t productDimension) { } } -template -void ProductLieGroup::checkMatchingDimensions( +template +void ProductLieGroup::checkMatchingDimensions( const ProductLieGroup& other, const char* operation) const { if (firstDim() != other.firstDim() || secondDim() != other.secondDim()) { throw std::invalid_argument(std::string("ProductLieGroup::") + operation + @@ -470,8 +377,8 @@ void ProductLieGroup::checkMatchingDimensions( } } -template -void ProductLieGroup::print(const std::string& s) const { +template +void ProductLieGroup::print(const std::string& s) const { std::cout << s << "ProductLieGroup" << std::endl; traits::Print(this->first, " first"); traits::Print(this->second, " second"); diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index d3b1de8363..450079da9b 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include @@ -27,145 +26,16 @@ #include #include #include -#include #include // pair #include namespace gtsam { -template -struct DirectProductAction; - -template -class ProductLieGroup; - -namespace product_lie_group_detail { - -template -struct IsStatelessLeftActionPolicy : std::false_type {}; - -template -struct IsStatelessLeftActionPolicy> - : std::bool_constant< - std::is_empty_v && std::is_default_constructible_v && - std::is_base_of_v, Action> && - Action::type == ActionType::Left && - std::is_invocable_r_v< - H, const Action&, const G&, const H&, - OptionalJacobian::dimension, traits::dimension>, - OptionalJacobian::dimension, traits::dimension>>> { -}; - -template -using ProductChartJacobian = std::conditional_t< - traits::dimension == Eigen::Dynamic || traits::dimension == Eigen::Dynamic, - OptionalJacobian, - OptionalJacobian::dimension + traits::dimension, - traits::dimension + traits::dimension>>; - -template -using ProductTangentVector = std::conditional_t< - traits::dimension == Eigen::Dynamic || traits::dimension == Eigen::Dynamic, - Vector, - Eigen::Matrix::dimension + traits::dimension, 1>>; - -template -using ProductJacobian = std::conditional_t< - traits::dimension == Eigen::Dynamic || traits::dimension == Eigen::Dynamic, - Matrix, - Eigen::Matrix::dimension + traits::dimension, - traits::dimension + traits::dimension>>; - -template -struct HasSemidirectExpmap : std::false_type {}; - -template -struct HasSemidirectExpmap< - Product, Action, G, H, - std::void_t( - std::declval::TangentVector&>(), - std::declval::TangentVector&>(), - std::declval>(), - std::declval>()))>> - : std::bool_constant( - std::declval::TangentVector&>(), - std::declval::TangentVector&>(), - std::declval>(), - std::declval>())), - Product>> {}; - -template -struct HasSemidirectLogmap : std::false_type {}; - -template -struct HasSemidirectLogmap< - Product, Action, G, H, - std::void_t( - std::declval(), - std::declval>()))>> - : std::bool_constant( - std::declval(), - std::declval>())), - ProductTangentVector>> {}; - -template -struct HasSemidirectAdjointMap : std::false_type {}; - -template -struct HasSemidirectAdjointMap< - Product, Action, G, H, - std::void_t( - std::declval()))>> - : std::bool_constant( - std::declval())), - ProductJacobian>> {}; - -} // namespace product_lie_group_detail - -template -struct DirectProductAction - : public GroupAction, G, H> { - static constexpr ActionType type = ActionType::Left; - - H operator()(const G& g, const H& h, - OptionalJacobian::dimension, traits::dimension> Hg = - {}, - OptionalJacobian::dimension, traits::dimension> Hh = - {}) const { - if (Hg) { - if constexpr (traits::dimension == Eigen::Dynamic || - traits::dimension == Eigen::Dynamic) { - Hg->setZero(static_cast(traits::GetDimension(h)), - static_cast(traits::GetDimension(g))); - } else { - Hg->setZero(); - } - } - if (Hh) { - if constexpr (traits::dimension == Eigen::Dynamic) { - const Eigen::Index hDim = - static_cast(traits::GetDimension(h)); - Hh->setIdentity(hDim, hDim); - } else { - Hh->setIdentity(); - } - } - return h; - } -}; - /** * @brief Template to construct the product Lie group of two other Lie groups * Assumes Lie group structure for G and H */ -template > +template class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsLieGroup); GTSAM_CONCEPT_ASSERT(IsLieGroup); @@ -173,24 +43,15 @@ class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsTestable); public: - using This = ProductLieGroup; - using FirstFactor = G; - using SecondFactor = H; - using ActionPolicy = Action; - /// Base pair type typedef std::pair Base; protected: - using DefaultAction = DirectProductAction; - /// Dimensions of the two subgroups inline constexpr static int dimension1 = traits::dimension; inline constexpr static int dimension2 = traits::dimension; inline constexpr static bool firstDynamic = dimension1 == Eigen::Dynamic; inline constexpr static bool secondDynamic = dimension2 == Eigen::Dynamic; - inline constexpr static bool isDirectProduct = - std::is_same_v; public: /// Manifold dimension @@ -207,19 +68,12 @@ class ProductLieGroup : public std::pair { OptionalJacobian, OptionalJacobian>; - using DynamicJacobian = OptionalJacobian; - /// Jacobian types for internal use using Jacobian = std::conditional_t>; using Jacobian1 = typename traits::Jacobian; using Jacobian2 = typename traits::Jacobian; - - static_assert(product_lie_group_detail::IsStatelessLeftActionPolicy< - Action, G, H>::value, - "ProductLieGroup action must be a stateless left " - "GroupAction policy on H."); public: /// @name Standard Constructors /// @{ @@ -302,7 +156,8 @@ class ProductLieGroup : public std::pair { static ProductLieGroup Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, - DynamicJacobian H1 = {}, DynamicJacobian H2 = {}); + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}); /// Logarithmic map static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}); @@ -667,9 +522,9 @@ class PowerLieGroup }; /// Traits specialization for ProductLieGroup -template -struct traits> - : internal::LieGroup> {}; +template +struct traits> + : internal::LieGroup> {}; /// Traits specialization for PowerLieGroup template diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp new file mode 100644 index 0000000000..37c30288d6 --- /dev/null +++ b/tests/testActionProductLieGroup.cpp @@ -0,0 +1,304 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- + */ + +/** + * @file testActionProductLieGroup.cpp + * @date April, 2026 + * @author Frank Dellaert + * @brief unit tests for action-parameterized product Lie groups + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +constexpr double kTol = 1e-9; + +using Product = ProductLieGroup; +using DirectActionProduct = ActionProductLieGroup; + +struct Rot3VectorAction : public GroupAction { + static constexpr ActionType type = ActionType::Left; + + Vector3 operator()(const Rot3& R, const Vector3& t, + OptionalJacobian<3, 3> HR = {}, + OptionalJacobian<3, 3> Ht = {}) const { + return R.rotate(t, HR, Ht); + } + + template + static ProductType Expmap( + const Eigen::Ref:: + TangentVector>& w, + const Eigen::Ref:: + TangentVector>& rho, + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) { + Matrix6 Hpose; + Vector6 xi; + xi << w, rho; + const Pose3 pose = Pose3::Expmap(xi, H1 || H2 ? &Hpose : nullptr); + if (H1) *H1 = Hpose.leftCols<3>(); + if (H2) *H2 = Hpose.rightCols<3>(); + return ProductType(pose.rotation(), pose.translation()); + } + + template + static typename ProductType::TangentVector Logmap( + const ProductType& p, typename ProductType::ChartJacobian H = {}) { + Matrix6 Hpose; + const Vector6 xi = + Pose3::Logmap(Pose3(p.first, p.second), H ? &Hpose : nullptr); + if (H) *H = Hpose; + return xi; + } + + template + static typename ProductType::Jacobian AdjointMap(const ProductType& p) { + return Pose3(p.first, p.second).AdjointMap(); + } +}; + +using Semidirect = + ActionProductLieGroup; + +namespace { + +Semidirect composeSemidirectProxy(const Semidirect& A, const Semidirect& B) { + return A.compose(B); +} + +Semidirect betweenSemidirectProxy(const Semidirect& A, const Semidirect& B) { + return A.between(B); +} + +Semidirect inverseSemidirectProxy(const Semidirect& A) { return A.inverse(); } + +Semidirect expmapSemidirectProxy(const Vector6& vec) { + return Semidirect::Expmap(vec); +} + +Vector6 logmapSemidirectProxy(const Semidirect& p) { + return Semidirect::Logmap(p); +} + +Semidirect retractSemidirectProxy(const Semidirect& X, const Vector6& v) { + return X.retract(v); +} + +Vector6 localCoordinatesSemidirectProxy(const Semidirect& X, + const Semidirect& Y) { + return X.localCoordinates(Y); +} + +Pose3 asPose3(const Semidirect& state) { + return Pose3(state.first, state.second); +} + +} // namespace + +/* ************************************************************************* */ +TEST(Lie, ActionProductLieGroupDirect) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + const Product state(Point2(1, 2), Pose2(3, 4, 5)); + const Product other(Point2(-0.5, 0.25), Pose2(-1, 2, -0.4)); + const DirectActionProduct actionState(state.first, state.second); + const DirectActionProduct actionOther(other.first, other.second); + + const Product defaultComposed = state.compose(other); + const DirectActionProduct actionComposed = actionState.compose(actionOther); + EXPECT(assert_equal(defaultComposed.first, actionComposed.first, kTol)); + EXPECT(assert_equal(defaultComposed.second, actionComposed.second, kTol)); + + const Product defaultBetween = state.between(other); + const DirectActionProduct actionBetween = actionState.between(actionOther); + EXPECT(assert_equal(defaultBetween.first, actionBetween.first, kTol)); + EXPECT(assert_equal(defaultBetween.second, actionBetween.second, kTol)); + + const Product defaultInverse = state.inverse(); + const DirectActionProduct actionInverse = actionState.inverse(); + EXPECT(assert_equal(defaultInverse.first, actionInverse.first, kTol)); + EXPECT(assert_equal(defaultInverse.second, actionInverse.second, kTol)); + + Vector5 xi; + xi << 0.1, -0.2, 0.05, 0.1, -0.15; + const Product defaultExp = Product::Expmap(xi); + const DirectActionProduct actionExp = DirectActionProduct::Expmap(xi); + EXPECT(assert_equal(defaultExp.first, actionExp.first, kTol)); + EXPECT(assert_equal(defaultExp.second, actionExp.second, kTol)); + EXPECT(assert_equal(Product::Logmap(defaultExp), + DirectActionProduct::Logmap(actionExp), kTol)); + EXPECT(assert_equal(state.AdjointMap(), actionState.AdjointMap(), kTol)); +} + +/* ************************************************************************* */ +TEST(Lie, ActionProductLieGroupSemidirect) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + const Rot3VectorAction action; + const Rot3 R1 = Rot3::RzRyRx(0.1, -0.2, 0.3); + const Rot3 R2 = Rot3::RzRyRx(-0.2, 0.05, 0.1); + const Vector3 t(0.4, -0.5, 0.6); + EXPECT_LEFT_ACTION(action, R1, R2, t); + + const Semidirect identity; + Vector6 xi; + xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; + const Semidirect actual = identity.expmap(xi); + EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); + EXPECT(assert_equal(xi, identity.logmap(actual), kTol)); + + const Semidirect a(Rot3::RzRyRx(0.1, 0.2, -0.1), + Vector3(1.0, -2.0, 0.5)); + const Semidirect b(Rot3::RzRyRx(-0.3, 0.15, 0.2), + Vector3(-0.25, 0.4, 1.5)); + const Semidirect c(Rot3::RzRyRx(0.2, -0.1, 0.05), + Vector3(0.3, -0.6, 0.8)); + EXPECT(assert_equal(asPose3((a * b) * c), asPose3(a * (b * c)), kTol)); + EXPECT(assert_equal(asPose3(a * a.inverse()), Pose3(), kTol)); + EXPECT(assert_equal(asPose3(a * b), asPose3(a) * asPose3(b), kTol)); +} + +/* ************************************************************************* */ +TEST(testActionProduct, compose) { + const Semidirect state1(Rot3::RzRyRx(0.1, 0.2, -0.3), + Vector3(1.0, -0.5, 0.25)); + const Semidirect state2(Rot3::RzRyRx(-0.2, 0.1, 0.15), + Vector3(-0.75, 0.4, 1.2)); + + Matrix actH1, actH2; + const Semidirect actual = state1.compose(state2, actH1, actH2); + const Matrix numericH1 = + numericalDerivative21(composeSemidirectProxy, state1, state2); + const Matrix numericH2 = + numericalDerivative22(composeSemidirectProxy, state1, state2); + + EXPECT(assert_equal(asPose3(actual), asPose3(state1) * asPose3(state2), kTol)); + EXPECT(assert_equal(numericH1, actH1, 1e-6)); + EXPECT(assert_equal(numericH2, actH2, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testActionProduct, between) { + const Semidirect state1(Rot3::RzRyRx(0.1, 0.2, -0.3), + Vector3(1.0, -0.5, 0.25)); + const Semidirect state2(Rot3::RzRyRx(-0.2, 0.1, 0.15), + Vector3(-0.75, 0.4, 1.2)); + + Matrix actH1, actH2; + const Semidirect actual = state1.between(state2, actH1, actH2); + const Matrix numericH1 = + numericalDerivative21(betweenSemidirectProxy, state1, state2); + const Matrix numericH2 = + numericalDerivative22(betweenSemidirectProxy, state1, state2); + + EXPECT(assert_equal(asPose3(actual), asPose3(state1).between(asPose3(state2)), + kTol)); + EXPECT(assert_equal(numericH1, actH1, 1e-6)); + EXPECT(assert_equal(numericH2, actH2, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testActionProduct, inverse) { + const Semidirect state(Rot3::RzRyRx(0.2, -0.1, 0.05), + Vector3(0.5, -1.2, 0.8)); + + Matrix actH; + const Semidirect actual = state.inverse(actH); + const Matrix numericH = numericalDerivative11(inverseSemidirectProxy, state); + + EXPECT(assert_equal(asPose3(actual), asPose3(state).inverse(), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testActionProduct, Expmap) { + Vector6 xi; + xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; + + Matrix actH; + const Semidirect actual = Semidirect::Expmap(xi, actH); + const Matrix numericH = numericalDerivative11(expmapSemidirectProxy, xi); + + EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testActionProduct, Logmap) { + const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), + Vector3(0.4, -0.1, 0.2)); + + Matrix actH; + const Vector6 actual = Semidirect::Logmap(state, actH); + const Matrix numericH = numericalDerivative11(logmapSemidirectProxy, state); + + EXPECT(assert_equal(Pose3::Logmap(asPose3(state)), actual, kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(testActionProduct, AdjointMap) { + const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), + Vector3(0.4, -0.1, 0.2)); + + EXPECT(assert_equal(asPose3(state).AdjointMap(), state.AdjointMap(), kTol)); +} + +/* ************************************************************************* */ +TEST(testActionProduct, retractAndLocalCoordinates) { + const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), + Vector3(0.4, -0.1, 0.2)); + Vector6 delta; + delta << 0.05, -0.04, 0.03, 0.1, -0.2, 0.05; + + Matrix retractH1, retractH2, localH1, localH2; + const Semidirect updated = state.retract(delta, retractH1, retractH2); + const Vector6 recovered = + state.localCoordinates(updated, localH1, localH2); + + EXPECT(assert_equal(asPose3(updated), asPose3(state).retract(delta), kTol)); + EXPECT(assert_equal(delta, recovered, kTol)); + + const Matrix numericRetractH1 = + numericalDerivative21(retractSemidirectProxy, state, delta); + const Matrix numericRetractH2 = + numericalDerivative22(retractSemidirectProxy, state, delta); + const Matrix numericLocalH1 = + numericalDerivative21(localCoordinatesSemidirectProxy, state, updated); + const Matrix numericLocalH2 = + numericalDerivative22(localCoordinatesSemidirectProxy, state, updated); + + EXPECT(assert_equal(numericRetractH1, retractH1, 1e-6)); + EXPECT(assert_equal(numericRetractH2, retractH2, 1e-6)); + EXPECT(assert_equal(numericLocalH1, localH1, 1e-6)); + EXPECT(assert_equal(numericLocalH2, localH2, 1e-6)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/tests/testProductLieGroup.cpp b/tests/testProductLieGroup.cpp index 5dd3c2c38a..78eb97a2dd 100644 --- a/tests/testProductLieGroup.cpp +++ b/tests/testProductLieGroup.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -33,54 +32,8 @@ using namespace gtsam; constexpr double kTol = 1e-9; using Product = ProductLieGroup; -using ProductExplicitDirect = - ProductLieGroup>; using ProductVR = ProductLieGroup; using ProductVV = ProductLieGroup; - -struct Rot3VectorAction : public GroupAction { - static constexpr ActionType type = ActionType::Left; - - Vector3 operator()(const Rot3& R, const Vector3& t, - OptionalJacobian<3, 3> HR = {}, - OptionalJacobian<3, 3> Ht = {}) const { - return R.rotate(t, HR, Ht); - } - - template - static Product Expmap( - const Eigen::Ref:: - TangentVector>& w, - const Eigen::Ref:: - TangentVector>& rho, - OptionalJacobian H1 = {}, - OptionalJacobian H2 = {}) { - Matrix6 Hpose; - Vector6 xi; - xi << w, rho; - const Pose3 pose = Pose3::Expmap(xi, H1 || H2 ? &Hpose : nullptr); - if (H1) *H1 = Hpose.leftCols<3>(); - if (H2) *H2 = Hpose.rightCols<3>(); - return Product(pose.rotation(), pose.translation()); - } - - template - static typename Product::TangentVector Logmap( - const Product& p, typename Product::ChartJacobian H = {}) { - Matrix6 Hpose; - const Vector6 xi = - Pose3::Logmap(Pose3(p.first, p.second), H ? &Hpose : nullptr); - if (H) *H = Hpose; - return xi; - } - - template - static typename Product::Jacobian AdjointMap(const Product& p) { - return Pose3(p.first, p.second).AdjointMap(); - } -}; - -using Semidirect = ProductLieGroup; constexpr int kPowerComponents = 2; using Power = PowerLieGroup; using PowerTangent = Power::TangentVector; @@ -165,37 +118,6 @@ Product expmapProductProxy(const Vector5& vec) { return Product::Expmap(vec); } Vector5 logmapProductProxy(const Product& p) { return Product::Logmap(p); } -Semidirect composeSemidirectProxy(const Semidirect& A, const Semidirect& B) { - return A.compose(B); -} - -Semidirect betweenSemidirectProxy(const Semidirect& A, const Semidirect& B) { - return A.between(B); -} - -Semidirect inverseSemidirectProxy(const Semidirect& A) { return A.inverse(); } - -Semidirect expmapSemidirectProxy(const Vector6& vec) { - return Semidirect::Expmap(vec); -} - -Vector6 logmapSemidirectProxy(const Semidirect& p) { - return Semidirect::Logmap(p); -} - -Semidirect retractSemidirectProxy(const Semidirect& X, const Vector6& v) { - return X.retract(v); -} - -Vector6 localCoordinatesSemidirectProxy(const Semidirect& X, - const Semidirect& Y) { - return X.localCoordinates(Y); -} - -Pose3 asPose3(const Semidirect& state) { - return Pose3(state.first, state.second); -} - ProductVR composeProductVRProxy(const ProductVR& A, const ProductVR& B) { return A.compose(B); } @@ -608,185 +530,6 @@ TEST(Lie, Interpolate) { EXPECT(assert_equal(numericH3, actH3, kTol)); } -/* ************************************************************************* */ -TEST(Lie, ProductLieGroupExplicitDirectAction) { - const Product state(Point2(1, 2), Pose2(3, 4, 5)); - const Product other(Point2(-0.5, 0.25), Pose2(-1, 2, -0.4)); - const ProductExplicitDirect explicitState(state.first, state.second); - const ProductExplicitDirect explicitOther(other.first, other.second); - - const Product defaultComposed = state.compose(other); - const ProductExplicitDirect explicitComposed = explicitState.compose(explicitOther); - EXPECT(assert_equal(defaultComposed.first, explicitComposed.first, kTol)); - EXPECT(assert_equal(defaultComposed.second, explicitComposed.second, kTol)); - - const Product defaultBetween = state.between(other); - const ProductExplicitDirect explicitBetween = explicitState.between(explicitOther); - EXPECT(assert_equal(defaultBetween.first, explicitBetween.first, kTol)); - EXPECT(assert_equal(defaultBetween.second, explicitBetween.second, kTol)); - - const Product defaultInverse = state.inverse(); - const ProductExplicitDirect explicitInverse = explicitState.inverse(); - EXPECT(assert_equal(defaultInverse.first, explicitInverse.first, kTol)); - EXPECT(assert_equal(defaultInverse.second, explicitInverse.second, kTol)); - - Vector5 xi; - xi << 0.1, -0.2, 0.05, 0.1, -0.15; - const Product defaultExp = Product::Expmap(xi); - const ProductExplicitDirect explicitExp = ProductExplicitDirect::Expmap(xi); - EXPECT(assert_equal(defaultExp.first, explicitExp.first, kTol)); - EXPECT(assert_equal(defaultExp.second, explicitExp.second, kTol)); - EXPECT(assert_equal(Product::Logmap(defaultExp), - ProductExplicitDirect::Logmap(explicitExp), kTol)); - EXPECT(assert_equal(state.AdjointMap(), explicitState.AdjointMap(), kTol)); -} - -/* ************************************************************************* */ -TEST(Lie, ProductLieGroupSemidirect) { - GTSAM_CONCEPT_ASSERT(IsGroup); - GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); - - const Rot3VectorAction action; - const Rot3 R1 = Rot3::RzRyRx(0.1, -0.2, 0.3); - const Rot3 R2 = Rot3::RzRyRx(-0.2, 0.05, 0.1); - const Vector3 t = Vector3(0.4, -0.5, 0.6); - EXPECT_LEFT_ACTION(action, R1, R2, t); - - const Semidirect identity; - Vector6 xi; - xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; - const Semidirect actual = identity.expmap(xi); - EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); - EXPECT(assert_equal(xi, identity.logmap(actual), kTol)); - - const Semidirect a(Rot3::RzRyRx(0.1, 0.2, -0.1), - Vector3(1.0, -2.0, 0.5)); - const Semidirect b(Rot3::RzRyRx(-0.3, 0.15, 0.2), - Vector3(-0.25, 0.4, 1.5)); - const Semidirect c(Rot3::RzRyRx(0.2, -0.1, 0.05), - Vector3(0.3, -0.6, 0.8)); - EXPECT(assert_equal(asPose3((a * b) * c), asPose3(a * (b * c)), kTol)); - EXPECT(assert_equal(asPose3(a * a.inverse()), Pose3(), kTol)); - EXPECT(assert_equal(asPose3(a * b), asPose3(a) * asPose3(b), kTol)); -} - -/* ************************************************************************* */ -TEST(testProductSemidirect, compose) { - const Semidirect state1(Rot3::RzRyRx(0.1, 0.2, -0.3), - Vector3(1.0, -0.5, 0.25)); - const Semidirect state2(Rot3::RzRyRx(-0.2, 0.1, 0.15), - Vector3(-0.75, 0.4, 1.2)); - - Matrix actH1, actH2; - const Semidirect actual = state1.compose(state2, actH1, actH2); - const Matrix numericH1 = - numericalDerivative21(composeSemidirectProxy, state1, state2); - const Matrix numericH2 = - numericalDerivative22(composeSemidirectProxy, state1, state2); - - EXPECT(assert_equal(asPose3(actual), asPose3(state1) * asPose3(state2), kTol)); - EXPECT(assert_equal(numericH1, actH1, 1e-6)); - EXPECT(assert_equal(numericH2, actH2, 1e-6)); -} - -/* ************************************************************************* */ -TEST(testProductSemidirect, between) { - const Semidirect state1(Rot3::RzRyRx(0.1, 0.2, -0.3), - Vector3(1.0, -0.5, 0.25)); - const Semidirect state2(Rot3::RzRyRx(-0.2, 0.1, 0.15), - Vector3(-0.75, 0.4, 1.2)); - - Matrix actH1, actH2; - const Semidirect actual = state1.between(state2, actH1, actH2); - const Matrix numericH1 = - numericalDerivative21(betweenSemidirectProxy, state1, state2); - const Matrix numericH2 = - numericalDerivative22(betweenSemidirectProxy, state1, state2); - - EXPECT(assert_equal(asPose3(actual), asPose3(state1).between(asPose3(state2)), - kTol)); - EXPECT(assert_equal(numericH1, actH1, 1e-6)); - EXPECT(assert_equal(numericH2, actH2, 1e-6)); -} - -/* ************************************************************************* */ -TEST(testProductSemidirect, inverse) { - const Semidirect state(Rot3::RzRyRx(0.2, -0.1, 0.05), - Vector3(0.5, -1.2, 0.8)); - - Matrix actH; - const Semidirect actual = state.inverse(actH); - const Matrix numericH = numericalDerivative11(inverseSemidirectProxy, state); - - EXPECT(assert_equal(asPose3(actual), asPose3(state).inverse(), kTol)); - EXPECT(assert_equal(numericH, actH, 1e-6)); -} - -/* ************************************************************************* */ -TEST(testProductSemidirect, Expmap) { - Vector6 xi; - xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; - - Matrix actH; - const Semidirect actual = Semidirect::Expmap(xi, actH); - const Matrix numericH = numericalDerivative11(expmapSemidirectProxy, xi); - - EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); - EXPECT(assert_equal(numericH, actH, 1e-6)); -} - -/* ************************************************************************* */ -TEST(testProductSemidirect, Logmap) { - const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), - Vector3(0.4, -0.1, 0.2)); - - Matrix actH; - const Vector6 actual = Semidirect::Logmap(state, actH); - const Matrix numericH = numericalDerivative11(logmapSemidirectProxy, state); - - EXPECT(assert_equal(Pose3::Logmap(asPose3(state)), actual, kTol)); - EXPECT(assert_equal(numericH, actH, 1e-6)); -} - -/* ************************************************************************* */ -TEST(testProductSemidirect, AdjointMap) { - const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), - Vector3(0.4, -0.1, 0.2)); - - EXPECT(assert_equal(asPose3(state).AdjointMap(), state.AdjointMap(), kTol)); -} - -/* ************************************************************************* */ -TEST(testProductSemidirect, retractAndLocalCoordinates) { - const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), - Vector3(0.4, -0.1, 0.2)); - Vector6 delta; - delta << 0.05, -0.04, 0.03, 0.1, -0.2, 0.05; - - Matrix retractH1, retractH2, localH1, localH2; - const Semidirect updated = state.retract(delta, retractH1, retractH2); - const Vector6 recovered = - state.localCoordinates(updated, localH1, localH2); - - EXPECT(assert_equal(asPose3(updated), asPose3(state).retract(delta), kTol)); - EXPECT(assert_equal(delta, recovered, kTol)); - - const Matrix numericRetractH1 = - numericalDerivative21(retractSemidirectProxy, state, delta); - const Matrix numericRetractH2 = - numericalDerivative22(retractSemidirectProxy, state, delta); - const Matrix numericLocalH1 = - numericalDerivative21(localCoordinatesSemidirectProxy, state, updated); - const Matrix numericLocalH2 = - numericalDerivative22(localCoordinatesSemidirectProxy, state, updated); - - EXPECT(assert_equal(numericRetractH1, retractH1, 1e-6)); - EXPECT(assert_equal(numericRetractH2, retractH2, 1e-6)); - EXPECT(assert_equal(numericLocalH1, localH1, 1e-6)); - EXPECT(assert_equal(numericLocalH2, localH2, 1e-6)); -} - /* ************************************************************************* */ TEST(Lie, PowerLieGroup) { GTSAM_CONCEPT_ASSERT(IsGroup); From 3d368f5100d13344280e8eda68acc516c83193ab Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 02:31:50 -0400 Subject: [PATCH 03/21] [skip ci] slim down --- gtsam/base/ActionProductLieGroup-inl.h | 16 +---- gtsam/base/ActionProductLieGroup.h | 98 -------------------------- gtsam/base/ProductLieGroup.h | 1 + 3 files changed, 2 insertions(+), 113 deletions(-) diff --git a/gtsam/base/ActionProductLieGroup-inl.h b/gtsam/base/ActionProductLieGroup-inl.h index b4f0a63141..b6ee242382 100644 --- a/gtsam/base/ActionProductLieGroup-inl.h +++ b/gtsam/base/ActionProductLieGroup-inl.h @@ -9,6 +9,7 @@ * -------------------------------------------------------------------------- */ + /** * @file ActionProductLieGroup-inl.h * @date April, 2026 @@ -214,11 +215,6 @@ ActionProductLieGroup::Expmap( if constexpr (isDirectProduct) { return This(Base::Expmap(v1, v2, H1, H2)); } else { - static_assert(action_product_lie_group_detail::HasSemidirectExpmap< - This, Action, G, H>::value, - "ActionProductLieGroup semidirect actions must provide " - "template static Product Expmap(v1, v2, H1, " - "H2)."); return Action::template Expmap(v1, v2, H1, H2); } } @@ -230,11 +226,6 @@ ActionProductLieGroup::Logmap(const This& p, if constexpr (isDirectProduct) { return Base::Logmap(p, Hp); } else { - static_assert(action_product_lie_group_detail::HasSemidirectLogmap< - This, Action, G, H>::value, - "ActionProductLieGroup semidirect actions must provide " - "template static typename Product::" - "TangentVector Logmap(const Product&, ChartJacobian)."); return Action::template Logmap(p, Hp); } } @@ -251,11 +242,6 @@ ActionProductLieGroup::AdjointMap() const { if constexpr (isDirectProduct) { return Base::AdjointMap(); } else { - static_assert(action_product_lie_group_detail::HasSemidirectAdjointMap< - This, Action, G, H>::value, - "ActionProductLieGroup semidirect actions must provide " - "template static typename Product::Jacobian " - "AdjointMap(const Product&)."); return Action::template AdjointMap(*this); } } diff --git a/gtsam/base/ActionProductLieGroup.h b/gtsam/base/ActionProductLieGroup.h index 0b1bee0c8d..a8c2519fb4 100644 --- a/gtsam/base/ActionProductLieGroup.h +++ b/gtsam/base/ActionProductLieGroup.h @@ -28,99 +28,6 @@ namespace gtsam { template struct DirectProductAction; -namespace action_product_lie_group_detail { - -template -struct IsStatelessLeftActionPolicy : std::false_type {}; - -template -struct IsStatelessLeftActionPolicy> - : std::bool_constant< - std::is_empty_v && std::is_default_constructible_v && - std::is_base_of_v, Action> && - Action::type == ActionType::Left && - std::is_invocable_r_v< - H, const Action&, const G&, const H&, - OptionalJacobian::dimension, traits::dimension>, - OptionalJacobian::dimension, traits::dimension>>> { -}; - -template -using ProductChartJacobian = std::conditional_t< - traits::dimension == Eigen::Dynamic || - traits::dimension == Eigen::Dynamic, - OptionalJacobian, - OptionalJacobian::dimension + traits::dimension, - traits::dimension + traits::dimension>>; - -template -using ProductTangentVector = std::conditional_t< - traits::dimension == Eigen::Dynamic || - traits::dimension == Eigen::Dynamic, - Vector, - Eigen::Matrix::dimension + traits::dimension, 1>>; - -template -using ProductJacobian = std::conditional_t< - traits::dimension == Eigen::Dynamic || - traits::dimension == Eigen::Dynamic, - Matrix, - Eigen::Matrix::dimension + traits::dimension, - traits::dimension + traits::dimension>>; - -template -struct HasSemidirectExpmap : std::false_type {}; - -template -struct HasSemidirectExpmap< - Product, Action, G, H, - std::void_t( - std::declval::TangentVector&>(), - std::declval::TangentVector&>(), - std::declval>(), - std::declval>()))>> - : std::bool_constant( - std::declval::TangentVector&>(), - std::declval::TangentVector&>(), - std::declval>(), - std::declval>())), - Product>> {}; - -template -struct HasSemidirectLogmap : std::false_type {}; - -template -struct HasSemidirectLogmap< - Product, Action, G, H, - std::void_t( - std::declval(), - std::declval>()))>> - : std::bool_constant( - std::declval(), - std::declval>())), - ProductTangentVector>> {}; - -template -struct HasSemidirectAdjointMap : std::false_type {}; - -template -struct HasSemidirectAdjointMap< - Product, Action, G, H, - std::void_t( - std::declval()))>> - : std::bool_constant( - std::declval())), - ProductJacobian>> {}; - -} // namespace action_product_lie_group_detail - template struct DirectProductAction : public GroupAction, G, H> { @@ -192,11 +99,6 @@ class ActionProductLieGroup : public ProductLieGroup { inline constexpr static int manifoldDimension = dimension; using DynamicJacobian = OptionalJacobian; - static_assert(action_product_lie_group_detail::IsStatelessLeftActionPolicy< - Action, G, H>::value, - "ActionProductLieGroup action must be a stateless left " - "GroupAction policy on H."); - typedef multiplicative_group_tag group_flavor; ActionProductLieGroup() : Base() {} diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 450079da9b..5bda8f4a2c 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -74,6 +74,7 @@ class ProductLieGroup : public std::pair { Eigen::Matrix>; using Jacobian1 = typename traits::Jacobian; using Jacobian2 = typename traits::Jacobian; + public: /// @name Standard Constructors /// @{ From a552ebe1fa255a333cad63099309f438b41edd13 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 02:32:32 -0400 Subject: [PATCH 04/21] [skip ci] --- gtsam/base/ProductLieGroup.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 5bda8f4a2c..297652204a 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -74,7 +74,7 @@ class ProductLieGroup : public std::pair { Eigen::Matrix>; using Jacobian1 = typename traits::Jacobian; using Jacobian2 = typename traits::Jacobian; - + public: /// @name Standard Constructors /// @{ From a549d3fedf573354b89b146426dfc19bd0780419 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 03:10:10 -0400 Subject: [PATCH 05/21] re-integrate with product lie group --- gtsam/base/ActionProductLieGroup-inl.h | 267 -------------- gtsam/base/ActionProductLieGroup.h | 184 ---------- gtsam/base/ProductLieGroup-inl.h | 460 ++++++++++++++----------- gtsam/base/ProductLieGroup.h | 61 +++- tests/testActionProductLieGroup.cpp | 37 +- 5 files changed, 339 insertions(+), 670 deletions(-) delete mode 100644 gtsam/base/ActionProductLieGroup-inl.h delete mode 100644 gtsam/base/ActionProductLieGroup.h diff --git a/gtsam/base/ActionProductLieGroup-inl.h b/gtsam/base/ActionProductLieGroup-inl.h deleted file mode 100644 index b6ee242382..0000000000 --- a/gtsam/base/ActionProductLieGroup-inl.h +++ /dev/null @@ -1,267 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - - -/** - * @file ActionProductLieGroup-inl.h - * @date April, 2026 - * @author Frank Dellaert - * @brief Internals for ActionProductLieGroup.h, not for general consumption - */ - -#pragma once - -namespace gtsam { - -template -ActionProductLieGroup -ActionProductLieGroup::operator*(const This& other) const { - checkMatchingDimensions(other, "operator*"); - if constexpr (isDirectProduct) { - return This(Base::operator*(other)); - } else { - const Action action{}; - const H actedSecond = action(this->first, other.second); - return This(traits::Compose(this->first, other.first), - traits::Compose(this->second, actedSecond)); - } -} - -template -ActionProductLieGroup -ActionProductLieGroup::inverse() const { - if constexpr (isDirectProduct) { - return This(Base::inverse()); - } else { - const Action action{}; - const G gInv = traits::Inverse(this->first); - const H hInv = traits::Inverse(this->second); - return This(gInv, action(gInv, hInv)); - } -} - -template -ActionProductLieGroup -ActionProductLieGroup::retract(const TangentVector& v, - ChartJacobian H1, - ChartJacobian H2) const { - if constexpr (isDirectProduct) { - return This(Base::retract(v, H1, H2)); - } else { - const size_t productDimension = - combinedDimension(this->firstDim(), this->secondDim()); - if (static_cast(v.size()) != productDimension) { - throw std::invalid_argument( - "ActionProductLieGroup::retract tangent dimension does not match " - "product dimension"); - } - Jacobian D_g_v; - This g = - This::Expmap(v, H2 ? OptionalJacobian(&D_g_v) - : ChartJacobian()); - This h = compose(g); - if (H1) *H1 = g.inverse().AdjointMap(); - if (H2) *H2 = D_g_v; - return h; - } -} - -template -typename ActionProductLieGroup::TangentVector -ActionProductLieGroup::localCoordinates(const This& g, - ChartJacobian H1, - ChartJacobian H2) const { - checkMatchingDimensions(g, "localCoordinates"); - if constexpr (isDirectProduct) { - return Base::localCoordinates(g, H1, H2); - } else { - This h = between(g); - Jacobian D_v_h; - TangentVector v = This::Logmap( - h, H1 || H2 ? OptionalJacobian(&D_v_h) - : ChartJacobian()); - if (H1) *H1 = -D_v_h * h.inverse().AdjointMap(); - if (H2) *H2 = D_v_h; - return v; - } -} - -template -ActionProductLieGroup -ActionProductLieGroup::compose(const This& other, - ChartJacobian H1, - ChartJacobian H2) const { - checkMatchingDimensions(other, "compose"); - if constexpr (isDirectProduct) { - return This(Base::compose(other, H1, H2)); - } else { - const size_t productDimension = - combinedDimension(this->firstDim(), this->secondDim()); - This result = (*this) * other; - if (H1) *H1 = other.inverse().AdjointMap(); - if (H2) *H2 = identityJacobian(productDimension); - return result; - } -} - -template -ActionProductLieGroup -ActionProductLieGroup::between(const This& other, - ChartJacobian H1, - ChartJacobian H2) const { - checkMatchingDimensions(other, "between"); - if constexpr (isDirectProduct) { - return This(Base::between(other, H1, H2)); - } else { - const size_t productDimension = - combinedDimension(this->firstDim(), this->secondDim()); - This result = this->inverse() * other; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = identityJacobian(productDimension); - return result; - } -} - -template -ActionProductLieGroup -ActionProductLieGroup::inverse(ChartJacobian D) const { - if constexpr (isDirectProduct) { - return This(Base::inverse(D)); - } else { - if (D) *D = -AdjointMap(); - return inverse(); - } -} - -template -ActionProductLieGroup -ActionProductLieGroup::Expmap(const TangentVector& v, - ChartJacobian Hv) { - if constexpr (isDirectProduct) { - return This(Base::Expmap(v, Hv)); - } else { - size_t firstDimension = 0; - size_t secondDimension = 0; - if constexpr (firstDynamic && secondDynamic) { - if (v.size() == 0) { - if (Hv) *Hv = Matrix::Zero(0, 0); - return This(); - } - throw std::invalid_argument( - "ActionProductLieGroup::Expmap requires split tangent vectors when " - "both factors are dynamic"); - } else if constexpr (firstDynamic) { - if (v.size() < dimension2) { - throw std::invalid_argument( - "ActionProductLieGroup::Expmap tangent dimension is too small for " - "the fixed second factor"); - } - firstDimension = static_cast(v.size() - dimension2); - secondDimension = static_cast(dimension2); - } else if constexpr (secondDynamic) { - if (v.size() < dimension1) { - throw std::invalid_argument( - "ActionProductLieGroup::Expmap tangent dimension is too small for " - "the fixed first factor"); - } - firstDimension = static_cast(dimension1); - secondDimension = static_cast(v.size() - dimension1); - } else { - firstDimension = static_cast(dimension1); - secondDimension = static_cast(dimension2); - } - - if (static_cast(v.size()) != - combinedDimension(firstDimension, secondDimension)) { - throw std::invalid_argument( - "ActionProductLieGroup::Expmap tangent dimension does not match " - "product dimension"); - } - - Matrix D_g_first; - Matrix D_h_second; - const auto v1 = tangentSegment(v, 0, firstDimension); - const auto v2 = tangentSegment(v, firstDimension, secondDimension); - This result = Expmap(v1, v2, Hv ? DynamicJacobian(D_g_first) - : DynamicJacobian(), - Hv ? DynamicJacobian(D_h_second) - : DynamicJacobian()); - if (Hv) { - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - *Hv = zeroJacobian(productDimension); - Hv->block(0, 0, productDimension, firstDimension) = D_g_first; - Hv->block(0, firstDimension, productDimension, secondDimension) = - D_h_second; - } - return result; - } -} - -template -ActionProductLieGroup -ActionProductLieGroup::Expmap( - const Eigen::Ref::TangentVector>& v1, - const Eigen::Ref::TangentVector>& v2, - DynamicJacobian H1, DynamicJacobian H2) { - if constexpr (isDirectProduct) { - return This(Base::Expmap(v1, v2, H1, H2)); - } else { - return Action::template Expmap(v1, v2, H1, H2); - } -} - -template -typename ActionProductLieGroup::TangentVector -ActionProductLieGroup::Logmap(const This& p, - ChartJacobian Hp) { - if constexpr (isDirectProduct) { - return Base::Logmap(p, Hp); - } else { - return Action::template Logmap(p, Hp); - } -} - -template -ActionProductLieGroup -ActionProductLieGroup::expmap(const TangentVector& v) const { - return compose(This::Expmap(v)); -} - -template -typename ActionProductLieGroup::Jacobian -ActionProductLieGroup::AdjointMap() const { - if constexpr (isDirectProduct) { - return Base::AdjointMap(); - } else { - return Action::template AdjointMap(*this); - } -} - -template -void ActionProductLieGroup::checkMatchingDimensions( - const This& other, const char* operation) const { - if (this->firstDim() != other.firstDim() || - this->secondDim() != other.secondDim()) { - throw std::invalid_argument(std::string("ActionProductLieGroup::") + - operation + - " requires matching component dimensions"); - } -} - -template -void ActionProductLieGroup::print(const std::string& s) const { - std::cout << s << "ActionProductLieGroup" << std::endl; - traits::Print(this->first, " first"); - traits::Print(this->second, " second"); -} - -} // namespace gtsam diff --git a/gtsam/base/ActionProductLieGroup.h b/gtsam/base/ActionProductLieGroup.h deleted file mode 100644 index a8c2519fb4..0000000000 --- a/gtsam/base/ActionProductLieGroup.h +++ /dev/null @@ -1,184 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file ActionProductLieGroup.h - * @date April, 2026 - * @author Frank Dellaert - * @brief Product Lie group parameterized by a left group action - */ - -#pragma once - -#include -#include - -#include - -namespace gtsam { - -template -struct DirectProductAction; - -template -struct DirectProductAction - : public GroupAction, G, H> { - static constexpr ActionType type = ActionType::Left; - - H operator()(const G& g, const H& h, - OptionalJacobian::dimension, traits::dimension> Hg = - {}, - OptionalJacobian::dimension, traits::dimension> Hh = - {}) const { - if (Hg) { - if constexpr (traits::dimension == Eigen::Dynamic || - traits::dimension == Eigen::Dynamic) { - Hg->setZero(static_cast(traits::GetDimension(h)), - static_cast(traits::GetDimension(g))); - } else { - Hg->setZero(); - } - } - if (Hh) { - if constexpr (traits::dimension == Eigen::Dynamic) { - const Eigen::Index hDim = - static_cast(traits::GetDimension(h)); - Hh->setIdentity(hDim, hDim); - } else { - Hh->setIdentity(); - } - } - return h; - } -}; - -/** - * @brief Product Lie group extended with an optional left action. - * If Action is omitted the group reduces to the direct product G x H. - * If Action is provided the group becomes the semidirect product G ⋉ H. - */ -template > -class ActionProductLieGroup : public ProductLieGroup { - GTSAM_CONCEPT_ASSERT(IsLieGroup); - GTSAM_CONCEPT_ASSERT(IsLieGroup); - GTSAM_CONCEPT_ASSERT(IsTestable); - GTSAM_CONCEPT_ASSERT(IsTestable); - - public: - using This = ActionProductLieGroup; - using FirstFactor = G; - using SecondFactor = H; - using ActionPolicy = Action; - using Base = ProductLieGroup; - using PairBase = typename Base::Base; - using typename Base::ChartJacobian; - using typename Base::Jacobian; - using typename Base::Jacobian1; - using typename Base::Jacobian2; - using typename Base::TangentVector; - - protected: - inline constexpr static int dimension1 = Base::dimension1; - inline constexpr static int dimension2 = Base::dimension2; - inline constexpr static bool firstDynamic = Base::firstDynamic; - inline constexpr static bool secondDynamic = Base::secondDynamic; - inline constexpr static bool isDirectProduct = - std::is_same_v>; - - public: - inline constexpr static int dimension = Base::dimension; - inline constexpr static int manifoldDimension = dimension; - using DynamicJacobian = OptionalJacobian; - - typedef multiplicative_group_tag group_flavor; - - ActionProductLieGroup() : Base() {} - ActionProductLieGroup(const G& g, const H& h) : Base(g, h) {} - ActionProductLieGroup(const PairBase& base) : Base(base) {} - ActionProductLieGroup(const Base& base) : Base(base) {} - - static This Identity() { return This(); } - static constexpr int Dim() { return manifoldDimension; } - size_t dim() const { return Base::dim(); } - - This operator*(const This& other) const; - This inverse() const; - - This compose(const This& g) const { return (*this) * g; } - This between(const This& g) const { return this->inverse() * g; } - - This retract(const TangentVector& v, ChartJacobian H1 = {}, - ChartJacobian H2 = {}) const; - TangentVector localCoordinates(const This& g, ChartJacobian H1 = {}, - ChartJacobian H2 = {}) const; - - This compose(const This& other, ChartJacobian H1, - ChartJacobian H2 = {}) const; - This between(const This& other, ChartJacobian H1, - ChartJacobian H2 = {}) const; - This inverse(ChartJacobian D) const; - - static This Expmap(const TangentVector& v, ChartJacobian Hv = {}); - static This Expmap( - const Eigen::Ref::TangentVector>& v1, - const Eigen::Ref::TangentVector>& v2, - DynamicJacobian H1 = {}, DynamicJacobian H2 = {}); - static TangentVector Logmap(const This& p, ChartJacobian Hp = {}); - static TangentVector LocalCoordinates(const This& p, ChartJacobian Hp = {}) { - return Logmap(p, Hp); - } - - This expmap(const TangentVector& v) const; - TangentVector logmap(const This& g) const { return This::Logmap(between(g)); } - Jacobian AdjointMap() const; - - void print(const std::string& s = "") const; - bool equals(const This& other, double tol = 1e-9) const { - return Base::equals(other, tol); - } - - protected: - static size_t combinedDimension(size_t d1, size_t d2) { - return Base::combinedDimension(d1, d2); - } - - template ::dimension> - static typename traits::TangentVector tangentSegment( - const TangentVector& v, size_t start, size_t runtimeDimension) { - return Base::template tangentSegment(v, start, runtimeDimension); - } - - static TangentVector makeTangentVector( - const typename traits::TangentVector& v1, - const typename traits::TangentVector& v2, size_t firstDimension, - size_t secondDimension) { - return Base::makeTangentVector(v1, v2, firstDimension, secondDimension); - } - - static Jacobian zeroJacobian(size_t productDimension) { - return Base::zeroJacobian(productDimension); - } - - static Jacobian identityJacobian(size_t productDimension) { - return Base::identityJacobian(productDimension); - } - - void checkMatchingDimensions(const This& other, const char* operation) const; -}; - -template -struct traits> - : internal::LieGroup> {}; - -} // namespace gtsam - -#include diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 42a0681459..2cc7a3fdca 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -20,162 +20,222 @@ namespace gtsam { -template -ProductLieGroup ProductLieGroup::operator*( +template +ProductLieGroup ProductLieGroup::operator*( const ProductLieGroup& other) const { checkMatchingDimensions(other, "operator*"); - return ProductLieGroup(traits::Compose(this->first, other.first), - traits::Compose(this->second, other.second)); + if constexpr (isDirectProduct) { + return ProductLieGroup(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); + } else { + const Action action{}; + const H actedSecond = action(this->first, other.second); + return ProductLieGroup(traits::Compose(this->first, other.first), + traits::Compose(this->second, actedSecond)); + } } -template -ProductLieGroup ProductLieGroup::inverse() const { - return ProductLieGroup(traits::Inverse(this->first), - traits::Inverse(this->second)); +template +ProductLieGroup ProductLieGroup::inverse() const { + if constexpr (isDirectProduct) { + return ProductLieGroup(traits::Inverse(this->first), + traits::Inverse(this->second)); + } else { + const Action action{}; + const G gInv = traits::Inverse(this->first); + const H hInv = traits::Inverse(this->second); + return ProductLieGroup(gInv, action(gInv, hInv)); + } } -template -ProductLieGroup ProductLieGroup::retract(const TangentVector& v, - ChartJacobian H1, - ChartJacobian H2) const { +template +ProductLieGroup ProductLieGroup::retract( + const TangentVector& v, ChartJacobian H1, ChartJacobian H2) const { const size_t firstDimension = firstDim(); const size_t secondDimension = secondDim(); const size_t productDimension = combinedDimension(firstDimension, secondDimension); - if (static_cast(v.size()) != - productDimension) { + if (static_cast(v.size()) != productDimension) { throw std::invalid_argument( "ProductLieGroup::retract tangent dimension does not match product " "dimension"); } - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - G g = traits::Retract(this->first, tangentSegment(v, 0, firstDimension), + + if constexpr (isDirectProduct) { + Jacobian1 D_g_first; + Jacobian1 D_g_second; + Jacobian2 D_h_first; + Jacobian2 D_h_second; + G g = + traits::Retract(this->first, tangentSegment(v, 0, firstDimension), H1 ? &D_g_first : nullptr, H2 ? &D_g_second : nullptr); - H h = traits::Retract( - this->second, tangentSegment(v, firstDimension, secondDimension), - H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return ProductLieGroup(g, h); -} - -template -typename ProductLieGroup::TangentVector -ProductLieGroup::localCoordinates(const ProductLieGroup& g, - ChartJacobian H1, - ChartJacobian H2) const { + H h = traits::Retract( + this->second, tangentSegment(v, firstDimension, secondDimension), + H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_first; + } + if (H2) { + *H2 = zeroJacobian(productDimension); + H2->block(0, 0, firstDimension, firstDimension) = D_g_second; + H2->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return ProductLieGroup(g, h); + } else { + Jacobian D_expmap; + const ProductLieGroup delta = ProductLieGroup::Expmap( + v, H2 ? ChartJacobian(&D_expmap) : ChartJacobian()); + const ProductLieGroup result = compose(delta); + if (H1) *H1 = delta.inverse().AdjointMap(); + if (H2) *H2 = D_expmap; + return result; + } +} + +template +typename ProductLieGroup::TangentVector +ProductLieGroup::localCoordinates(const ProductLieGroup& g, + ChartJacobian H1, + ChartJacobian H2) const { checkMatchingDimensions(g, "localCoordinates"); const size_t firstDimension = firstDim(); const size_t secondDimension = secondDim(); const size_t productDimension = combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - typename traits::TangentVector v2 = traits::Local( - this->second, g.second, H1 ? &D_h_first : nullptr, - H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return makeTangentVector(v1, v2, firstDimension, secondDimension); -} - -template -ProductLieGroup ProductLieGroup::compose( + + if constexpr (isDirectProduct) { + Jacobian1 D_g_first; + Jacobian1 D_g_second; + Jacobian2 D_h_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, + H2 ? &D_g_second : nullptr); + typename traits::TangentVector v2 = traits::Local( + this->second, g.second, H1 ? &D_h_first : nullptr, + H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_first; + } + if (H2) { + *H2 = zeroJacobian(productDimension); + H2->block(0, 0, firstDimension, firstDimension) = D_g_second; + H2->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return makeTangentVector(v1, v2, firstDimension, secondDimension); + } else { + const ProductLieGroup relative = between(g); + Jacobian D_logmap; + TangentVector v = ProductLieGroup::Logmap( + relative, H1 || H2 ? ChartJacobian(&D_logmap) : ChartJacobian()); + if (H1) *H1 = -D_logmap * relative.inverse().AdjointMap(); + if (H2) *H2 = D_logmap; + return v; + } +} + +template +ProductLieGroup ProductLieGroup::compose( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "compose"); const size_t firstDimension = firstDim(); const size_t secondDimension = secondDim(); const size_t productDimension = combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Compose(this->first, other.first, H1 ? &D_g_first : nullptr); - H h = traits::Compose(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); -} - -template -ProductLieGroup ProductLieGroup::between( + + if constexpr (isDirectProduct) { + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = + traits::Compose(this->first, other.first, H1 ? &D_g_first : nullptr); + H h = traits::Compose(this->second, other.second, + H1 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + if (H2) *H2 = identityJacobian(productDimension); + return ProductLieGroup(g, h); + } else { + const ProductLieGroup result = (*this) * other; + if (H1) *H1 = other.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(productDimension); + return result; + } +} + +template +ProductLieGroup ProductLieGroup::between( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "between"); const size_t firstDimension = firstDim(); const size_t secondDimension = secondDim(); const size_t productDimension = combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Between(this->first, other.first, H1 ? &D_g_first : nullptr); - H h = traits::Between(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); -} - -template -ProductLieGroup ProductLieGroup::inverse(ChartJacobian D) const { + + if constexpr (isDirectProduct) { + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = + traits::Between(this->first, other.first, H1 ? &D_g_first : nullptr); + H h = traits::Between(this->second, other.second, + H1 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + if (H2) *H2 = identityJacobian(productDimension); + return ProductLieGroup(g, h); + } else { + const ProductLieGroup result = this->inverse() * other; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(productDimension); + return result; + } +} + +template +ProductLieGroup ProductLieGroup::inverse( + ChartJacobian D) const { const size_t firstDimension = firstDim(); const size_t secondDimension = secondDim(); const size_t productDimension = combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Inverse(this->first, D ? &D_g_first : nullptr); - H h = traits::Inverse(this->second, D ? &D_h_second : nullptr); - if (D) { - *D = zeroJacobian(productDimension); - D->block(0, 0, firstDimension, firstDimension) = D_g_first; - D->block(firstDimension, firstDimension, secondDimension, secondDimension) = - D_h_second; + + if constexpr (isDirectProduct) { + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Inverse(this->first, D ? &D_g_first : nullptr); + H h = traits::Inverse(this->second, D ? &D_h_second : nullptr); + if (D) { + *D = zeroJacobian(productDimension); + D->block(0, 0, firstDimension, firstDimension) = D_g_first; + D->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return ProductLieGroup(g, h); + } else { + if (D) *D = -AdjointMap(); + return inverse(); } - return ProductLieGroup(g, h); } -template -ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, - ChartJacobian Hv) { +template +ProductLieGroup ProductLieGroup::Expmap( + const TangentVector& v, ChartJacobian Hv) { size_t firstDimension = 0; size_t secondDimension = 0; if constexpr (firstDynamic && secondDynamic) { @@ -212,16 +272,14 @@ ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, "ProductLieGroup::Expmap tangent dimension does not match product " "dimension"); } + Matrix D_g_first; Matrix D_h_second; const auto v1 = tangentSegment(v, 0, firstDimension); const auto v2 = tangentSegment(v, firstDimension, secondDimension); ProductLieGroup result = - Expmap(v1, v2, - Hv ? OptionalJacobian(D_g_first) - : OptionalJacobian(), - Hv ? OptionalJacobian(D_h_second) - : OptionalJacobian()); + Expmap(v1, v2, Hv ? DynamicJacobian(D_g_first) : DynamicJacobian(), + Hv ? DynamicJacobian(D_h_second) : DynamicJacobian()); if (Hv) { const size_t productDimension = combinedDimension(firstDimension, secondDimension); @@ -233,76 +291,90 @@ ProductLieGroup ProductLieGroup::Expmap(const TangentVector& v, return result; } -template -ProductLieGroup ProductLieGroup::Expmap( +template +ProductLieGroup ProductLieGroup::Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, - OptionalJacobian H1, - OptionalJacobian H2) { - const size_t firstDimension = static_cast(v1.size()); - const size_t secondDimension = static_cast(v2.size()); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); - H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = Matrix::Zero(productDimension, firstDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - } - if (H2) { - *H2 = Matrix::Zero(productDimension, secondDimension); - H2->block(firstDimension, 0, secondDimension, secondDimension) = D_h_second; - } - return ProductLieGroup(g, h); -} - -template -typename ProductLieGroup::TangentVector ProductLieGroup::Logmap( - const ProductLieGroup& p, ChartJacobian Hp) { - const size_t firstDimension = p.firstDim(); - const size_t secondDimension = p.secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - Jacobian1 D_g_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Logmap(p.first, Hp ? &D_g_first : nullptr); - typename traits::TangentVector v2 = - traits::Logmap(p.second, Hp ? &D_h_second : nullptr); - TangentVector v = makeTangentVector(v1, v2, firstDimension, secondDimension); - if (Hp) { - *Hp = zeroJacobian(productDimension); - Hp->block(0, 0, firstDimension, firstDimension) = D_g_first; - Hp->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; + DynamicJacobian H1, DynamicJacobian H2) { + if constexpr (isDirectProduct) { + const size_t firstDimension = static_cast(v1.size()); + const size_t secondDimension = static_cast(v2.size()); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); + H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = Matrix::Zero(productDimension, firstDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + } + if (H2) { + *H2 = Matrix::Zero(productDimension, secondDimension); + H2->block(firstDimension, 0, secondDimension, secondDimension) = + D_h_second; + } + return ProductLieGroup(g, h); + } else { + return Action::template Expmap(v1, v2, H1, H2); + } +} + +template +typename ProductLieGroup::TangentVector +ProductLieGroup::Logmap(const ProductLieGroup& p, + ChartJacobian Hp) { + if constexpr (isDirectProduct) { + const size_t firstDimension = p.firstDim(); + const size_t secondDimension = p.secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Logmap(p.first, Hp ? &D_g_first : nullptr); + typename traits::TangentVector v2 = + traits::Logmap(p.second, Hp ? &D_h_second : nullptr); + TangentVector v = + makeTangentVector(v1, v2, firstDimension, secondDimension); + if (Hp) { + *Hp = zeroJacobian(productDimension); + Hp->block(0, 0, firstDimension, firstDimension) = D_g_first; + Hp->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } + return v; + } else { + return Action::template Logmap(p, Hp); } - return v; } -template -ProductLieGroup ProductLieGroup::expmap( +template +ProductLieGroup ProductLieGroup::expmap( const TangentVector& v) const { return compose(ProductLieGroup::Expmap(v)); } -template -typename ProductLieGroup::Jacobian ProductLieGroup::AdjointMap() - const { - const auto adjG = traits::AdjointMap(this->first); - const auto adjH = traits::AdjointMap(this->second); - const size_t d1 = static_cast(adjG.rows()); - const size_t d2 = static_cast(adjH.rows()); - Jacobian adj = zeroJacobian(d1 + d2); - adj.block(0, 0, d1, d1) = adjG; - adj.block(d1, d1, d2, d2) = adjH; - return adj; +template +typename ProductLieGroup::Jacobian +ProductLieGroup::AdjointMap() const { + if constexpr (isDirectProduct) { + const auto adjG = traits::AdjointMap(this->first); + const auto adjH = traits::AdjointMap(this->second); + const size_t d1 = static_cast(adjG.rows()); + const size_t d2 = static_cast(adjH.rows()); + Jacobian adj = zeroJacobian(d1 + d2); + adj.block(0, 0, d1, d1) = adjG; + adj.block(d1, d1, d2, d2) = adjH; + return adj; + } else { + return Action::template AdjointMap(*this); + } } -template +template template -T ProductLieGroup::defaultIdentity() { +T ProductLieGroup::defaultIdentity() { if constexpr (traits::dimension == Eigen::Dynamic) { return T(); } else { @@ -310,9 +382,9 @@ T ProductLieGroup::defaultIdentity() { } } -template +template template -typename traits::TangentVector ProductLieGroup::tangentSegment( +typename traits::TangentVector ProductLieGroup::tangentSegment( const TangentVector& v, size_t start, size_t runtimeDimension) { const int startIndex = static_cast(start); const int runtimeIndex = static_cast(runtimeDimension); @@ -324,9 +396,9 @@ typename traits::TangentVector ProductLieGroup::tangentSegment( } } -template -typename ProductLieGroup::TangentVector -ProductLieGroup::makeTangentVector( +template +typename ProductLieGroup::TangentVector +ProductLieGroup::makeTangentVector( const typename traits::TangentVector& v1, const typename traits::TangentVector& v2, size_t firstDimension, size_t secondDimension) { @@ -346,9 +418,9 @@ ProductLieGroup::makeTangentVector( } } -template -typename ProductLieGroup::Jacobian ProductLieGroup::zeroJacobian( - size_t productDimension) { +template +typename ProductLieGroup::Jacobian +ProductLieGroup::zeroJacobian(size_t productDimension) { if constexpr (dimension == Eigen::Dynamic) { return Jacobian::Zero(productDimension, productDimension); } else { @@ -357,9 +429,9 @@ typename ProductLieGroup::Jacobian ProductLieGroup::zeroJacobian( } } -template -typename ProductLieGroup::Jacobian -ProductLieGroup::identityJacobian(size_t productDimension) { +template +typename ProductLieGroup::Jacobian +ProductLieGroup::identityJacobian(size_t productDimension) { if constexpr (dimension == Eigen::Dynamic) { return Jacobian::Identity(productDimension, productDimension); } else { @@ -368,8 +440,8 @@ ProductLieGroup::identityJacobian(size_t productDimension) { } } -template -void ProductLieGroup::checkMatchingDimensions( +template +void ProductLieGroup::checkMatchingDimensions( const ProductLieGroup& other, const char* operation) const { if (firstDim() != other.firstDim() || secondDim() != other.secondDim()) { throw std::invalid_argument(std::string("ProductLieGroup::") + operation + @@ -377,8 +449,8 @@ void ProductLieGroup::checkMatchingDimensions( } } -template -void ProductLieGroup::print(const std::string& s) const { +template +void ProductLieGroup::print(const std::string& s) const { std::cout << s << "ProductLieGroup" << std::endl; traits::Print(this->first, " first"); traits::Print(this->second, " second"); diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 297652204a..835323958c 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -26,16 +27,55 @@ #include #include #include +#include #include // pair #include namespace gtsam { +template +struct DirectProductAction; + +template +struct DirectProductAction + : public GroupAction, G, H> { + static constexpr ActionType type = ActionType::Left; + + H operator()(const G& g, const H& h, + OptionalJacobian::dimension, traits::dimension> Hg = + {}, + OptionalJacobian::dimension, traits::dimension> Hh = + {}) const { + if (Hg) { + if constexpr (traits::dimension == Eigen::Dynamic || + traits::dimension == Eigen::Dynamic) { + Hg->setZero(static_cast(traits::GetDimension(h)), + static_cast(traits::GetDimension(g))); + } else { + Hg->setZero(); + } + } + if (Hh) { + if constexpr (traits::dimension == Eigen::Dynamic) { + const Eigen::Index hDim = + static_cast(traits::GetDimension(h)); + Hh->setIdentity(hDim, hDim); + } else { + Hh->setIdentity(); + } + } + return h; + } +}; + /** * @brief Template to construct the product Lie group of two other Lie groups - * Assumes Lie group structure for G and H + * Assumes Lie group structure for G and H. If Action is omitted the group is + * the direct product G x H. If Action is provided the group is the left + * semidirect product G ⋉ H. */ -template +template > class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsLieGroup); GTSAM_CONCEPT_ASSERT(IsLieGroup); @@ -45,6 +85,11 @@ class ProductLieGroup : public std::pair { public: /// Base pair type typedef std::pair Base; + using FirstFactor = G; + using SecondFactor = H; + + static_assert(Action::type == ActionType::Left, + "ProductLieGroup only supports left group actions"); protected: /// Dimensions of the two subgroups @@ -52,6 +97,8 @@ class ProductLieGroup : public std::pair { inline constexpr static int dimension2 = traits::dimension; inline constexpr static bool firstDynamic = dimension1 == Eigen::Dynamic; inline constexpr static bool secondDynamic = dimension2 == Eigen::Dynamic; + inline constexpr static bool isDirectProduct = + std::is_same_v>; public: /// Manifold dimension @@ -74,6 +121,7 @@ class ProductLieGroup : public std::pair { Eigen::Matrix>; using Jacobian1 = typename traits::Jacobian; using Jacobian2 = typename traits::Jacobian; + using DynamicJacobian = OptionalJacobian; public: /// @name Standard Constructors @@ -157,8 +205,7 @@ class ProductLieGroup : public std::pair { static ProductLieGroup Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, - OptionalJacobian H1 = {}, - OptionalJacobian H2 = {}); + DynamicJacobian H1 = {}, DynamicJacobian H2 = {}); /// Logarithmic map static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}); @@ -523,9 +570,9 @@ class PowerLieGroup }; /// Traits specialization for ProductLieGroup -template -struct traits> - : internal::LieGroup> {}; +template +struct traits> + : internal::LieGroup> {}; /// Traits specialization for PowerLieGroup template diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index 37c30288d6..4590cb4fbb 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -9,16 +9,17 @@ * -------------------------------1------------------------------------------- */ - + /** * @file testActionProductLieGroup.cpp * @date April, 2026 - * @author Frank Dellaert + * @author Rohan Bansal + * @author Jennifer Oum * @brief unit tests for action-parameterized product Lie groups */ #include -#include +#include #include #include #include @@ -31,7 +32,8 @@ using namespace gtsam; constexpr double kTol = 1e-9; using Product = ProductLieGroup; -using DirectActionProduct = ActionProductLieGroup; +using ExplicitDirectProduct = + ProductLieGroup>; struct Rot3VectorAction : public GroupAction { static constexpr ActionType type = ActionType::Left; @@ -75,8 +77,7 @@ struct Rot3VectorAction : public GroupAction { } }; -using Semidirect = - ActionProductLieGroup; +using Semidirect = ProductLieGroup; namespace { @@ -114,44 +115,44 @@ Pose3 asPose3(const Semidirect& state) { } // namespace /* ************************************************************************* */ -TEST(Lie, ActionProductLieGroupDirect) { - GTSAM_CONCEPT_ASSERT(IsGroup); - GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); +TEST(Lie, ProductLieGroupExplicitDirectAction) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); const Product state(Point2(1, 2), Pose2(3, 4, 5)); const Product other(Point2(-0.5, 0.25), Pose2(-1, 2, -0.4)); - const DirectActionProduct actionState(state.first, state.second); - const DirectActionProduct actionOther(other.first, other.second); + const ExplicitDirectProduct actionState(state.first, state.second); + const ExplicitDirectProduct actionOther(other.first, other.second); const Product defaultComposed = state.compose(other); - const DirectActionProduct actionComposed = actionState.compose(actionOther); + const ExplicitDirectProduct actionComposed = actionState.compose(actionOther); EXPECT(assert_equal(defaultComposed.first, actionComposed.first, kTol)); EXPECT(assert_equal(defaultComposed.second, actionComposed.second, kTol)); const Product defaultBetween = state.between(other); - const DirectActionProduct actionBetween = actionState.between(actionOther); + const ExplicitDirectProduct actionBetween = actionState.between(actionOther); EXPECT(assert_equal(defaultBetween.first, actionBetween.first, kTol)); EXPECT(assert_equal(defaultBetween.second, actionBetween.second, kTol)); const Product defaultInverse = state.inverse(); - const DirectActionProduct actionInverse = actionState.inverse(); + const ExplicitDirectProduct actionInverse = actionState.inverse(); EXPECT(assert_equal(defaultInverse.first, actionInverse.first, kTol)); EXPECT(assert_equal(defaultInverse.second, actionInverse.second, kTol)); Vector5 xi; xi << 0.1, -0.2, 0.05, 0.1, -0.15; const Product defaultExp = Product::Expmap(xi); - const DirectActionProduct actionExp = DirectActionProduct::Expmap(xi); + const ExplicitDirectProduct actionExp = ExplicitDirectProduct::Expmap(xi); EXPECT(assert_equal(defaultExp.first, actionExp.first, kTol)); EXPECT(assert_equal(defaultExp.second, actionExp.second, kTol)); EXPECT(assert_equal(Product::Logmap(defaultExp), - DirectActionProduct::Logmap(actionExp), kTol)); + ExplicitDirectProduct::Logmap(actionExp), kTol)); EXPECT(assert_equal(state.AdjointMap(), actionState.AdjointMap(), kTol)); } /* ************************************************************************* */ -TEST(Lie, ActionProductLieGroupSemidirect) { +TEST(Lie, ProductLieGroupSemidirectAction) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); GTSAM_CONCEPT_ASSERT(IsLieGroup); From a88691fd4e9925d9ec630ea04ad5e86298856cad Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 03:12:17 -0400 Subject: [PATCH 06/21] [skip ci] remove unused --- gtsam/base/ProductLieGroup-inl.h | 10 +++++++--- gtsam/base/ProductLieGroup.h | 6 ++---- tests/testActionProductLieGroup.cpp | 6 ++---- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 2cc7a3fdca..7f14a816e2 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -278,8 +278,11 @@ ProductLieGroup ProductLieGroup::Expmap( const auto v1 = tangentSegment(v, 0, firstDimension); const auto v2 = tangentSegment(v, firstDimension, secondDimension); ProductLieGroup result = - Expmap(v1, v2, Hv ? DynamicJacobian(D_g_first) : DynamicJacobian(), - Hv ? DynamicJacobian(D_h_second) : DynamicJacobian()); + Expmap(v1, v2, + Hv ? OptionalJacobian(D_g_first) + : OptionalJacobian(), + Hv ? OptionalJacobian(D_h_second) + : OptionalJacobian()); if (Hv) { const size_t productDimension = combinedDimension(firstDimension, secondDimension); @@ -295,7 +298,8 @@ template ProductLieGroup ProductLieGroup::Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, - DynamicJacobian H1, DynamicJacobian H2) { + OptionalJacobian H1, + OptionalJacobian H2) { if constexpr (isDirectProduct) { const size_t firstDimension = static_cast(v1.size()); const size_t secondDimension = static_cast(v2.size()); diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 835323958c..64ce02178c 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -85,8 +85,6 @@ class ProductLieGroup : public std::pair { public: /// Base pair type typedef std::pair Base; - using FirstFactor = G; - using SecondFactor = H; static_assert(Action::type == ActionType::Left, "ProductLieGroup only supports left group actions"); @@ -121,7 +119,6 @@ class ProductLieGroup : public std::pair { Eigen::Matrix>; using Jacobian1 = typename traits::Jacobian; using Jacobian2 = typename traits::Jacobian; - using DynamicJacobian = OptionalJacobian; public: /// @name Standard Constructors @@ -205,7 +202,8 @@ class ProductLieGroup : public std::pair { static ProductLieGroup Expmap( const Eigen::Ref::TangentVector>& v1, const Eigen::Ref::TangentVector>& v2, - DynamicJacobian H1 = {}, DynamicJacobian H2 = {}); + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}); /// Logarithmic map static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}); diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index 4590cb4fbb..a97cf8a9db 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -46,10 +46,8 @@ struct Rot3VectorAction : public GroupAction { template static ProductType Expmap( - const Eigen::Ref:: - TangentVector>& w, - const Eigen::Ref:: - TangentVector>& rho, + const Eigen::Ref::TangentVector>& w, + const Eigen::Ref::TangentVector>& rho, OptionalJacobian H1 = {}, OptionalJacobian H2 = {}) { Matrix6 Hpose; From a2d9b0f175bcd2b22e2bf011110e625bc3acd21c Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 03:20:15 -0400 Subject: [PATCH 07/21] format, fix tests --- gtsam/base/ProductLieGroup-inl.h | 13 ++-- gtsam/base/ProductLieGroup.h | 13 ++-- tests/testActionProductLieGroup.cpp | 117 +++++++++++++++++++--------- 3 files changed, 93 insertions(+), 50 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 7f14a816e2..7c90d3f6c2 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -66,10 +66,9 @@ ProductLieGroup ProductLieGroup::retract( Jacobian1 D_g_second; Jacobian2 D_h_first; Jacobian2 D_h_second; - G g = - traits::Retract(this->first, tangentSegment(v, 0, firstDimension), - H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); + G g = traits::Retract( + this->first, tangentSegment(v, 0, firstDimension), + H1 ? &D_g_first : nullptr, H2 ? &D_g_second : nullptr); H h = traits::Retract( this->second, tangentSegment(v, firstDimension, secondDimension), H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); @@ -116,9 +115,9 @@ ProductLieGroup::localCoordinates(const ProductLieGroup& g, typename traits::TangentVector v1 = traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, H2 ? &D_g_second : nullptr); - typename traits::TangentVector v2 = traits::Local( - this->second, g.second, H1 ? &D_h_first : nullptr, - H2 ? &D_h_second : nullptr); + typename traits::TangentVector v2 = + traits::Local(this->second, g.second, H1 ? &D_h_first : nullptr, + H2 ? &D_h_second : nullptr); if (H1) { *H1 = zeroJacobian(productDimension); H1->block(0, 0, firstDimension, firstDimension) = D_g_first; diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 64ce02178c..7d4dd8ae71 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -41,11 +41,11 @@ struct DirectProductAction : public GroupAction, G, H> { static constexpr ActionType type = ActionType::Left; - H operator()(const G& g, const H& h, - OptionalJacobian::dimension, traits::dimension> Hg = - {}, - OptionalJacobian::dimension, traits::dimension> Hh = - {}) const { + H operator()( + const G& g, const H& h, + OptionalJacobian::dimension, traits::dimension> Hg = {}, + OptionalJacobian::dimension, traits::dimension> Hh = {}) + const { if (Hg) { if constexpr (traits::dimension == Eigen::Dynamic || traits::dimension == Eigen::Dynamic) { @@ -74,8 +74,7 @@ struct DirectProductAction * the direct product G x H. If Action is provided the group is the left * semidirect product G ⋉ H. */ -template > +template > class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsLieGroup); GTSAM_CONCEPT_ASSERT(IsLieGroup); diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index a97cf8a9db..f9751c2115 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -9,7 +9,7 @@ * -------------------------------1------------------------------------------- */ - + /** * @file testActionProductLieGroup.cpp * @date April, 2026 @@ -79,6 +79,46 @@ using Semidirect = ProductLieGroup; namespace { +Product directState() { return Product(Point2(1, 2), Pose2(3, 4, 5)); } + +Product directOther() { + return Product(Point2(-0.5, 0.25), Pose2(-1, 2, -0.4)); +} + +Vector5 directXi() { + Vector5 xi; + xi << 0.1, -0.2, 0.05, 0.1, -0.15; + return xi; +} + +Semidirect semidirectState1() { + return Semidirect(Rot3::RzRyRx(0.1, 0.2, -0.3), Vector3(1.0, -0.5, 0.25)); +} + +Semidirect semidirectState2() { + return Semidirect(Rot3::RzRyRx(-0.2, 0.1, 0.15), Vector3(-0.75, 0.4, 1.2)); +} + +Semidirect semidirectState3() { + return Semidirect(Rot3::RzRyRx(0.2, -0.1, 0.05), Vector3(0.3, -0.6, 0.8)); +} + +Semidirect semidirectState4() { + return Semidirect(Rot3::RzRyRx(0.1, -0.2, 0.3), Vector3(0.4, -0.1, 0.2)); +} + +Vector6 semidirectXi() { + Vector6 xi; + xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; + return xi; +} + +Vector6 retractDelta() { + Vector6 delta; + delta << 0.05, -0.04, 0.03, 0.1, -0.2, 0.05; + return delta; +} + Semidirect composeSemidirectProxy(const Semidirect& A, const Semidirect& B) { return A.compose(B); } @@ -113,13 +153,15 @@ Pose3 asPose3(const Semidirect& state) { } // namespace /* ************************************************************************* */ +// Verify the explicit direct-action specialization matches the original +// direct-product behavior. TEST(Lie, ProductLieGroupExplicitDirectAction) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); GTSAM_CONCEPT_ASSERT(IsLieGroup); - const Product state(Point2(1, 2), Pose2(3, 4, 5)); - const Product other(Point2(-0.5, 0.25), Pose2(-1, 2, -0.4)); + const Product state = directState(); + const Product other = directOther(); const ExplicitDirectProduct actionState(state.first, state.second); const ExplicitDirectProduct actionOther(other.first, other.second); @@ -138,8 +180,7 @@ TEST(Lie, ProductLieGroupExplicitDirectAction) { EXPECT(assert_equal(defaultInverse.first, actionInverse.first, kTol)); EXPECT(assert_equal(defaultInverse.second, actionInverse.second, kTol)); - Vector5 xi; - xi << 0.1, -0.2, 0.05, 0.1, -0.15; + const Vector5 xi = directXi(); const Product defaultExp = Product::Expmap(xi); const ExplicitDirectProduct actionExp = ExplicitDirectProduct::Expmap(xi); EXPECT(assert_equal(defaultExp.first, actionExp.first, kTol)); @@ -150,6 +191,8 @@ TEST(Lie, ProductLieGroupExplicitDirectAction) { } /* ************************************************************************* */ +// Verify the semidirect product obeys the left action law and matches Pose3 +// behavior. TEST(Lie, ProductLieGroupSemidirectAction) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); @@ -162,29 +205,27 @@ TEST(Lie, ProductLieGroupSemidirectAction) { EXPECT_LEFT_ACTION(action, R1, R2, t); const Semidirect identity; - Vector6 xi; - xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; + const Vector6 xi = semidirectXi(); const Semidirect actual = identity.expmap(xi); EXPECT(assert_equal(Pose3::Expmap(xi), asPose3(actual), kTol)); EXPECT(assert_equal(xi, identity.logmap(actual), kTol)); - const Semidirect a(Rot3::RzRyRx(0.1, 0.2, -0.1), - Vector3(1.0, -2.0, 0.5)); - const Semidirect b(Rot3::RzRyRx(-0.3, 0.15, 0.2), - Vector3(-0.25, 0.4, 1.5)); - const Semidirect c(Rot3::RzRyRx(0.2, -0.1, 0.05), - Vector3(0.3, -0.6, 0.8)); + const Semidirect a = + Semidirect(Rot3::RzRyRx(0.1, 0.2, -0.1), Vector3(1.0, -2.0, 0.5)); + const Semidirect b = + Semidirect(Rot3::RzRyRx(-0.3, 0.15, 0.2), Vector3(-0.25, 0.4, 1.5)); + const Semidirect c = semidirectState3(); EXPECT(assert_equal(asPose3((a * b) * c), asPose3(a * (b * c)), kTol)); EXPECT(assert_equal(asPose3(a * a.inverse()), Pose3(), kTol)); EXPECT(assert_equal(asPose3(a * b), asPose3(a) * asPose3(b), kTol)); } /* ************************************************************************* */ +// Check semidirect compose values and Jacobians against Pose3 and numerical +// derivatives. TEST(testActionProduct, compose) { - const Semidirect state1(Rot3::RzRyRx(0.1, 0.2, -0.3), - Vector3(1.0, -0.5, 0.25)); - const Semidirect state2(Rot3::RzRyRx(-0.2, 0.1, 0.15), - Vector3(-0.75, 0.4, 1.2)); + const Semidirect state1 = semidirectState1(); + const Semidirect state2 = semidirectState2(); Matrix actH1, actH2; const Semidirect actual = state1.compose(state2, actH1, actH2); @@ -193,17 +234,18 @@ TEST(testActionProduct, compose) { const Matrix numericH2 = numericalDerivative22(composeSemidirectProxy, state1, state2); - EXPECT(assert_equal(asPose3(actual), asPose3(state1) * asPose3(state2), kTol)); + EXPECT( + assert_equal(asPose3(actual), asPose3(state1) * asPose3(state2), kTol)); EXPECT(assert_equal(numericH1, actH1, 1e-6)); EXPECT(assert_equal(numericH2, actH2, 1e-6)); } /* ************************************************************************* */ +// Check semidirect between values and Jacobians against Pose3 and numerical +// derivatives. TEST(testActionProduct, between) { - const Semidirect state1(Rot3::RzRyRx(0.1, 0.2, -0.3), - Vector3(1.0, -0.5, 0.25)); - const Semidirect state2(Rot3::RzRyRx(-0.2, 0.1, 0.15), - Vector3(-0.75, 0.4, 1.2)); + const Semidirect state1 = semidirectState1(); + const Semidirect state2 = semidirectState2(); Matrix actH1, actH2; const Semidirect actual = state1.between(state2, actH1, actH2); @@ -219,9 +261,11 @@ TEST(testActionProduct, between) { } /* ************************************************************************* */ +// Check the semidirect inverse and its Jacobian against Pose3 and numerical +// derivatives. TEST(testActionProduct, inverse) { - const Semidirect state(Rot3::RzRyRx(0.2, -0.1, 0.05), - Vector3(0.5, -1.2, 0.8)); + const Semidirect state = + Semidirect(Rot3::RzRyRx(0.2, -0.1, 0.05), Vector3(0.5, -1.2, 0.8)); Matrix actH; const Semidirect actual = state.inverse(actH); @@ -232,9 +276,10 @@ TEST(testActionProduct, inverse) { } /* ************************************************************************* */ +// Check the semidirect Expmap and its Jacobian against Pose3 and numerical +// derivatives. TEST(testActionProduct, Expmap) { - Vector6 xi; - xi << 0.1, -0.2, 0.3, 0.4, -0.1, 0.2; + const Vector6 xi = semidirectXi(); Matrix actH; const Semidirect actual = Semidirect::Expmap(xi, actH); @@ -245,9 +290,10 @@ TEST(testActionProduct, Expmap) { } /* ************************************************************************* */ +// Check the semidirect Logmap and its Jacobian against Pose3 and numerical +// derivatives. TEST(testActionProduct, Logmap) { - const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), - Vector3(0.4, -0.1, 0.2)); + const Semidirect state = semidirectState4(); Matrix actH; const Vector6 actual = Semidirect::Logmap(state, actH); @@ -258,24 +304,23 @@ TEST(testActionProduct, Logmap) { } /* ************************************************************************* */ +// Check that the semidirect adjoint matches the Pose3 adjoint. TEST(testActionProduct, AdjointMap) { - const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), - Vector3(0.4, -0.1, 0.2)); + const Semidirect state = semidirectState4(); EXPECT(assert_equal(asPose3(state).AdjointMap(), state.AdjointMap(), kTol)); } /* ************************************************************************* */ +// Check retract/localCoordinates consistency and both Jacobians against +// numerical derivatives. TEST(testActionProduct, retractAndLocalCoordinates) { - const Semidirect state(Rot3::RzRyRx(0.1, -0.2, 0.3), - Vector3(0.4, -0.1, 0.2)); - Vector6 delta; - delta << 0.05, -0.04, 0.03, 0.1, -0.2, 0.05; + const Semidirect state = semidirectState4(); + const Vector6 delta = retractDelta(); Matrix retractH1, retractH2, localH1, localH2; const Semidirect updated = state.retract(delta, retractH1, retractH2); - const Vector6 recovered = - state.localCoordinates(updated, localH1, localH2); + const Vector6 recovered = state.localCoordinates(updated, localH1, localH2); EXPECT(assert_equal(asPose3(updated), asPose3(state).retract(delta), kTol)); EXPECT(assert_equal(delta, recovered, kTol)); From 848d9cda8ea2a0e1e4f4b9f97ab36ed0c1b4c168 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 12:53:55 -0400 Subject: [PATCH 08/21] remove direct product action, fix tests, clang format --- gtsam/base/ProductLieGroup.h | 50 +++++++---------------------- tests/testActionProductLieGroup.cpp | 13 ++++---- 2 files changed, 18 insertions(+), 45 deletions(-) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 7d4dd8ae71..e04fff6d80 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -33,40 +33,15 @@ namespace gtsam { -template -struct DirectProductAction; - -template -struct DirectProductAction - : public GroupAction, G, H> { - static constexpr ActionType type = ActionType::Left; - - H operator()( - const G& g, const H& h, - OptionalJacobian::dimension, traits::dimension> Hg = {}, - OptionalJacobian::dimension, traits::dimension> Hh = {}) - const { - if (Hg) { - if constexpr (traits::dimension == Eigen::Dynamic || - traits::dimension == Eigen::Dynamic) { - Hg->setZero(static_cast(traits::GetDimension(h)), - static_cast(traits::GetDimension(g))); - } else { - Hg->setZero(); - } - } - if (Hh) { - if constexpr (traits::dimension == Eigen::Dynamic) { - const Eigen::Index hDim = - static_cast(traits::GetDimension(h)); - Hh->setIdentity(hDim, hDim); - } else { - Hh->setIdentity(); - } - } - return h; - } -}; +namespace product_lie_group { + +template +struct IsValidAction : std::bool_constant {}; + +template <> +struct IsValidAction : std::true_type {}; + +} // namespace product_lie_group /** * @brief Template to construct the product Lie group of two other Lie groups @@ -74,7 +49,7 @@ struct DirectProductAction * the direct product G x H. If Action is provided the group is the left * semidirect product G ⋉ H. */ -template > +template class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsLieGroup); GTSAM_CONCEPT_ASSERT(IsLieGroup); @@ -85,7 +60,7 @@ class ProductLieGroup : public std::pair { /// Base pair type typedef std::pair Base; - static_assert(Action::type == ActionType::Left, + static_assert(product_lie_group::IsValidAction::value, "ProductLieGroup only supports left group actions"); protected: @@ -94,8 +69,7 @@ class ProductLieGroup : public std::pair { inline constexpr static int dimension2 = traits::dimension; inline constexpr static bool firstDynamic = dimension1 == Eigen::Dynamic; inline constexpr static bool secondDynamic = dimension2 == Eigen::Dynamic; - inline constexpr static bool isDirectProduct = - std::is_same_v>; + inline constexpr static bool isDirectProduct = std::is_void_v; public: /// Manifold dimension diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index f9751c2115..999cbbe10f 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -32,8 +32,7 @@ using namespace gtsam; constexpr double kTol = 1e-9; using Product = ProductLieGroup; -using ExplicitDirectProduct = - ProductLieGroup>; +using ExplicitDirectProduct = ProductLieGroup; struct Rot3VectorAction : public GroupAction { static constexpr ActionType type = ActionType::Left; @@ -153,8 +152,7 @@ Pose3 asPose3(const Semidirect& state) { } // namespace /* ************************************************************************* */ -// Verify the explicit direct-action specialization matches the original -// direct-product behavior. +// Verify the dummy-action specialization matches the original direct-product behavior. TEST(Lie, ProductLieGroupExplicitDirectAction) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); @@ -312,8 +310,8 @@ TEST(testActionProduct, AdjointMap) { } /* ************************************************************************* */ -// Check retract/localCoordinates consistency and both Jacobians against -// numerical derivatives. +// Check Expmap-based retract/localCoordinates consistency and both Jacobians +// against numerical derivatives. TEST(testActionProduct, retractAndLocalCoordinates) { const Semidirect state = semidirectState4(); const Vector6 delta = retractDelta(); @@ -322,7 +320,8 @@ TEST(testActionProduct, retractAndLocalCoordinates) { const Semidirect updated = state.retract(delta, retractH1, retractH2); const Vector6 recovered = state.localCoordinates(updated, localH1, localH2); - EXPECT(assert_equal(asPose3(updated), asPose3(state).retract(delta), kTol)); + EXPECT(assert_equal(asPose3(updated), + asPose3(state).compose(Pose3::Expmap(delta)), kTol)); EXPECT(assert_equal(delta, recovered, kTol)); const Matrix numericRetractH1 = From 2784039ed52dcd3e5cd76887468d4e8d55d62912 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 12:54:18 -0400 Subject: [PATCH 09/21] clang format --- tests/testActionProductLieGroup.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index 999cbbe10f..dfc2bb6840 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -152,7 +152,8 @@ Pose3 asPose3(const Semidirect& state) { } // namespace /* ************************************************************************* */ -// Verify the dummy-action specialization matches the original direct-product behavior. +// Verify the dummy-action specialization matches the original direct-product +// behavior. TEST(Lie, ProductLieGroupExplicitDirectAction) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); From 89f9139125d009c6b3d132a4ef496f5f738ef559 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Wed, 8 Apr 2026 12:56:19 -0400 Subject: [PATCH 10/21] remove static assert --- gtsam/base/ProductLieGroup.h | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index e04fff6d80..0fe36d8a2c 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -33,16 +33,6 @@ namespace gtsam { -namespace product_lie_group { - -template -struct IsValidAction : std::bool_constant {}; - -template <> -struct IsValidAction : std::true_type {}; - -} // namespace product_lie_group - /** * @brief Template to construct the product Lie group of two other Lie groups * Assumes Lie group structure for G and H. If Action is omitted the group is @@ -60,9 +50,6 @@ class ProductLieGroup : public std::pair { /// Base pair type typedef std::pair Base; - static_assert(product_lie_group::IsValidAction::value, - "ProductLieGroup only supports left group actions"); - protected: /// Dimensions of the two subgroups inline constexpr static int dimension1 = traits::dimension; From f192d64ac510823973e557ccd46866741e1d25ed Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Mon, 13 Apr 2026 02:13:53 -0400 Subject: [PATCH 11/21] temp --- gtsam/base/ProductLieGroup-inl.h | 93 +++++++------------------------- 1 file changed, 20 insertions(+), 73 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 7c90d3f6c2..ec3b969fc4 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -28,6 +28,8 @@ ProductLieGroup ProductLieGroup::operator*( return ProductLieGroup(traits::Compose(this->first, other.first), traits::Compose(this->second, other.second)); } else { + // Semidirect multiplication first moves the RHS second factor by the + // LHS first factor's action, then composes in H. const Action action{}; const H actedSecond = action(this->first, other.second); return ProductLieGroup(traits::Compose(this->first, other.first), @@ -62,6 +64,8 @@ ProductLieGroup ProductLieGroup::retract( } if constexpr (isDirectProduct) { + // Keep the direct product on the component charts. Using Expmap here would + // change behavior for factors with custom retract/localCoordinates charts. Jacobian1 D_g_first; Jacobian1 D_g_second; Jacobian2 D_h_first; @@ -108,6 +112,7 @@ ProductLieGroup::localCoordinates(const ProductLieGroup& g, combinedDimension(firstDimension, secondDimension); if constexpr (isDirectProduct) { + // Keep this componentwise for the same chart reason as retract(). Jacobian1 D_g_first; Jacobian1 D_g_second; Jacobian2 D_h_first; @@ -146,90 +151,27 @@ template ProductLieGroup ProductLieGroup::compose( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "compose"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - - if constexpr (isDirectProduct) { - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = - traits::Compose(this->first, other.first, H1 ? &D_g_first : nullptr); - H h = traits::Compose(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); - } else { - const ProductLieGroup result = (*this) * other; - if (H1) *H1 = other.inverse().AdjointMap(); - if (H2) *H2 = identityJacobian(productDimension); - return result; - } + const ProductLieGroup result = (*this) * other; + if (H1) *H1 = other.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(dim()); + return result; } template ProductLieGroup ProductLieGroup::between( const ProductLieGroup& other, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(other, "between"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - - if constexpr (isDirectProduct) { - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = - traits::Between(this->first, other.first, H1 ? &D_g_first : nullptr); - H h = traits::Between(this->second, other.second, - H1 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - if (H2) *H2 = identityJacobian(productDimension); - return ProductLieGroup(g, h); - } else { - const ProductLieGroup result = this->inverse() * other; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = identityJacobian(productDimension); - return result; - } + const ProductLieGroup result = this->inverse() * other; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = identityJacobian(dim()); + return result; } template ProductLieGroup ProductLieGroup::inverse( ChartJacobian D) const { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - - if constexpr (isDirectProduct) { - Jacobian1 D_g_first; - Jacobian2 D_h_second; - G g = traits::Inverse(this->first, D ? &D_g_first : nullptr); - H h = traits::Inverse(this->second, D ? &D_h_second : nullptr); - if (D) { - *D = zeroJacobian(productDimension); - D->block(0, 0, firstDimension, firstDimension) = D_g_first; - D->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; - } - return ProductLieGroup(g, h); - } else { - if (D) *D = -AdjointMap(); - return inverse(); - } + if (D) *D = -AdjointMap(); + return inverse(); } template @@ -319,6 +261,8 @@ ProductLieGroup ProductLieGroup::Expmap( } return ProductLieGroup(g, h); } else { + // A semidirect Expmap is not determined by Action::operator() alone. + // The action must provide the Lie-level formula for its coupling. return Action::template Expmap(v1, v2, H1, H2); } } @@ -348,6 +292,8 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, } return v; } else { + // As with Expmap, the semidirect Logmap depends on the action's Lie-level + // coupling and cannot be inferred from the group action alone. return Action::template Logmap(p, Hp); } } @@ -371,6 +317,7 @@ ProductLieGroup::AdjointMap() const { adj.block(d1, d1, d2, d2) = adjH; return adj; } else { + // The semidirect adjoint carries the action-specific coupling blocks. return Action::template AdjointMap(*this); } } From 32b8028011e65ffc87088d9519f4567f2da29ea8 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Tue, 14 Apr 2026 13:09:19 -0400 Subject: [PATCH 12/21] [skip ci] address comments, tighten --- gtsam/base/ProductLieGroup-inl.h | 100 +++++++++++++++++----------- gtsam/base/ProductLieGroup.h | 31 +++++++-- tests/testActionProductLieGroup.cpp | 66 +----------------- 3 files changed, 90 insertions(+), 107 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index ec3b969fc4..c73c16e23e 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -39,25 +39,22 @@ ProductLieGroup ProductLieGroup::operator*( template ProductLieGroup ProductLieGroup::inverse() const { + const G gInv = traits::Inverse(this->first); + const H hInv = traits::Inverse(this->second); if constexpr (isDirectProduct) { - return ProductLieGroup(traits::Inverse(this->first), - traits::Inverse(this->second)); + return ProductLieGroup(gInv, hInv); } else { - const Action action{}; - const G gInv = traits::Inverse(this->first); - const H hInv = traits::Inverse(this->second); - return ProductLieGroup(gInv, action(gInv, hInv)); + return ProductLieGroup(gInv, Action{}(gInv, hInv)); } } template ProductLieGroup ProductLieGroup::retract( const TangentVector& v, ChartJacobian H1, ChartJacobian H2) const { - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); - if (static_cast(v.size()) != productDimension) { + const size_t d1 = firstDim(); + const size_t d2 = secondDim(); + const size_t d = combinedDimension(d1, d2); + if (static_cast(v.size()) != d) { throw std::invalid_argument( "ProductLieGroup::retract tangent dimension does not match product " "dimension"); @@ -70,23 +67,21 @@ ProductLieGroup ProductLieGroup::retract( Jacobian1 D_g_second; Jacobian2 D_h_first; Jacobian2 D_h_second; - G g = traits::Retract( - this->first, tangentSegment(v, 0, firstDimension), - H1 ? &D_g_first : nullptr, H2 ? &D_g_second : nullptr); - H h = traits::Retract( - this->second, tangentSegment(v, firstDimension, secondDimension), - H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); + G g = traits::Retract(this->first, tangentSegment(v, 0, d1), + H1 ? &D_g_first : nullptr, + H2 ? &D_g_second : nullptr); + H h = traits::Retract(this->second, tangentSegment(v, d1, d2), + H1 ? &D_h_first : nullptr, + H2 ? &D_h_second : nullptr); if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; + *H1 = zeroJacobian(d); + H1->block(0, 0, d1, d1) = D_g_first; + H1->block(d1, d1, d2, d2) = D_h_first; } if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; + *H2 = zeroJacobian(d); + H2->block(0, 0, d1, d1) = D_g_second; + H2->block(d1, d1, d2, d2) = D_h_second; } return ProductLieGroup(g, h); } else { @@ -106,10 +101,9 @@ ProductLieGroup::localCoordinates(const ProductLieGroup& g, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(g, "localCoordinates"); - const size_t firstDimension = firstDim(); - const size_t secondDimension = secondDim(); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); + const size_t d1 = firstDim(); + const size_t d2 = secondDim(); + const size_t d = combinedDimension(d1, d2); if constexpr (isDirectProduct) { // Keep this componentwise for the same chart reason as retract(). @@ -124,18 +118,16 @@ ProductLieGroup::localCoordinates(const ProductLieGroup& g, traits::Local(this->second, g.second, H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); if (H1) { - *H1 = zeroJacobian(productDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; - H1->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_first; + *H1 = zeroJacobian(d); + H1->block(0, 0, d1, d1) = D_g_first; + H1->block(d1, d1, d2, d2) = D_h_first; } if (H2) { - *H2 = zeroJacobian(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; + *H2 = zeroJacobian(d); + H2->block(0, 0, d1, d1) = D_g_second; + H2->block(d1, d1, d2, d2) = D_h_second; } - return makeTangentVector(v1, v2, firstDimension, secondDimension); + return makeTangentVector(v1, v2, d1, d2); } else { const ProductLieGroup relative = between(g); Jacobian D_logmap; @@ -317,8 +309,36 @@ ProductLieGroup::AdjointMap() const { adj.block(d1, d1, d2, d2) = adjH; return adj; } else { - // The semidirect adjoint carries the action-specific coupling blocks. - return Action::template AdjointMap(*this); + // Generic semidirect AdjointMap assembled from the action's Jacobians: + // + // Ad_(g,h) = [[ Ad_G(g), 0 ], + // [ -Jg * Ad_G(g), Jh ]] + // + // where Jg = D_g φ(e_G, h) (action Jacobian w.r.t. G at identity) + // Jh = D_h φ(g, e_H) (action Jacobian w.r.t. H at identity) + // + // Both come directly from the action's operator() Jacobian arguments. + const size_t d1 = firstDim(); + const size_t d2 = secondDim(); + const size_t d = combinedDimension(d1, d2); + + const Action action{}; + + // Jg: DimH × DimG — evaluated at (e_G, h) + ActionJacobianG Jg; + action(defaultIdentity(), this->second, &Jg, {}); + + // Jh: DimH × DimH — evaluated at (g, e_H) + Jacobian2 Jh; + action(this->first, defaultIdentity(), {}, &Jh); + + const auto adjG = traits::AdjointMap(this->first); + + Jacobian adj = zeroJacobian(d); + adj.block(0, 0, d1, d1) = adjG; + adj.block(d1, d1, d2, d2) = Jh; + adj.block(d1, 0, d2, d1) = -Jg * adjG; + return adj; } } diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 0fe36d8a2c..04ca3f1456 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -34,10 +34,28 @@ namespace gtsam { /** - * @brief Template to construct the product Lie group of two other Lie groups - * Assumes Lie group structure for G and H. If Action is omitted the group is - * the direct product G x H. If Action is provided the group is the left - * semidirect product G ⋉ H. + * @brief Product Lie group of G and H, optionally with a semidirect structure. + * + * When Action is omitted (default void), this is the direct product G × H: + * operations are componentwise and tangent vectors concatenate independently. + * + * When Action is provided, this is the left semidirect product G ⋉ H, where G + * acts on H via the left group action φ: G × H → H. The group law becomes: + * + * (g₁, h₁) · (g₂, h₂) = (g₁g₂, h₁ · φ(g₁, h₂)) + * (g, h)⁻¹ = (g⁻¹, φ(g⁻¹, h⁻¹)) + * + * Action must derive from GroupAction and implement: + * - operator()(g, h, Hg={}, Hh={}) — the group action with optional + * Jacobians w.r.t. g (DimH×DimG) and h (DimH×DimH). These Jacobians are + * used to automatically derive the AdjointMap. + * - static Expmap(v1, v2, H1={}, H2={}) — product exponential + * map (cannot be derived generically from the action alone). + * - static Logmap(p, H={}) — product logarithm. + * + * Example: SE(3) as a semidirect product: + * using SE3 = ProductLieGroup; + * where Rot3VectorAction implements φ(R, t) = R·t. */ template class ProductLieGroup : public std::pair { @@ -79,6 +97,11 @@ class ProductLieGroup : public std::pair { Eigen::Matrix>; using Jacobian1 = typename traits::Jacobian; using Jacobian2 = typename traits::Jacobian; + /// Jacobian of the action w.r.t. G (DimH × DimG), used in the generic + /// semidirect AdjointMap formula. + using ActionJacobianG = std::conditional_t< + firstDynamic || secondDynamic, Matrix, + Eigen::Matrix>; public: /// @name Standard Constructors diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index dfc2bb6840..a0432dc106 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -22,8 +22,6 @@ #include #include #include -#include -#include #include #include @@ -31,9 +29,6 @@ using namespace gtsam; constexpr double kTol = 1e-9; -using Product = ProductLieGroup; -using ExplicitDirectProduct = ProductLieGroup; - struct Rot3VectorAction : public GroupAction { static constexpr ActionType type = ActionType::Left; @@ -45,8 +40,7 @@ struct Rot3VectorAction : public GroupAction { template static ProductType Expmap( - const Eigen::Ref::TangentVector>& w, - const Eigen::Ref::TangentVector>& rho, + const Vector3& w, const Vector3& rho, OptionalJacobian H1 = {}, OptionalJacobian H2 = {}) { Matrix6 Hpose; @@ -59,8 +53,8 @@ struct Rot3VectorAction : public GroupAction { } template - static typename ProductType::TangentVector Logmap( - const ProductType& p, typename ProductType::ChartJacobian H = {}) { + static Vector6 Logmap(const ProductType& p, + OptionalJacobian<6, 6> H = {}) { Matrix6 Hpose; const Vector6 xi = Pose3::Logmap(Pose3(p.first, p.second), H ? &Hpose : nullptr); @@ -68,28 +62,12 @@ struct Rot3VectorAction : public GroupAction { return xi; } - template - static typename ProductType::Jacobian AdjointMap(const ProductType& p) { - return Pose3(p.first, p.second).AdjointMap(); - } }; using Semidirect = ProductLieGroup; namespace { -Product directState() { return Product(Point2(1, 2), Pose2(3, 4, 5)); } - -Product directOther() { - return Product(Point2(-0.5, 0.25), Pose2(-1, 2, -0.4)); -} - -Vector5 directXi() { - Vector5 xi; - xi << 0.1, -0.2, 0.05, 0.1, -0.15; - return xi; -} - Semidirect semidirectState1() { return Semidirect(Rot3::RzRyRx(0.1, 0.2, -0.3), Vector3(1.0, -0.5, 0.25)); } @@ -151,44 +129,6 @@ Pose3 asPose3(const Semidirect& state) { } // namespace -/* ************************************************************************* */ -// Verify the dummy-action specialization matches the original direct-product -// behavior. -TEST(Lie, ProductLieGroupExplicitDirectAction) { - GTSAM_CONCEPT_ASSERT(IsGroup); - GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); - - const Product state = directState(); - const Product other = directOther(); - const ExplicitDirectProduct actionState(state.first, state.second); - const ExplicitDirectProduct actionOther(other.first, other.second); - - const Product defaultComposed = state.compose(other); - const ExplicitDirectProduct actionComposed = actionState.compose(actionOther); - EXPECT(assert_equal(defaultComposed.first, actionComposed.first, kTol)); - EXPECT(assert_equal(defaultComposed.second, actionComposed.second, kTol)); - - const Product defaultBetween = state.between(other); - const ExplicitDirectProduct actionBetween = actionState.between(actionOther); - EXPECT(assert_equal(defaultBetween.first, actionBetween.first, kTol)); - EXPECT(assert_equal(defaultBetween.second, actionBetween.second, kTol)); - - const Product defaultInverse = state.inverse(); - const ExplicitDirectProduct actionInverse = actionState.inverse(); - EXPECT(assert_equal(defaultInverse.first, actionInverse.first, kTol)); - EXPECT(assert_equal(defaultInverse.second, actionInverse.second, kTol)); - - const Vector5 xi = directXi(); - const Product defaultExp = Product::Expmap(xi); - const ExplicitDirectProduct actionExp = ExplicitDirectProduct::Expmap(xi); - EXPECT(assert_equal(defaultExp.first, actionExp.first, kTol)); - EXPECT(assert_equal(defaultExp.second, actionExp.second, kTol)); - EXPECT(assert_equal(Product::Logmap(defaultExp), - ExplicitDirectProduct::Logmap(actionExp), kTol)); - EXPECT(assert_equal(state.AdjointMap(), actionState.AdjointMap(), kTol)); -} - /* ************************************************************************* */ // Verify the semidirect product obeys the left action law and matches Pose3 // behavior. From 2d83e48ae722a04a01326f75896967ef22484d99 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Tue, 14 Apr 2026 14:31:56 -0400 Subject: [PATCH 13/21] add comments, clean up a bit --- gtsam/base/ProductLieGroup-inl.h | 132 ++++++++++++---------------- gtsam/base/ProductLieGroup.h | 12 +-- tests/testActionProductLieGroup.cpp | 4 +- 3 files changed, 61 insertions(+), 87 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index c73c16e23e..d96b7f254c 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -25,13 +25,13 @@ ProductLieGroup ProductLieGroup::operator*( const ProductLieGroup& other) const { checkMatchingDimensions(other, "operator*"); if constexpr (isDirectProduct) { + // Direct product: (g₁,h₁)·(g₂,h₂) = (g₁g₂, h₁h₂) return ProductLieGroup(traits::Compose(this->first, other.first), traits::Compose(this->second, other.second)); } else { - // Semidirect multiplication first moves the RHS second factor by the - // LHS first factor's action, then composes in H. - const Action action{}; - const H actedSecond = action(this->first, other.second); + // Semidirect product: (g₁,h₁)·(g₂,h₂) = (g₁g₂, h₁·φ(g₁,h₂)). + // The LHS G-component acts on the RHS H-component before composing in H. + const H actedSecond = Action{}(this->first, other.second); return ProductLieGroup(traits::Compose(this->first, other.first), traits::Compose(this->second, actedSecond)); } @@ -42,8 +42,11 @@ ProductLieGroup ProductLieGroup::inverse() const { const G gInv = traits::Inverse(this->first); const H hInv = traits::Inverse(this->second); if constexpr (isDirectProduct) { + // Direct product: (g,h)⁻¹ = (g⁻¹, h⁻¹) return ProductLieGroup(gInv, hInv); } else { + // Semidirect product: (g,h)⁻¹ = (g⁻¹, φ(g⁻¹, h⁻¹)). + // g⁻¹ must also un-rotate h⁻¹ to undo the action baked into the group law. return ProductLieGroup(gInv, Action{}(gInv, hInv)); } } @@ -59,40 +62,28 @@ ProductLieGroup ProductLieGroup::retract( "ProductLieGroup::retract tangent dimension does not match product " "dimension"); } - - if constexpr (isDirectProduct) { - // Keep the direct product on the component charts. Using Expmap here would - // change behavior for factors with custom retract/localCoordinates charts. - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - G g = traits::Retract(this->first, tangentSegment(v, 0, d1), - H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - H h = traits::Retract(this->second, tangentSegment(v, d1, d2), - H1 ? &D_h_first : nullptr, - H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(d); - H1->block(0, 0, d1, d1) = D_g_first; - H1->block(d1, d1, d2, d2) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(d); - H2->block(0, 0, d1, d1) = D_g_second; - H2->block(d1, d1, d2, d2) = D_h_second; - } - return ProductLieGroup(g, h); - } else { - Jacobian D_expmap; - const ProductLieGroup delta = ProductLieGroup::Expmap( - v, H2 ? ChartJacobian(&D_expmap) : ChartJacobian()); - const ProductLieGroup result = compose(delta); - if (H1) *H1 = delta.inverse().AdjointMap(); - if (H2) *H2 = D_expmap; - return result; + // Expmap-based retract works for both direct and semidirect products: + // retract(p, v) = p · Expmap(v) + // The chart Jacobians are H1 = Ad(Expmap(v)⁻¹) and H2 = D_Expmap(v). + // For a direct product these are block-diagonal and match the componentwise + // formula; for a semidirect product they carry the full coupled structure. + // + // We pre-split v here so the 2-arg Expmap overload is used, which handles + // the dynamic-dynamic case correctly (the 1-arg form can't infer the split + // without an instance to query d1 and d2 from). + Matrix D_v1, D_v2; + using DynJ = OptionalJacobian; + const ProductLieGroup delta = + Expmap(tangentSegment(v, 0, d1), tangentSegment(v, d1, d2), + H2 ? DynJ(D_v1) : DynJ(), H2 ? DynJ(D_v2) : DynJ()); + const ProductLieGroup result = compose(delta); + if (H1) *H1 = delta.inverse().AdjointMap(); + if (H2) { + *H2 = zeroJacobian(d); + H2->leftCols(d1) = D_v1; + H2->rightCols(d2) = D_v2; } + return result; } template @@ -101,42 +92,18 @@ ProductLieGroup::localCoordinates(const ProductLieGroup& g, ChartJacobian H1, ChartJacobian H2) const { checkMatchingDimensions(g, "localCoordinates"); - const size_t d1 = firstDim(); - const size_t d2 = secondDim(); - const size_t d = combinedDimension(d1, d2); - - if constexpr (isDirectProduct) { - // Keep this componentwise for the same chart reason as retract(). - Jacobian1 D_g_first; - Jacobian1 D_g_second; - Jacobian2 D_h_first; - Jacobian2 D_h_second; - typename traits::TangentVector v1 = - traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, - H2 ? &D_g_second : nullptr); - typename traits::TangentVector v2 = - traits::Local(this->second, g.second, H1 ? &D_h_first : nullptr, - H2 ? &D_h_second : nullptr); - if (H1) { - *H1 = zeroJacobian(d); - H1->block(0, 0, d1, d1) = D_g_first; - H1->block(d1, d1, d2, d2) = D_h_first; - } - if (H2) { - *H2 = zeroJacobian(d); - H2->block(0, 0, d1, d1) = D_g_second; - H2->block(d1, d1, d2, d2) = D_h_second; - } - return makeTangentVector(v1, v2, d1, d2); - } else { - const ProductLieGroup relative = between(g); - Jacobian D_logmap; - TangentVector v = ProductLieGroup::Logmap( - relative, H1 || H2 ? ChartJacobian(&D_logmap) : ChartJacobian()); - if (H1) *H1 = -D_logmap * relative.inverse().AdjointMap(); - if (H2) *H2 = D_logmap; - return v; - } + // Logmap-based local coordinates work for both direct and semidirect + // products: + // localCoordinates(p, q) = Logmap(p⁻¹·q) + // For a direct product Logmap is componentwise; for a semidirect product it + // carries the full coupled Lie algebra structure from Action::Logmap. + const ProductLieGroup relative = between(g); + Jacobian D_logmap; + TangentVector v = + Logmap(relative, H1 || H2 ? ChartJacobian(&D_logmap) : ChartJacobian()); + if (H1) *H1 = -D_logmap * relative.inverse().AdjointMap(); + if (H2) *H2 = D_logmap; + return v; } template @@ -234,6 +201,9 @@ ProductLieGroup ProductLieGroup::Expmap( OptionalJacobian H1, OptionalJacobian H2) { if constexpr (isDirectProduct) { + // Direct product: the two Lie algebras are independent, so the exponential + // is componentwise: exp(v₁,v₂) = (exp_G(v₁), exp_H(v₂)). + // The Jacobian columns split cleanly between the G and H blocks. const size_t firstDimension = static_cast(v1.size()); const size_t secondDimension = static_cast(v2.size()); const size_t productDimension = @@ -253,8 +223,9 @@ ProductLieGroup ProductLieGroup::Expmap( } return ProductLieGroup(g, h); } else { - // A semidirect Expmap is not determined by Action::operator() alone. - // The action must provide the Lie-level formula for its coupling. + // Semidirect product: the Lie bracket couples the two algebra components + // via the infinitesimal action, so the exponential cannot be derived from + // Action::operator() alone. The action must supply the formula explicitly. return Action::template Expmap(v1, v2, H1, H2); } } @@ -264,6 +235,8 @@ typename ProductLieGroup::TangentVector ProductLieGroup::Logmap(const ProductLieGroup& p, ChartJacobian Hp) { if constexpr (isDirectProduct) { + // Direct product: the inverse of a componentwise Expmap is componentwise, + // so log(g,h) = (log_G(g), log_H(h)) with a block-diagonal Jacobian. const size_t firstDimension = p.firstDim(); const size_t secondDimension = p.secondDim(); const size_t productDimension = @@ -284,8 +257,8 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, } return v; } else { - // As with Expmap, the semidirect Logmap depends on the action's Lie-level - // coupling and cannot be inferred from the group action alone. + // Semidirect product: the inverse of the coupled Expmap is equally coupled + // and cannot be inferred from the action alone. return Action::template Logmap(p, Hp); } } @@ -300,6 +273,8 @@ template typename ProductLieGroup::Jacobian ProductLieGroup::AdjointMap() const { if constexpr (isDirectProduct) { + // Direct product: the two algebras are independent, so the adjoint is + // block-diagonal: Ad_(g,h) = diag(Ad_G(g), Ad_H(h)). const auto adjG = traits::AdjointMap(this->first); const auto adjH = traits::AdjointMap(this->second); const size_t d1 = static_cast(adjG.rows()); @@ -309,7 +284,8 @@ ProductLieGroup::AdjointMap() const { adj.block(d1, d1, d2, d2) = adjH; return adj; } else { - // Generic semidirect AdjointMap assembled from the action's Jacobians: + // Semidirect product: the action couples the algebra blocks, producing an + // off-diagonal term. The formula is assembled from the action's Jacobians: // // Ad_(g,h) = [[ Ad_G(g), 0 ], // [ -Jg * Ad_G(g), Jh ]] diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 04ca3f1456..58ad751352 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -46,12 +46,12 @@ namespace gtsam { * (g, h)⁻¹ = (g⁻¹, φ(g⁻¹, h⁻¹)) * * Action must derive from GroupAction and implement: - * - operator()(g, h, Hg={}, Hh={}) — the group action with optional + * - operator()(g, h, Hg={}, Hh={}): the group action with optional * Jacobians w.r.t. g (DimH×DimG) and h (DimH×DimH). These Jacobians are * used to automatically derive the AdjointMap. - * - static Expmap(v1, v2, H1={}, H2={}) — product exponential + * - static Expmap(v1, v2, H1={}, H2={}): product exponential * map (cannot be derived generically from the action alone). - * - static Logmap(p, H={}) — product logarithm. + * - static Logmap(p, H={}): product logarithm. * * Example: SE(3) as a semidirect product: * using SE3 = ProductLieGroup; @@ -99,9 +99,9 @@ class ProductLieGroup : public std::pair { using Jacobian2 = typename traits::Jacobian; /// Jacobian of the action w.r.t. G (DimH × DimG), used in the generic /// semidirect AdjointMap formula. - using ActionJacobianG = std::conditional_t< - firstDynamic || secondDynamic, Matrix, - Eigen::Matrix>; + using ActionJacobianG = + std::conditional_t>; public: /// @name Standard Constructors diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index a0432dc106..297b9b1e13 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -53,15 +53,13 @@ struct Rot3VectorAction : public GroupAction { } template - static Vector6 Logmap(const ProductType& p, - OptionalJacobian<6, 6> H = {}) { + static Vector6 Logmap(const ProductType& p, OptionalJacobian<6, 6> H = {}) { Matrix6 Hpose; const Vector6 xi = Pose3::Logmap(Pose3(p.first, p.second), H ? &Hpose : nullptr); if (H) *H = Hpose; return xi; } - }; using Semidirect = ProductLieGroup; From d83bc941c7b7c7c978d07e2ac2d2a084468e0b32 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Fri, 17 Apr 2026 20:38:30 -0400 Subject: [PATCH 14/21] add infinitessimal based on notes --- gtsam/base/GroupAction.h | 10 +++ gtsam/base/ProductLieGroup-inl.h | 131 ++++++++++++++++++++++++++-- gtsam/base/ProductLieGroup.h | 36 +++++++- tests/testActionProductLieGroup.cpp | 27 ++---- 4 files changed, 175 insertions(+), 29 deletions(-) diff --git a/gtsam/base/GroupAction.h b/gtsam/base/GroupAction.h index 348fdf89ac..7b5fb47ed3 100644 --- a/gtsam/base/GroupAction.h +++ b/gtsam/base/GroupAction.h @@ -213,6 +213,16 @@ struct InducedVectorField : public Action { * OptionalJacobian Hg = {}) const; * (for Right action) * + * For semidirect products where M is a vector space (Eigen column vector), + * providing the optional static method below enables ProductLieGroup to + * derive Expmap and Logmap automatically via the φ₁ kernel: + * + * static Eigen::Matrix generator(const TangentVector_G& u); + * + * where generator(u)·h = d/dt φ(expG(t·u), h)|_{t=0} is the infinitesimal + * generator of the representation (a DimM×DimM matrix, linear in u). + * Without this method the action must supply Expmap and Logmap explicitly. + * * @tparam Derived The user's action functor. * @tparam G The group type. * @tparam M The manifold type. diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index d96b7f254c..be24d38013 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -18,8 +18,41 @@ #pragma once +#include + namespace gtsam { +// --------------------------------------------------------------------------- +// phi01Kernel: compute φ₀(A)=exp(A) and φ₁(A)=Σ Aᵏ/(k+1)! in one shot. +// +// Identity: exp([[A, I], [0, 0]]) = [[exp(A), φ₁(A)], [0, I]] +// Proof: M = [[A,I],[0,0]]; M^k = [[A^k, A^{k-1}],[0,0]] for k≥1. +// exp(M) = I + Σ M^k/k! = [[exp(A), Σ A^{k-1}/k!],[0,I]] = [[φ₀,φ₁],[0,I]]. +// +// A single (2r)×(2r) matrix exponential yields both kernels. +// Only called when ProductLieGroup::hasGenerator is true. +// --------------------------------------------------------------------------- +template +std::pair::Jacobian2, + typename ProductLieGroup::Jacobian2> +ProductLieGroup::phi01Kernel(const Jacobian2& A) { + const int r = static_cast(A.rows()); + Eigen::MatrixXd M = Eigen::MatrixXd::Zero(2 * r, 2 * r); + M.topLeftCorner(r, r) = A; + M.topRightCorner(r, r) = Eigen::MatrixXd::Identity(r, r); + const Eigen::MatrixXd expM = M.exp(); + Jacobian2 phi0, phi1; + if constexpr (secondDynamic) { + phi0.resize(r, r); + phi1.resize(r, r); + } + phi0 = expM.topLeftCorner(r, r); + phi1 = expM.topRightCorner(r, r); + return {phi0, phi1}; +} + + + template ProductLieGroup ProductLieGroup::operator*( const ProductLieGroup& other) const { @@ -222,10 +255,55 @@ ProductLieGroup ProductLieGroup::Expmap( D_h_second; } return ProductLieGroup(g, h); + } else if constexpr (hasGenerator) { + // Generic semidirect Expmap for vector-space H via the φ₁ kernel: + // Expmap(u, v) = (expG(u), φ₁(Aφ(u)) · v) + // where Aφ(u) = Action::generator(u) is the infinitesimal generator and + // φ₁(A) = Σ Aᵏ/(k+1)! is computed from exp([[A,I],[0,0]]). + const size_t d1 = static_cast(v1.size()); + const size_t d2 = static_cast(v2.size()); + const size_t d = combinedDimension(d1, d2); + + const Jacobian2 A = Action::generator(v1); + auto [phi0, phi1] = phi01Kernel(A); + const auto phi0Solver = phi0.lu(); + + Jacobian1 D_G; + const G g = traits::Expmap(v1, H1 ? &D_G : nullptr); + const H h = phi1 * v2; + + if (H1) { + // Top rows: D Exp_G(u) in the output chart (from traits::Expmap). + // Bottom rows: D(φ₁(Aφ(u))·v) pulled back by φ₀(A)⁻¹, because + // Expmap Jacobians are expressed in local coordinates at Expmap(u,v). + // Linearity of generator: generator(u ± ε·eⱼ) = A ± ε·generator(eⱼ), + // so function values are exact and the only error is O(ε²) truncation. + *H1 = Matrix::Zero(d, d1); + H1->topRows(d1) = D_G; + const double eps = 1e-5; + typename traits::TangentVector ej; + if constexpr (firstDynamic) ej.resize(static_cast(d1)); + ej.setZero(); + for (Eigen::Index j = 0; j < static_cast(d1); ++j) { + ej(j) = 1.0; + const Jacobian2 Bj = Action::generator(ej); + const Jacobian2 phi1p = phi01Kernel(A + eps * Bj).second; + const Jacobian2 phi1m = phi01Kernel(A - eps * Bj).second; + const typename traits::TangentVector dh = + (phi1p - phi1m) * v2 / (2.0 * eps); + H1->col(j).tail(d2) = phi0Solver.solve(dh); + ej(j) = 0.0; + } + } + if (H2) { + // ∂(φ₁(A)·v)/∂v = φ₁(A) in coordinates, then pulled back by φ₀(A)⁻¹ + // to match the output chart. For SO(3) this is Rᵀ J_l = J_r. + *H2 = Matrix::Zero(d, d2); + H2->bottomRows(d2) = phi0Solver.solve(phi1); + } + return ProductLieGroup(g, h); } else { - // Semidirect product: the Lie bracket couples the two algebra components - // via the infinitesimal action, so the exponential cannot be derived from - // Action::operator() alone. The action must supply the formula explicitly. + // Semidirect product with no generator: action must supply the formula. return Action::template Expmap(v1, v2, H1, H2); } } @@ -256,9 +334,52 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, secondDimension) = D_h_second; } return v; + } else if constexpr (hasGenerator) { + // Generic semidirect Logmap for vector-space H via the φ₁ kernel: + // Logmap(g, h) = (logG(g), φ₁(Aφ(logG(g)))⁻¹ · h) + // This is the exact inverse of the Expmap formula above. + const size_t d1 = p.firstDim(); + const size_t d2 = p.secondDim(); + const size_t d = combinedDimension(d1, d2); + + Jacobian1 D_G; + const auto v1 = traits::Logmap(p.first, Hp ? &D_G : nullptr); + const Jacobian2 A = Action::generator(v1); + auto [phi0, phi1] = phi01Kernel(A); + const typename traits::TangentVector v2 = phi1.lu().solve(p.second); + TangentVector v = makeTangentVector(v1, v2, d1, d2); + + if (Hp) { + *Hp = zeroJacobian(d); + // Top-left: ∂logG(g)/∂g — analytic. + Hp->topLeftCorner(d1, d1) = D_G; + // Top-right: 0 — logG doesn't depend on h (already zeroed). + // Bottom-right: ∂v₂/∂(δh) = φ₁(A)⁻¹ · φ₀(A). + // Perturbing h by δh in the semidirect frame moves h by φ₀(A)·δh + // (because Expmap(0, δh) = (I, δh) and (g,h)*(I,δh) → h + φ₀(A)·δh), + // so ∂v₂/∂(δh) = φ₁⁻¹ · φ₀. + Hp->bottomRightCorner(d2, d2) = phi1.lu().solve(phi0); + // Bottom-left: ∂v₂/∂(δg) — perturbing g via right multiplication + // changes u=logG(g) non-linearly; computed by central differences. + // Values are exact (matrix exp), so error is O(ε²). + const double eps = 1e-5; + for (Eigen::Index j = 0; j < static_cast(d1); ++j) { + typename traits::TangentVector ej; + if constexpr (firstDynamic) ej.resize(static_cast(d1)); + ej.setZero(); + ej(j) = eps; + const G gp = traits::Compose(p.first, traits::Expmap(ej)); + ej(j) = -eps; + const G gm = traits::Compose(p.first, traits::Expmap(ej)); + const Jacobian2 phi1p = phi01Kernel(Action::generator(traits::Logmap(gp))).second; + const Jacobian2 phi1m = phi01Kernel(Action::generator(traits::Logmap(gm))).second; + Hp->col(j).tail(d2) = + (phi1p.lu().solve(p.second) - phi1m.lu().solve(p.second)) / (2.0 * eps); + } + } + return v; } else { - // Semidirect product: the inverse of the coupled Expmap is equally coupled - // and cannot be inferred from the action alone. + // Semidirect product with no generator: action must supply the formula. return Action::template Logmap(p, Hp); } } diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 58ad751352..68b4532be8 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -33,6 +33,21 @@ namespace gtsam { +/// Detects whether Action has a static generator() method. +/// Used to enable the generic semidirect Expmap/Logmap via the φ₁ kernel. +template +struct HasGenerator : std::false_type {}; +template +struct HasGenerator()))>> + : std::true_type {}; + +/// Detects vector-space Lie groups (Eigen column vectors, group law = addition). +template +struct IsVectorGroup : std::false_type {}; +template +struct IsVectorGroup> : std::true_type {}; + /** * @brief Product Lie group of G and H, optionally with a semidirect structure. * @@ -49,8 +64,15 @@ namespace gtsam { * - operator()(g, h, Hg={}, Hh={}): the group action with optional * Jacobians w.r.t. g (DimH×DimG) and h (DimH×DimH). These Jacobians are * used to automatically derive the AdjointMap. - * - static Expmap(v1, v2, H1={}, H2={}): product exponential - * map (cannot be derived generically from the action alone). + * + * When H is a vector-space Lie group (Eigen column vector), the action may + * also supply the infinitesimal generator: + * - static Jacobian_H generator(const TangentVector_G& u): returns the + * DimH×DimH matrix Aφ(u) = d/dt φ(expG(t·u), ·)|_{t=0} (linear in u). + * If provided, Expmap and Logmap are derived automatically via φ₁(Aφ(u)). + * + * Otherwise (non-vector H or no generator), the action must supply: + * - static Expmap(v1, v2, H1={}, H2={}): product exponential. * - static Logmap(p, H={}): product logarithm. * * Example: SE(3) as a semidirect product: @@ -75,6 +97,11 @@ class ProductLieGroup : public std::pair { inline constexpr static bool firstDynamic = dimension1 == Eigen::Dynamic; inline constexpr static bool secondDynamic = dimension2 == Eigen::Dynamic; inline constexpr static bool isDirectProduct = std::is_void_v; + /// True when H is a vector-space group AND Action provides generator(). + /// Enables generic semidirect Expmap/Logmap via the φ₁ kernel formula. + inline constexpr static bool hasGenerator = + !isDirectProduct && IsVectorGroup::value && !secondDynamic && + HasGenerator::TangentVector>::value; public: /// Manifold dimension @@ -242,6 +269,11 @@ class ProductLieGroup : public std::pair { void checkMatchingDimensions(const ProductLieGroup& other, const char* operation) const; + /// Compute φ₀(A) = exp(A) and φ₁(A) = Σ_{k≥0} Aᵏ/(k+1)! together. + /// Uses the identity: exp([[A, I], [0, 0]]) = [[φ₀(A), φ₁(A)], [0, I]]. + /// Only valid (and only called) when hasGenerator is true. + static std::pair phi01Kernel(const Jacobian2& A); + public: /// @name Testable interface /// @{ diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index 297b9b1e13..f095a2326c 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -29,6 +29,10 @@ using namespace gtsam; constexpr double kTol = 1e-9; +// Rot3 acting on Vector3 by rotation: φ(R, t) = R·t. +// The infinitesimal generator Aφ(u)·t = d/dt(exp(tu∧)·t)|₀ = u∧·t, +// so generator(u) = u∧ (skew-symmetric matrix). ProductLieGroup derives +// Expmap and Logmap automatically via φ₁(u∧) = SO(3) left Jacobian. struct Rot3VectorAction : public GroupAction { static constexpr ActionType type = ActionType::Left; @@ -38,28 +42,7 @@ struct Rot3VectorAction : public GroupAction { return R.rotate(t, HR, Ht); } - template - static ProductType Expmap( - const Vector3& w, const Vector3& rho, - OptionalJacobian H1 = {}, - OptionalJacobian H2 = {}) { - Matrix6 Hpose; - Vector6 xi; - xi << w, rho; - const Pose3 pose = Pose3::Expmap(xi, H1 || H2 ? &Hpose : nullptr); - if (H1) *H1 = Hpose.leftCols<3>(); - if (H2) *H2 = Hpose.rightCols<3>(); - return ProductType(pose.rotation(), pose.translation()); - } - - template - static Vector6 Logmap(const ProductType& p, OptionalJacobian<6, 6> H = {}) { - Matrix6 Hpose; - const Vector6 xi = - Pose3::Logmap(Pose3(p.first, p.second), H ? &Hpose : nullptr); - if (H) *H = Hpose; - return xi; - } + static Matrix3 generator(const Vector3& u) { return skewSymmetric(u); } }; using Semidirect = ProductLieGroup; From 97f09bdb829deb4e9a4d536179d5dbfcc873629c Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Mon, 20 Apr 2026 01:49:38 -0400 Subject: [PATCH 15/21] remove no-infinitessimal path [skip ci] --- gtsam/base/GroupAction.h | 5 ++--- gtsam/base/ProductLieGroup-inl.h | 10 ++++++---- gtsam/base/ProductLieGroup.h | 16 +++++++--------- 3 files changed, 15 insertions(+), 16 deletions(-) diff --git a/gtsam/base/GroupAction.h b/gtsam/base/GroupAction.h index 7b5fb47ed3..cd0fe4750e 100644 --- a/gtsam/base/GroupAction.h +++ b/gtsam/base/GroupAction.h @@ -213,15 +213,14 @@ struct InducedVectorField : public Action { * OptionalJacobian Hg = {}) const; * (for Right action) * - * For semidirect products where M is a vector space (Eigen column vector), - * providing the optional static method below enables ProductLieGroup to + * For ProductLieGroup semidirect products where M is a vector space (Eigen + * column vector), provide the static method below so ProductLieGroup can * derive Expmap and Logmap automatically via the φ₁ kernel: * * static Eigen::Matrix generator(const TangentVector_G& u); * * where generator(u)·h = d/dt φ(expG(t·u), h)|_{t=0} is the infinitesimal * generator of the representation (a DimM×DimM matrix, linear in u). - * Without this method the action must supply Expmap and Logmap explicitly. * * @tparam Derived The user's action functor. * @tparam G The group type. diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index be24d38013..e868430019 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -303,8 +303,9 @@ ProductLieGroup ProductLieGroup::Expmap( } return ProductLieGroup(g, h); } else { - // Semidirect product with no generator: action must supply the formula. - return Action::template Expmap(v1, v2, H1, H2); + static_assert(hasGenerator, + "ProductLieGroup semidirect Expmap requires H to be a " + "fixed-size Eigen column vector and Action::generator(u)."); } } @@ -379,8 +380,9 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, } return v; } else { - // Semidirect product with no generator: action must supply the formula. - return Action::template Logmap(p, Hp); + static_assert(hasGenerator, + "ProductLieGroup semidirect Logmap requires H to be a " + "fixed-size Eigen column vector and Action::generator(u)."); } } diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 68b4532be8..91e5b481e8 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -65,15 +65,11 @@ struct IsVectorGroup> : std::true_type {}; * Jacobians w.r.t. g (DimH×DimG) and h (DimH×DimH). These Jacobians are * used to automatically derive the AdjointMap. * - * When H is a vector-space Lie group (Eigen column vector), the action may - * also supply the infinitesimal generator: + * Semidirect products require H to be a fixed-size vector-space Lie group + * (Eigen column vector) and the action to supply the infinitesimal generator: * - static Jacobian_H generator(const TangentVector_G& u): returns the * DimH×DimH matrix Aφ(u) = d/dt φ(expG(t·u), ·)|_{t=0} (linear in u). - * If provided, Expmap and Logmap are derived automatically via φ₁(Aφ(u)). - * - * Otherwise (non-vector H or no generator), the action must supply: - * - static Expmap(v1, v2, H1={}, H2={}): product exponential. - * - static Logmap(p, H={}): product logarithm. + * Expmap and Logmap are derived automatically via φ₁(Aφ(u)). * * Example: SE(3) as a semidirect product: * using SE3 = ProductLieGroup; @@ -97,11 +93,13 @@ class ProductLieGroup : public std::pair { inline constexpr static bool firstDynamic = dimension1 == Eigen::Dynamic; inline constexpr static bool secondDynamic = dimension2 == Eigen::Dynamic; inline constexpr static bool isDirectProduct = std::is_void_v; - /// True when H is a vector-space group AND Action provides generator(). - /// Enables generic semidirect Expmap/Logmap via the φ₁ kernel formula. + /// True when the semidirect product has the required generator support. inline constexpr static bool hasGenerator = !isDirectProduct && IsVectorGroup::value && !secondDynamic && HasGenerator::TangentVector>::value; + static_assert(isDirectProduct || hasGenerator, + "ProductLieGroup semidirect products require H to be a " + "fixed-size Eigen column vector and Action::generator(u)."); public: /// Manifold dimension From 6d3d0369f15f07f32e0237235c3ef976441db76d Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Tue, 21 Apr 2026 15:36:32 -0400 Subject: [PATCH 16/21] rename --- gtsam/base/ProductLieGroup-inl.h | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index e868430019..8d17d4c10e 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -237,22 +237,20 @@ ProductLieGroup ProductLieGroup::Expmap( // Direct product: the two Lie algebras are independent, so the exponential // is componentwise: exp(v₁,v₂) = (exp_G(v₁), exp_H(v₂)). // The Jacobian columns split cleanly between the G and H blocks. - const size_t firstDimension = static_cast(v1.size()); - const size_t secondDimension = static_cast(v2.size()); - const size_t productDimension = - combinedDimension(firstDimension, secondDimension); + const size_t d1 = static_cast(v1.size()); + const size_t d2 = static_cast(v2.size()); + const size_t d = combinedDimension(d1, d2); Jacobian1 D_g_first; Jacobian2 D_h_second; G g = traits::Expmap(v1, H1 ? &D_g_first : nullptr); H h = traits::Expmap(v2, H2 ? &D_h_second : nullptr); if (H1) { - *H1 = Matrix::Zero(productDimension, firstDimension); - H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + *H1 = Matrix::Zero(d, d1); + H1->block(0, 0, d1, d1) = D_g_first; } if (H2) { - *H2 = Matrix::Zero(productDimension, secondDimension); - H2->block(firstDimension, 0, secondDimension, secondDimension) = - D_h_second; + *H2 = Matrix::Zero(d, d2); + H2->block(d1, 0, d2, d2) = D_h_second; } return ProductLieGroup(g, h); } else if constexpr (hasGenerator) { From 6857a379a051cc3a0de9f2883d3eb4ce8fa8e534 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Thu, 23 Apr 2026 11:54:21 -0400 Subject: [PATCH 17/21] fixes --- gtsam/base/ProductLieGroup-inl.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 8d17d4c10e..8eb2ece2ea 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -23,13 +23,15 @@ namespace gtsam { // --------------------------------------------------------------------------- -// phi01Kernel: compute φ₀(A)=exp(A) and φ₁(A)=Σ Aᵏ/(k+1)! in one shot. +// phi01Kernel: compute φ₀(A)=exp(A) and φ₁(A)=Σ Aᵏ/(k+1)! from one block +// matrix exponential. // // Identity: exp([[A, I], [0, 0]]) = [[exp(A), φ₁(A)], [0, I]] // Proof: M = [[A,I],[0,0]]; M^k = [[A^k, A^{k-1}],[0,0]] for k≥1. // exp(M) = I + Σ M^k/k! = [[exp(A), Σ A^{k-1}/k!],[0,I]] = [[φ₀,φ₁],[0,I]]. // -// A single (2r)×(2r) matrix exponential yields both kernels. +// This is the generic path for arbitrary generators, not a faster replacement +// for closed-form kernels available for specific groups such as Rot3. // Only called when ProductLieGroup::hasGenerator is true. // --------------------------------------------------------------------------- template From ec5797b1aa7fd2f5c7210d5d5e4304c9e6fd137b Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Thu, 23 Apr 2026 22:29:58 -0400 Subject: [PATCH 18/21] test another case --- tests/testActionProductLieGroup.cpp | 105 ++++++++++++++++++++++++++++ 1 file changed, 105 insertions(+) diff --git a/tests/testActionProductLieGroup.cpp b/tests/testActionProductLieGroup.cpp index f095a2326c..fdcefb292d 100644 --- a/tests/testActionProductLieGroup.cpp +++ b/tests/testActionProductLieGroup.cpp @@ -22,13 +22,29 @@ #include #include #include +#include #include +#include #include using namespace gtsam; constexpr double kTol = 1e-9; +// Rot2 acting on Point2 by rotation: φ(R, t) = R·t. +// The infinitesimal generator is Aφ(u) = Hat(u), a 2x2 skew matrix. +struct Rot2PointAction : public GroupAction { + static constexpr ActionType type = ActionType::Left; + + Point2 operator()(const Rot2& R, const Point2& t, + OptionalJacobian<2, 1> HR = {}, + OptionalJacobian<2, 2> Ht = {}) const { + return R.rotate(t, HR, Ht); + } + + static Matrix2 generator(const Vector1& u) { return Rot2::Hat(u); } +}; + // Rot3 acting on Vector3 by rotation: φ(R, t) = R·t. // The infinitesimal generator Aφ(u)·t = d/dt(exp(tu∧)·t)|₀ = u∧·t, // so generator(u) = u∧ (skew-symmetric matrix). ProductLieGroup derives @@ -45,10 +61,45 @@ struct Rot3VectorAction : public GroupAction { static Matrix3 generator(const Vector3& u) { return skewSymmetric(u); } }; +using Semidirect2 = ProductLieGroup; using Semidirect = ProductLieGroup; namespace { +Semidirect2 semidirect2State() { + return Semidirect2(Rot2::fromAngle(0.35), Point2(0.4, -0.6)); +} + +Vector3 semidirect2Xi() { + Vector3 xi; + xi << 0.25, 0.3, -0.2; + return xi; +} + +Vector3 pose2XiFromSemidirect2(const Vector3& xi) { + Vector3 pose2Xi; + pose2Xi << xi(1), xi(2), xi(0); + return pose2Xi; +} + +Vector3 semidirect2XiFromPose2(const Vector3& xi) { + Vector3 semidirectXi; + semidirectXi << xi(2), xi(0), xi(1); + return semidirectXi; +} + +Semidirect2 expmapSemidirect2Proxy(const Vector3& vec) { + return Semidirect2::Expmap(vec); +} + +Vector3 logmapSemidirect2Proxy(const Semidirect2& p) { + return Semidirect2::Logmap(p); +} + +Pose2 asPose2(const Semidirect2& state) { + return Pose2(state.first, state.second); +} + Semidirect semidirectState1() { return Semidirect(Rot3::RzRyRx(0.1, 0.2, -0.3), Vector3(1.0, -0.5, 0.25)); } @@ -110,6 +161,60 @@ Pose3 asPose3(const Semidirect& state) { } // namespace +/* ************************************************************************* */ +// A second semidirect instance, Rot2 ⋉ R², checks the generic kernel path in +// a different dimension with Pose2 as the oracle. +TEST(Lie, ProductLieGroupSemidirectAction2D) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + const Rot2PointAction action; + const Rot2 R1 = Rot2::fromAngle(0.3); + const Rot2 R2 = Rot2::fromAngle(-0.2); + const Point2 t(0.4, -0.5); + EXPECT_LEFT_ACTION(action, R1, R2, t); + + const Vector1 u = (Vector1() << 0.35).finished(); + const Point2 p(0.2, -0.7); + const double eps = 1e-7; + const Point2 generatorAction = Rot2PointAction::generator(u) * p; + const Point2 generatorFiniteDifference = + (Rot2::Expmap((Vector1() << eps * u(0)).finished()).rotate(p) - p) / eps; + EXPECT(assert_equal(generatorAction, generatorFiniteDifference, 1e-6)); + + const Vector3 xi = semidirect2Xi(); + const Semidirect2 actual = Semidirect2::Expmap(xi); + EXPECT(assert_equal(Pose2::Expmap(pose2XiFromSemidirect2(xi)), asPose2(actual), + kTol)); + EXPECT(assert_equal( + xi, semidirect2XiFromPose2(Pose2::Logmap(asPose2(actual))), kTol)); +} + +/* ************************************************************************* */ +// Check Expmap/Logmap values and Jacobians for a second semidirect product, +// independent of the Pose3-specific ordering conventions. +TEST(testActionProduct, ExpmapLogmap2D) { + const Vector3 xi = semidirect2Xi(); + + Matrix expH; + const Semidirect2 actual = Semidirect2::Expmap(xi, expH); + const Matrix numericExpH = numericalDerivative11(expmapSemidirect2Proxy, xi); + + EXPECT(assert_equal(Pose2::Expmap(pose2XiFromSemidirect2(xi)), asPose2(actual), + kTol)); + EXPECT(assert_equal(numericExpH, expH, 1e-6)); + + const Semidirect2 state = semidirect2State(); + Matrix logH; + const Vector3 actualLog = Semidirect2::Logmap(state, logH); + const Matrix numericLogH = numericalDerivative11(logmapSemidirect2Proxy, state); + + EXPECT(assert_equal( + semidirect2XiFromPose2(Pose2::Logmap(asPose2(state))), actualLog, kTol)); + EXPECT(assert_equal(numericLogH, logH, 1e-6)); +} + /* ************************************************************************* */ // Verify the semidirect product obeys the left action law and matches Pose3 // behavior. From 0c285dc6bae8df6afdf9af91a4ec6d9baf533745 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Fri, 24 Apr 2026 00:23:33 -0400 Subject: [PATCH 19/21] split kernels --- gtsam/base/ProductLieGroup-inl.h | 108 +++++++++++++++++-------------- gtsam/base/ProductLieGroup.h | 10 ++- 2 files changed, 68 insertions(+), 50 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 8eb2ece2ea..3978442b3f 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -23,8 +23,19 @@ namespace gtsam { // --------------------------------------------------------------------------- -// phi01Kernel: compute φ₀(A)=exp(A) and φ₁(A)=Σ Aᵏ/(k+1)! from one block -// matrix exponential. +// phi0Kernel: compute φ₀(A)=exp(A) directly. +// --------------------------------------------------------------------------- +template +typename ProductLieGroup::Jacobian2 +ProductLieGroup::phi0Kernel(const Jacobian2& A) { + Jacobian2 phi0; + if constexpr (secondDynamic) phi0.resize(A.rows(), A.cols()); + phi0 = A.exp(); + return phi0; +} + +// --------------------------------------------------------------------------- +// phi1Kernel: compute φ₁(A)=Σ Aᵏ/(k+1)! from one block matrix exponential. // // Identity: exp([[A, I], [0, 0]]) = [[exp(A), φ₁(A)], [0, I]] // Proof: M = [[A,I],[0,0]]; M^k = [[A^k, A^{k-1}],[0,0]] for k≥1. @@ -35,22 +46,17 @@ namespace gtsam { // Only called when ProductLieGroup::hasGenerator is true. // --------------------------------------------------------------------------- template -std::pair::Jacobian2, - typename ProductLieGroup::Jacobian2> -ProductLieGroup::phi01Kernel(const Jacobian2& A) { +typename ProductLieGroup::Jacobian2 +ProductLieGroup::phi1Kernel(const Jacobian2& A) { const int r = static_cast(A.rows()); Eigen::MatrixXd M = Eigen::MatrixXd::Zero(2 * r, 2 * r); M.topLeftCorner(r, r) = A; M.topRightCorner(r, r) = Eigen::MatrixXd::Identity(r, r); const Eigen::MatrixXd expM = M.exp(); - Jacobian2 phi0, phi1; - if constexpr (secondDynamic) { - phi0.resize(r, r); - phi1.resize(r, r); - } - phi0 = expM.topLeftCorner(r, r); + Jacobian2 phi1; + if constexpr (secondDynamic) phi1.resize(r, r); phi1 = expM.topRightCorner(r, r); - return {phi0, phi1}; + return phi1; } @@ -259,47 +265,51 @@ ProductLieGroup ProductLieGroup::Expmap( // Generic semidirect Expmap for vector-space H via the φ₁ kernel: // Expmap(u, v) = (expG(u), φ₁(Aφ(u)) · v) // where Aφ(u) = Action::generator(u) is the infinitesimal generator and - // φ₁(A) = Σ Aᵏ/(k+1)! is computed from exp([[A,I],[0,0]]). + // φ₁(A) = Σ Aᵏ/(k+1)! is computed by phi1Kernel. const size_t d1 = static_cast(v1.size()); const size_t d2 = static_cast(v2.size()); const size_t d = combinedDimension(d1, d2); const Jacobian2 A = Action::generator(v1); - auto [phi0, phi1] = phi01Kernel(A); - const auto phi0Solver = phi0.lu(); + const Jacobian2 phi1 = phi1Kernel(A); Jacobian1 D_G; const G g = traits::Expmap(v1, H1 ? &D_G : nullptr); const H h = phi1 * v2; - if (H1) { - // Top rows: D Exp_G(u) in the output chart (from traits::Expmap). - // Bottom rows: D(φ₁(Aφ(u))·v) pulled back by φ₀(A)⁻¹, because - // Expmap Jacobians are expressed in local coordinates at Expmap(u,v). - // Linearity of generator: generator(u ± ε·eⱼ) = A ± ε·generator(eⱼ), - // so function values are exact and the only error is O(ε²) truncation. - *H1 = Matrix::Zero(d, d1); - H1->topRows(d1) = D_G; - const double eps = 1e-5; - typename traits::TangentVector ej; - if constexpr (firstDynamic) ej.resize(static_cast(d1)); - ej.setZero(); - for (Eigen::Index j = 0; j < static_cast(d1); ++j) { - ej(j) = 1.0; - const Jacobian2 Bj = Action::generator(ej); - const Jacobian2 phi1p = phi01Kernel(A + eps * Bj).second; - const Jacobian2 phi1m = phi01Kernel(A - eps * Bj).second; - const typename traits::TangentVector dh = - (phi1p - phi1m) * v2 / (2.0 * eps); - H1->col(j).tail(d2) = phi0Solver.solve(dh); - ej(j) = 0.0; + if (H1 || H2) { + const Jacobian2 phi0 = phi0Kernel(A); + const auto phi0Solver = phi0.lu(); + + if (H1) { + // Top rows: D Exp_G(u) in the output chart (from traits::Expmap). + // Bottom rows: D(φ₁(Aφ(u))·v) pulled back by φ₀(A)⁻¹, because + // Expmap Jacobians are expressed in local coordinates at Expmap(u,v). + // Linearity of generator: generator(u ± ε·eⱼ) = A ± ε·generator(eⱼ), + // so function values are exact and the only error is O(ε²) truncation. + *H1 = Matrix::Zero(d, d1); + H1->topRows(d1) = D_G; + const double eps = 1e-5; + typename traits::TangentVector ej; + if constexpr (firstDynamic) ej.resize(static_cast(d1)); + ej.setZero(); + for (Eigen::Index j = 0; j < static_cast(d1); ++j) { + ej(j) = 1.0; + const Jacobian2 Bj = Action::generator(ej); + const Jacobian2 phi1p = phi1Kernel(A + eps * Bj); + const Jacobian2 phi1m = phi1Kernel(A - eps * Bj); + const typename traits::TangentVector dh = + (phi1p - phi1m) * v2 / (2.0 * eps); + H1->col(j).tail(d2) = phi0Solver.solve(dh); + ej(j) = 0.0; + } + } + if (H2) { + // ∂(φ₁(A)·v)/∂v = φ₁(A) in coordinates, then pulled back by φ₀(A)⁻¹ + // to match the output chart. For SO(3) this is Rᵀ J_l = J_r. + *H2 = Matrix::Zero(d, d2); + H2->bottomRows(d2) = phi0Solver.solve(phi1); } - } - if (H2) { - // ∂(φ₁(A)·v)/∂v = φ₁(A) in coordinates, then pulled back by φ₀(A)⁻¹ - // to match the output chart. For SO(3) this is Rᵀ J_l = J_r. - *H2 = Matrix::Zero(d, d2); - H2->bottomRows(d2) = phi0Solver.solve(phi1); } return ProductLieGroup(g, h); } else { @@ -346,11 +356,13 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, Jacobian1 D_G; const auto v1 = traits::Logmap(p.first, Hp ? &D_G : nullptr); const Jacobian2 A = Action::generator(v1); - auto [phi0, phi1] = phi01Kernel(A); - const typename traits::TangentVector v2 = phi1.lu().solve(p.second); + const Jacobian2 phi1 = phi1Kernel(A); + const auto phi1Solver = phi1.lu(); + const typename traits::TangentVector v2 = phi1Solver.solve(p.second); TangentVector v = makeTangentVector(v1, v2, d1, d2); if (Hp) { + const Jacobian2 phi0 = phi0Kernel(A); *Hp = zeroJacobian(d); // Top-left: ∂logG(g)/∂g — analytic. Hp->topLeftCorner(d1, d1) = D_G; @@ -359,7 +371,7 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, // Perturbing h by δh in the semidirect frame moves h by φ₀(A)·δh // (because Expmap(0, δh) = (I, δh) and (g,h)*(I,δh) → h + φ₀(A)·δh), // so ∂v₂/∂(δh) = φ₁⁻¹ · φ₀. - Hp->bottomRightCorner(d2, d2) = phi1.lu().solve(phi0); + Hp->bottomRightCorner(d2, d2) = phi1Solver.solve(phi0); // Bottom-left: ∂v₂/∂(δg) — perturbing g via right multiplication // changes u=logG(g) non-linearly; computed by central differences. // Values are exact (matrix exp), so error is O(ε²). @@ -372,8 +384,10 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, const G gp = traits::Compose(p.first, traits::Expmap(ej)); ej(j) = -eps; const G gm = traits::Compose(p.first, traits::Expmap(ej)); - const Jacobian2 phi1p = phi01Kernel(Action::generator(traits::Logmap(gp))).second; - const Jacobian2 phi1m = phi01Kernel(Action::generator(traits::Logmap(gm))).second; + const Jacobian2 phi1p = + phi1Kernel(Action::generator(traits::Logmap(gp))); + const Jacobian2 phi1m = + phi1Kernel(Action::generator(traits::Logmap(gm))); Hp->col(j).tail(d2) = (phi1p.lu().solve(p.second) - phi1m.lu().solve(p.second)) / (2.0 * eps); } diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 91e5b481e8..8a0d43510a 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -267,10 +267,14 @@ class ProductLieGroup : public std::pair { void checkMatchingDimensions(const ProductLieGroup& other, const char* operation) const; - /// Compute φ₀(A) = exp(A) and φ₁(A) = Σ_{k≥0} Aᵏ/(k+1)! together. - /// Uses the identity: exp([[A, I], [0, 0]]) = [[φ₀(A), φ₁(A)], [0, I]]. + /// Compute φ₀(A) = exp(A). + /// Only valid/called when hasGenerator is true. + static Jacobian2 phi0Kernel(const Jacobian2& A); + + /// Compute φ₁(A) = Σ_{k≥0} Aᵏ/(k+1)! via the block identity + /// exp([[A, I], [0, 0]]) = [[φ₀(A), φ₁(A)], [0, I]]. /// Only valid (and only called) when hasGenerator is true. - static std::pair phi01Kernel(const Jacobian2& A); + static Jacobian2 phi1Kernel(const Jacobian2& A); public: /// @name Testable interface From 6a0be6bdce270171e9c12ef33c00e8952f75b087 Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Fri, 24 Apr 2026 11:28:51 -0400 Subject: [PATCH 20/21] implement frechet derivative --- gtsam/base/ProductLieGroup-inl.h | 146 +++++++++++++++++++------------ gtsam/base/ProductLieGroup.h | 12 +++ 2 files changed, 104 insertions(+), 54 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 3978442b3f..4de6799527 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -59,6 +59,42 @@ ProductLieGroup::phi1Kernel(const Jacobian2& A) { return phi1; } +// --------------------------------------------------------------------------- +// phi1FrechetBlock: compute φ₀(A), φ₁(A), and L_{φ₁}(A, B) from one block +// matrix exponential. +// +// Identity: +// exp([[0, I, 0], +// [0, A, B], +// [0, 0, A]]) +// = [[I, φ₁(A), L_{φ₁}(A, B)], +// [0, exp(A), L_exp(A, B) ], +// [0, 0, exp(A) ]] +// --------------------------------------------------------------------------- +template +typename ProductLieGroup::Phi1FrechetResult +ProductLieGroup::phi1FrechetBlock(const Jacobian2& A, + const Jacobian2& B) { + const int r = static_cast(A.rows()); + Eigen::MatrixXd M = Eigen::MatrixXd::Zero(3 * r, 3 * r); + M.block(0, r, r, r) = Eigen::MatrixXd::Identity(r, r); + M.block(r, r, r, r) = A; + M.block(r, 2 * r, r, r) = B; + M.block(2 * r, 2 * r, r, r) = A; + const Eigen::MatrixXd expM = M.exp(); + + Phi1FrechetResult result; + if constexpr (secondDynamic) { + result.phi0.resize(r, r); + result.phi1.resize(r, r); + result.Lphi1.resize(r, r); + } + result.phi1 = expM.block(0, r, r, r); + result.Lphi1 = expM.block(0, 2 * r, r, r); + result.phi0 = expM.block(r, r, r, r); + return result; +} + template @@ -271,47 +307,54 @@ ProductLieGroup ProductLieGroup::Expmap( const size_t d = combinedDimension(d1, d2); const Jacobian2 A = Action::generator(v1); - const Jacobian2 phi1 = phi1Kernel(A); Jacobian1 D_G; const G g = traits::Expmap(v1, H1 ? &D_G : nullptr); - const H h = phi1 * v2; - - if (H1 || H2) { - const Jacobian2 phi0 = phi0Kernel(A); - const auto phi0Solver = phi0.lu(); - - if (H1) { - // Top rows: D Exp_G(u) in the output chart (from traits::Expmap). - // Bottom rows: D(φ₁(Aφ(u))·v) pulled back by φ₀(A)⁻¹, because - // Expmap Jacobians are expressed in local coordinates at Expmap(u,v). - // Linearity of generator: generator(u ± ε·eⱼ) = A ± ε·generator(eⱼ), - // so function values are exact and the only error is O(ε²) truncation. - *H1 = Matrix::Zero(d, d1); - H1->topRows(d1) = D_G; - const double eps = 1e-5; - typename traits::TangentVector ej; - if constexpr (firstDynamic) ej.resize(static_cast(d1)); - ej.setZero(); - for (Eigen::Index j = 0; j < static_cast(d1); ++j) { - ej(j) = 1.0; - const Jacobian2 Bj = Action::generator(ej); - const Jacobian2 phi1p = phi1Kernel(A + eps * Bj); - const Jacobian2 phi1m = phi1Kernel(A - eps * Bj); - const typename traits::TangentVector dh = - (phi1p - phi1m) * v2 / (2.0 * eps); - H1->col(j).tail(d2) = phi0Solver.solve(dh); - ej(j) = 0.0; - } + + if (H1) { + Jacobian2 zeroB = A; + zeroB.setZero(); + const auto kernels = phi1FrechetBlock(A, zeroB); + const auto phi0Solver = kernels.phi0.lu(); + const H h = kernels.phi1 * v2; + + // Top rows: D Exp_G(u) in the output chart (from traits::Expmap). + // Bottom rows: D(φ₁(Aφ(u))·v) pulled back by φ₀(A)⁻¹, because + // Expmap Jacobians are expressed in local coordinates at Expmap(u,v). + // Here dφ₁(A) is evaluated analytically via its Fréchet derivative. + *H1 = Matrix::Zero(d, d1); + H1->topRows(d1) = D_G; + typename traits::TangentVector ej; + if constexpr (firstDynamic) ej.resize(static_cast(d1)); + ej.setZero(); + for (Eigen::Index j = 0; j < static_cast(d1); ++j) { + ej(j) = 1.0; + const Jacobian2 Bj = Action::generator(ej); + const typename traits::TangentVector dh = + phi1FrechetBlock(A, Bj).Lphi1 * v2; + H1->col(j).tail(d2) = phi0Solver.solve(dh); + ej(j) = 0.0; + } + if (H2) { + // ∂(φ₁(A)·v)/∂v = φ₁(A) in coordinates, then pulled back by φ₀(A)⁻¹ + // to match the output chart. For SO(3) this is Rᵀ J_l = J_r. + *H2 = Matrix::Zero(d, d2); + H2->bottomRows(d2) = phi0Solver.solve(kernels.phi1); } + return ProductLieGroup(g, h); + } else { + const Jacobian2 phi1 = phi1Kernel(A); + const H h = phi1 * v2; if (H2) { + const Jacobian2 phi0 = phi0Kernel(A); + const auto phi0Solver = phi0.lu(); // ∂(φ₁(A)·v)/∂v = φ₁(A) in coordinates, then pulled back by φ₀(A)⁻¹ // to match the output chart. For SO(3) this is Rᵀ J_l = J_r. *H2 = Matrix::Zero(d, d2); H2->bottomRows(d2) = phi0Solver.solve(phi1); } + return ProductLieGroup(g, h); } - return ProductLieGroup(g, h); } else { static_assert(hasGenerator, "ProductLieGroup semidirect Expmap requires H to be a " @@ -356,13 +399,14 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, Jacobian1 D_G; const auto v1 = traits::Logmap(p.first, Hp ? &D_G : nullptr); const Jacobian2 A = Action::generator(v1); - const Jacobian2 phi1 = phi1Kernel(A); - const auto phi1Solver = phi1.lu(); - const typename traits::TangentVector v2 = phi1Solver.solve(p.second); - TangentVector v = makeTangentVector(v1, v2, d1, d2); - if (Hp) { - const Jacobian2 phi0 = phi0Kernel(A); + Jacobian2 zeroB = A; + zeroB.setZero(); + const auto kernels = phi1FrechetBlock(A, zeroB); + const auto phi1Solver = kernels.phi1.lu(); + const typename traits::TangentVector v2 = phi1Solver.solve(p.second); + TangentVector v = makeTangentVector(v1, v2, d1, d2); + *Hp = zeroJacobian(d); // Top-left: ∂logG(g)/∂g — analytic. Hp->topLeftCorner(d1, d1) = D_G; @@ -371,28 +415,22 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, // Perturbing h by δh in the semidirect frame moves h by φ₀(A)·δh // (because Expmap(0, δh) = (I, δh) and (g,h)*(I,δh) → h + φ₀(A)·δh), // so ∂v₂/∂(δh) = φ₁⁻¹ · φ₀. - Hp->bottomRightCorner(d2, d2) = phi1Solver.solve(phi0); - // Bottom-left: ∂v₂/∂(δg) — perturbing g via right multiplication - // changes u=logG(g) non-linearly; computed by central differences. - // Values are exact (matrix exp), so error is O(ε²). - const double eps = 1e-5; + Hp->bottomRightCorner(d2, d2) = phi1Solver.solve(kernels.phi0); + // Bottom-left: ∂v₂/∂(δg) follows from dφ₁(A) = L_{φ₁}(A, dA) + // with dA = generator(du) and du = D Log_G(g) · δg. for (Eigen::Index j = 0; j < static_cast(d1); ++j) { - typename traits::TangentVector ej; - if constexpr (firstDynamic) ej.resize(static_cast(d1)); - ej.setZero(); - ej(j) = eps; - const G gp = traits::Compose(p.first, traits::Expmap(ej)); - ej(j) = -eps; - const G gm = traits::Compose(p.first, traits::Expmap(ej)); - const Jacobian2 phi1p = - phi1Kernel(Action::generator(traits::Logmap(gp))); - const Jacobian2 phi1m = - phi1Kernel(Action::generator(traits::Logmap(gm))); + const typename traits::TangentVector du = D_G.col(j); + const Jacobian2 Bj = Action::generator(du); Hp->col(j).tail(d2) = - (phi1p.lu().solve(p.second) - phi1m.lu().solve(p.second)) / (2.0 * eps); + -phi1Solver.solve(phi1FrechetBlock(A, Bj).Lphi1 * v2); } + return v; + } else { + const Jacobian2 phi1 = phi1Kernel(A); + const auto phi1Solver = phi1.lu(); + const typename traits::TangentVector v2 = phi1Solver.solve(p.second); + return makeTangentVector(v1, v2, d1, d2); } - return v; } else { static_assert(hasGenerator, "ProductLieGroup semidirect Logmap requires H to be a " diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 8a0d43510a..24c46550d5 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -276,6 +276,18 @@ class ProductLieGroup : public std::pair { /// Only valid (and only called) when hasGenerator is true. static Jacobian2 phi1Kernel(const Jacobian2& A); + /// Result of the analytic Fréchet derivative helper for φ₁. + struct Phi1FrechetResult { + Jacobian2 phi0; ///< exp(A) + Jacobian2 phi1; ///< φ₁(A) + Jacobian2 Lphi1; ///< Fréchet derivative L_{φ₁}(A, B) + }; + + /// Compute exp(A), φ₁(A), and the Fréchet derivative L_{φ₁}(A, B) from a + /// single block exponential. Only valid/called when hasGenerator is true. + static Phi1FrechetResult phi1FrechetBlock(const Jacobian2& A, + const Jacobian2& B); + public: /// @name Testable interface /// @{ From 9b73fd24fcfb10f704087a44feed954f6157d59e Mon Sep 17 00:00:00 2001 From: Rohan Bansal Date: Fri, 24 Apr 2026 11:42:06 -0400 Subject: [PATCH 21/21] clean up old paths --- gtsam/base/ProductLieGroup-inl.h | 119 +++++++++---------------------- gtsam/base/ProductLieGroup.h | 9 --- 2 files changed, 33 insertions(+), 95 deletions(-) diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 4de6799527..822f6ee324 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -22,43 +22,6 @@ namespace gtsam { -// --------------------------------------------------------------------------- -// phi0Kernel: compute φ₀(A)=exp(A) directly. -// --------------------------------------------------------------------------- -template -typename ProductLieGroup::Jacobian2 -ProductLieGroup::phi0Kernel(const Jacobian2& A) { - Jacobian2 phi0; - if constexpr (secondDynamic) phi0.resize(A.rows(), A.cols()); - phi0 = A.exp(); - return phi0; -} - -// --------------------------------------------------------------------------- -// phi1Kernel: compute φ₁(A)=Σ Aᵏ/(k+1)! from one block matrix exponential. -// -// Identity: exp([[A, I], [0, 0]]) = [[exp(A), φ₁(A)], [0, I]] -// Proof: M = [[A,I],[0,0]]; M^k = [[A^k, A^{k-1}],[0,0]] for k≥1. -// exp(M) = I + Σ M^k/k! = [[exp(A), Σ A^{k-1}/k!],[0,I]] = [[φ₀,φ₁],[0,I]]. -// -// This is the generic path for arbitrary generators, not a faster replacement -// for closed-form kernels available for specific groups such as Rot3. -// Only called when ProductLieGroup::hasGenerator is true. -// --------------------------------------------------------------------------- -template -typename ProductLieGroup::Jacobian2 -ProductLieGroup::phi1Kernel(const Jacobian2& A) { - const int r = static_cast(A.rows()); - Eigen::MatrixXd M = Eigen::MatrixXd::Zero(2 * r, 2 * r); - M.topLeftCorner(r, r) = A; - M.topRightCorner(r, r) = Eigen::MatrixXd::Identity(r, r); - const Eigen::MatrixXd expM = M.exp(); - Jacobian2 phi1; - if constexpr (secondDynamic) phi1.resize(r, r); - phi1 = expM.topRightCorner(r, r); - return phi1; -} - // --------------------------------------------------------------------------- // phi1FrechetBlock: compute φ₀(A), φ₁(A), and L_{φ₁}(A, B) from one block // matrix exponential. @@ -300,40 +263,41 @@ ProductLieGroup ProductLieGroup::Expmap( } else if constexpr (hasGenerator) { // Generic semidirect Expmap for vector-space H via the φ₁ kernel: // Expmap(u, v) = (expG(u), φ₁(Aφ(u)) · v) - // where Aφ(u) = Action::generator(u) is the infinitesimal generator and - // φ₁(A) = Σ Aᵏ/(k+1)! is computed by phi1Kernel. + // where Aφ(u) = Action::generator(u) is the infinitesimal generator. const size_t d1 = static_cast(v1.size()); const size_t d2 = static_cast(v2.size()); const size_t d = combinedDimension(d1, d2); const Jacobian2 A = Action::generator(v1); + Jacobian2 zeroB = A; + zeroB.setZero(); + const auto kernels = phi1FrechetBlock(A, zeroB); Jacobian1 D_G; const G g = traits::Expmap(v1, H1 ? &D_G : nullptr); + const H h = kernels.phi1 * v2; - if (H1) { - Jacobian2 zeroB = A; - zeroB.setZero(); - const auto kernels = phi1FrechetBlock(A, zeroB); + if (H1 || H2) { const auto phi0Solver = kernels.phi0.lu(); - const H h = kernels.phi1 * v2; - // Top rows: D Exp_G(u) in the output chart (from traits::Expmap). - // Bottom rows: D(φ₁(Aφ(u))·v) pulled back by φ₀(A)⁻¹, because - // Expmap Jacobians are expressed in local coordinates at Expmap(u,v). - // Here dφ₁(A) is evaluated analytically via its Fréchet derivative. - *H1 = Matrix::Zero(d, d1); - H1->topRows(d1) = D_G; - typename traits::TangentVector ej; - if constexpr (firstDynamic) ej.resize(static_cast(d1)); - ej.setZero(); - for (Eigen::Index j = 0; j < static_cast(d1); ++j) { - ej(j) = 1.0; - const Jacobian2 Bj = Action::generator(ej); - const typename traits::TangentVector dh = - phi1FrechetBlock(A, Bj).Lphi1 * v2; - H1->col(j).tail(d2) = phi0Solver.solve(dh); - ej(j) = 0.0; + if (H1) { + // Top rows: D Exp_G(u) in the output chart (from traits::Expmap). + // Bottom rows: D(φ₁(Aφ(u))·v) pulled back by φ₀(A)⁻¹, because + // Expmap Jacobians are expressed in local coordinates at Expmap(u,v). + // Here dφ₁(A) is evaluated analytically via its Fréchet derivative. + *H1 = Matrix::Zero(d, d1); + H1->topRows(d1) = D_G; + typename traits::TangentVector ej; + if constexpr (firstDynamic) ej.resize(static_cast(d1)); + ej.setZero(); + for (Eigen::Index j = 0; j < static_cast(d1); ++j) { + ej(j) = 1.0; + const Jacobian2 Bj = Action::generator(ej); + const typename traits::TangentVector dh = + phi1FrechetBlock(A, Bj).Lphi1 * v2; + H1->col(j).tail(d2) = phi0Solver.solve(dh); + ej(j) = 0.0; + } } if (H2) { // ∂(φ₁(A)·v)/∂v = φ₁(A) in coordinates, then pulled back by φ₀(A)⁻¹ @@ -341,20 +305,8 @@ ProductLieGroup ProductLieGroup::Expmap( *H2 = Matrix::Zero(d, d2); H2->bottomRows(d2) = phi0Solver.solve(kernels.phi1); } - return ProductLieGroup(g, h); - } else { - const Jacobian2 phi1 = phi1Kernel(A); - const H h = phi1 * v2; - if (H2) { - const Jacobian2 phi0 = phi0Kernel(A); - const auto phi0Solver = phi0.lu(); - // ∂(φ₁(A)·v)/∂v = φ₁(A) in coordinates, then pulled back by φ₀(A)⁻¹ - // to match the output chart. For SO(3) this is Rᵀ J_l = J_r. - *H2 = Matrix::Zero(d, d2); - H2->bottomRows(d2) = phi0Solver.solve(phi1); - } - return ProductLieGroup(g, h); } + return ProductLieGroup(g, h); } else { static_assert(hasGenerator, "ProductLieGroup semidirect Expmap requires H to be a " @@ -399,14 +351,14 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, Jacobian1 D_G; const auto v1 = traits::Logmap(p.first, Hp ? &D_G : nullptr); const Jacobian2 A = Action::generator(v1); - if (Hp) { - Jacobian2 zeroB = A; - zeroB.setZero(); - const auto kernels = phi1FrechetBlock(A, zeroB); - const auto phi1Solver = kernels.phi1.lu(); - const typename traits::TangentVector v2 = phi1Solver.solve(p.second); - TangentVector v = makeTangentVector(v1, v2, d1, d2); + Jacobian2 zeroB = A; + zeroB.setZero(); + const auto kernels = phi1FrechetBlock(A, zeroB); + const auto phi1Solver = kernels.phi1.lu(); + const typename traits::TangentVector v2 = phi1Solver.solve(p.second); + TangentVector v = makeTangentVector(v1, v2, d1, d2); + if (Hp) { *Hp = zeroJacobian(d); // Top-left: ∂logG(g)/∂g — analytic. Hp->topLeftCorner(d1, d1) = D_G; @@ -424,13 +376,8 @@ ProductLieGroup::Logmap(const ProductLieGroup& p, Hp->col(j).tail(d2) = -phi1Solver.solve(phi1FrechetBlock(A, Bj).Lphi1 * v2); } - return v; - } else { - const Jacobian2 phi1 = phi1Kernel(A); - const auto phi1Solver = phi1.lu(); - const typename traits::TangentVector v2 = phi1Solver.solve(p.second); - return makeTangentVector(v1, v2, d1, d2); } + return v; } else { static_assert(hasGenerator, "ProductLieGroup semidirect Logmap requires H to be a " diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 24c46550d5..67b11e47bc 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -267,15 +267,6 @@ class ProductLieGroup : public std::pair { void checkMatchingDimensions(const ProductLieGroup& other, const char* operation) const; - /// Compute φ₀(A) = exp(A). - /// Only valid/called when hasGenerator is true. - static Jacobian2 phi0Kernel(const Jacobian2& A); - - /// Compute φ₁(A) = Σ_{k≥0} Aᵏ/(k+1)! via the block identity - /// exp([[A, I], [0, 0]]) = [[φ₀(A), φ₁(A)], [0, I]]. - /// Only valid (and only called) when hasGenerator is true. - static Jacobian2 phi1Kernel(const Jacobian2& A); - /// Result of the analytic Fréchet derivative helper for φ₁. struct Phi1FrechetResult { Jacobian2 phi0; ///< exp(A)