diff --git a/gtsam/base/GroupAction.h b/gtsam/base/GroupAction.h index 348fdf89ac..cd0fe4750e 100644 --- a/gtsam/base/GroupAction.h +++ b/gtsam/base/GroupAction.h @@ -213,6 +213,15 @@ struct InducedVectorField : public Action { * OptionalJacobian Hg = {}) const; * (for Right action) * + * 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). + * * @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 42a0681459..822f6ee324 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -18,164 +18,164 @@ #pragma once +#include + namespace gtsam { -template -ProductLieGroup ProductLieGroup::operator*( +// --------------------------------------------------------------------------- +// 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 +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) { + // 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 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)); + } } -template -ProductLieGroup ProductLieGroup::inverse() const { - return ProductLieGroup(traits::Inverse(this->first), - traits::Inverse(this->second)); +template +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)); + } } -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) { +template +ProductLieGroup ProductLieGroup::retract( + const TangentVector& v, ChartJacobian H1, ChartJacobian H2) const { + 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"); } - 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; - } + // 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(productDimension); - H2->block(0, 0, firstDimension, firstDimension) = D_g_second; - H2->block(firstDimension, firstDimension, secondDimension, - secondDimension) = D_h_second; + *H2 = zeroJacobian(d); + H2->leftCols(d1) = D_v1; + H2->rightCols(d2) = D_v2; } - return ProductLieGroup(g, h); + return result; } -template -typename ProductLieGroup::TangentVector -ProductLieGroup::localCoordinates(const ProductLieGroup& g, - ChartJacobian H1, - ChartJacobian H2) const { +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); + // 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 -ProductLieGroup ProductLieGroup::compose( +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 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); - 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); + 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 { + if (D) *D = -AdjointMap(); + return inverse(); } -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,6 +212,7 @@ 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); @@ -233,76 +234,215 @@ 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 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 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(d, d1); + H1->block(0, 0, d1, d1) = D_g_first; + } + if (H2) { + *H2 = Matrix::Zero(d, d2); + H2->block(d1, 0, d2, d2) = 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. + 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 || H2) { + const auto phi0Solver = kernels.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). + // 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 { + static_assert(hasGenerator, + "ProductLieGroup semidirect Expmap requires H to be a " + "fixed-size Eigen column vector and Action::generator(u)."); } - 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; +} + +template +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 = + 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 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); + 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; + // 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) = 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) { + const typename traits::TangentVector du = D_G.col(j); + const Jacobian2 Bj = Action::generator(du); + Hp->col(j).tail(d2) = + -phi1Solver.solve(phi1FrechetBlock(A, Bj).Lphi1 * v2); + } + } + return v; + } else { + static_assert(hasGenerator, + "ProductLieGroup semidirect Logmap requires H to be a " + "fixed-size Eigen column vector and Action::generator(u)."); } - 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) { + // 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()); + 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 { + // 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 ]] + // + // 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; + } } -template +template template -T ProductLieGroup::defaultIdentity() { +T ProductLieGroup::defaultIdentity() { if constexpr (traits::dimension == Eigen::Dynamic) { return T(); } else { @@ -310,9 +450,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 +464,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 +486,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 +497,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 +508,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 +517,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..67b11e47bc 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 { +/// 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 Template to construct the product Lie group of two other Lie groups - * Assumes Lie group structure for G and 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. + * + * 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). + * Expmap and Logmap are derived automatically via φ₁(Aφ(u)). + * + * Example: SE(3) as a semidirect product: + * using SE3 = ProductLieGroup; + * where Rot3VectorAction implements φ(R, t) = R·t. */ -template +template class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsLieGroup); GTSAM_CONCEPT_ASSERT(IsLieGroup); @@ -52,6 +92,14 @@ 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_void_v; + /// 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 @@ -74,6 +122,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>; public: /// @name Standard Constructors @@ -214,6 +267,18 @@ class ProductLieGroup : public std::pair { void checkMatchingDimensions(const ProductLieGroup& other, const char* operation) const; + /// 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 /// @{ @@ -523,9 +588,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..fdcefb292d --- /dev/null +++ b/tests/testActionProductLieGroup.cpp @@ -0,0 +1,374 @@ +/* ---------------------------------------------------------------------------- + + * 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 Rohan Bansal + * @author Jennifer Oum + * @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; + +// 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 +// Expmap and Logmap automatically via φ₁(u∧) = SO(3) left Jacobian. +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); + } + + 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)); +} + +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); +} + +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 + +/* ************************************************************************* */ +// 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. +TEST(Lie, ProductLieGroupSemidirectAction) { + 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; + 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 = + 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 = semidirectState1(); + const Semidirect state2 = semidirectState2(); + + 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)); +} + +/* ************************************************************************* */ +// Check semidirect between values and Jacobians against Pose3 and numerical +// derivatives. +TEST(testActionProduct, between) { + const Semidirect state1 = semidirectState1(); + const Semidirect state2 = semidirectState2(); + + 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)); +} + +/* ************************************************************************* */ +// Check the semidirect inverse and its Jacobian against Pose3 and numerical +// derivatives. +TEST(testActionProduct, inverse) { + 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); + const Matrix numericH = numericalDerivative11(inverseSemidirectProxy, state); + + EXPECT(assert_equal(asPose3(actual), asPose3(state).inverse(), kTol)); + EXPECT(assert_equal(numericH, actH, 1e-6)); +} + +/* ************************************************************************* */ +// Check the semidirect Expmap and its Jacobian against Pose3 and numerical +// derivatives. +TEST(testActionProduct, Expmap) { + const Vector6 xi = semidirectXi(); + + 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)); +} + +/* ************************************************************************* */ +// Check the semidirect Logmap and its Jacobian against Pose3 and numerical +// derivatives. +TEST(testActionProduct, Logmap) { + const Semidirect state = semidirectState4(); + + 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)); +} + +/* ************************************************************************* */ +// Check that the semidirect adjoint matches the Pose3 adjoint. +TEST(testActionProduct, AdjointMap) { + const Semidirect state = semidirectState4(); + + EXPECT(assert_equal(asPose3(state).AdjointMap(), state.AdjointMap(), kTol)); +} + +/* ************************************************************************* */ +// Check Expmap-based retract/localCoordinates consistency and both Jacobians +// against numerical derivatives. +TEST(testActionProduct, retractAndLocalCoordinates) { + 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); + + EXPECT(assert_equal(asPose3(updated), + asPose3(state).compose(Pose3::Expmap(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); +} +//******************************************************************************